kopia lustrzana https://github.com/projecthorus/chasemapper
Remove horuslib dependency. Initial commit of serial GPS class.
Mark Jessop
rodzic
934929f9d0
commit
af84d5442c
|
|
@ -0,0 +1,117 @@
|
|||
#!/usr/bin/env python
|
||||
#
|
||||
# Project Horus - Atmosphere / Descent Rate Modelling
|
||||
#
|
||||
# Copyright (C) 2018 Mark Jessop <vk5qi@rfhead.net>
|
||||
# Released under GNU GPL v3 or later
|
||||
#
|
||||
import math
|
||||
|
||||
def getDensity(altitude):
|
||||
'''
|
||||
Calculate the atmospheric density for a given altitude in metres.
|
||||
This is a direct port of the oziplotter Atmosphere class
|
||||
'''
|
||||
|
||||
#Constants
|
||||
airMolWeight = 28.9644 # Molecular weight of air
|
||||
densitySL = 1.225 # Density at sea level [kg/m3]
|
||||
pressureSL = 101325 # Pressure at sea level [Pa]
|
||||
temperatureSL = 288.15 # Temperature at sea level [deg K]
|
||||
gamma = 1.4
|
||||
gravity = 9.80665 # Acceleration of gravity [m/s2]
|
||||
tempGrad = -0.0065 # Temperature gradient [deg K/m]
|
||||
RGas = 8.31432 # Gas constant [kg/Mol/K]
|
||||
R = 287.053
|
||||
deltaTemperature = 0.0;
|
||||
|
||||
# Lookup Tables
|
||||
altitudes = [0, 11000, 20000, 32000, 47000, 51000, 71000, 84852]
|
||||
pressureRels = [1, 2.23361105092158e-1, 5.403295010784876e-2, 8.566678359291667e-3, 1.0945601337771144e-3, 6.606353132858367e-4, 3.904683373343926e-5, 3.6850095235747942e-6]
|
||||
temperatures = [288.15, 216.65, 216.65, 228.65, 270.65, 270.65, 214.65, 186.946]
|
||||
tempGrads = [-6.5, 0, 1, 2.8, 0, -2.8, -2, 0]
|
||||
gMR = gravity * airMolWeight / RGas;
|
||||
|
||||
# Pick a region to work in
|
||||
i = 0
|
||||
if(altitude > 0):
|
||||
while (altitude > altitudes[i+1]):
|
||||
i = i + 1
|
||||
|
||||
|
||||
# Lookup based on region
|
||||
baseTemp = temperatures[i]
|
||||
tempGrad = tempGrads[i] / 1000.0
|
||||
pressureRelBase = pressureRels[i]
|
||||
deltaAltitude = altitude - altitudes[i]
|
||||
temperature = baseTemp + tempGrad * deltaAltitude
|
||||
|
||||
# Calculate relative pressure
|
||||
if(math.fabs(tempGrad) < 1e-10):
|
||||
pressureRel = pressureRelBase * math.exp(-1 *gMR * deltaAltitude / 1000.0 / baseTemp)
|
||||
else:
|
||||
pressureRel = pressureRelBase * math.pow(baseTemp / temperature, gMR / tempGrad / 1000.0)
|
||||
|
||||
|
||||
# Add temperature offset
|
||||
temperature = temperature + deltaTemperature
|
||||
|
||||
# Finally, work out the density...
|
||||
speedOfSound = math.sqrt(gamma * R * temperature)
|
||||
pressure = pressureRel * pressureSL
|
||||
density = densitySL * pressureRel * temperatureSL / temperature
|
||||
|
||||
return density
|
||||
|
||||
|
||||
def seaLevelDescentRate(descent_rate, altitude):
|
||||
''' Calculate the descent rate at sea level, for a given descent rate at altitude '''
|
||||
|
||||
rho = getDensity(altitude)
|
||||
return math.sqrt((rho / 1.22) * math.pow(descent_rate, 2))
|
||||
|
||||
|
||||
|
||||
def time_to_landing(current_altitude, current_descent_rate=-5.0, ground_asl=0.0, step_size=1):
|
||||
''' Calculate an estimated time to landing (in seconds) of a payload, based on its current altitude and descent rate '''
|
||||
|
||||
# A few checks on the input data.
|
||||
if current_descent_rate > 0.0:
|
||||
# If we are still ascending, return none.
|
||||
return None
|
||||
|
||||
if current_altitude <= ground_asl:
|
||||
# If the current altitude is *below* ground level, we have landed.
|
||||
return 0
|
||||
|
||||
# Calculate the sea level descent rate.
|
||||
_desc_rate = math.fabs(seaLevelDescentRate(current_descent_rate, current_altitude))
|
||||
_drag_coeff = _desc_rate*1.1045 # Magic multiplier from predict.php
|
||||
|
||||
|
||||
_alt = current_altitude
|
||||
_start_time = 0
|
||||
# Now step through the flight in <step_size> second steps.
|
||||
# Once the altitude is below our ground level, stop, and return the elapsed time.
|
||||
while _alt >= ground_asl:
|
||||
_alt += step_size * -1*(_drag_coeff/math.sqrt(getDensity(_alt)))
|
||||
_start_time += step_size
|
||||
|
||||
|
||||
return _start_time
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
# Test Cases
|
||||
_altitudes = [1000, 10000, 30000, 1000, 10000, 30000]
|
||||
_rates = [-10.0, -10.0, -10.0, -30.0, -30.0, -30.0]
|
||||
|
||||
for i in range(len(_altitudes)):
|
||||
print("Altitude: %d m, Rate: %.2f m/s" % (_altitudes[i], _rates[i]))
|
||||
print("Density: %.5f" % getDensity(_altitudes[i]))
|
||||
print("Sea Level Descent Rate: %.2f m/s" % seaLevelDescentRate(_rates[i], _altitudes[i]))
|
||||
_landing = time_to_landing(_altitudes[i],_rates[i])
|
||||
_landing_min = _landing//60
|
||||
_landing_sec = _landing%60
|
||||
print("Time to landing: %d sec, %s:%s " % (_landing, _landing_min,_landing_sec))
|
||||
print("")
|
||||
|
|
@ -0,0 +1,137 @@
|
|||
#!/usr/bin/env python
|
||||
#
|
||||
# Project Horus - Browser-Based Chase Mapper
|
||||
# Listeners
|
||||
#
|
||||
# Copyright (C) 2018 Mark Jessop <vk5qi@rfhead.net>
|
||||
# Released under GNU GPL v3 or later
|
||||
#
|
||||
|
||||
from math import radians, degrees, sin, cos, atan2, sqrt, pi
|
||||
|
||||
def position_info(listener, balloon):
|
||||
"""
|
||||
Calculate and return information from 2 (lat, lon, alt) tuples
|
||||
|
||||
Copyright 2012 (C) Daniel Richman; GNU GPL 3
|
||||
|
||||
Returns a dict with:
|
||||
|
||||
- angle at centre
|
||||
- great circle distance
|
||||
- distance in a straight line
|
||||
- bearing (azimuth or initial course)
|
||||
- elevation (altitude)
|
||||
|
||||
Input and output latitudes, longitudes, angles, bearings and elevations are
|
||||
in degrees, and input altitudes and output distances are in meters.
|
||||
"""
|
||||
|
||||
# Earth:
|
||||
#radius = 6371000.0
|
||||
radius = 6364963.0 # Optimized for Australia :-)
|
||||
|
||||
(lat1, lon1, alt1) = listener
|
||||
(lat2, lon2, alt2) = balloon
|
||||
|
||||
lat1 = radians(lat1)
|
||||
lat2 = radians(lat2)
|
||||
lon1 = radians(lon1)
|
||||
lon2 = radians(lon2)
|
||||
|
||||
# Calculate the bearing, the angle at the centre, and the great circle
|
||||
# distance using Vincenty's_formulae with f = 0 (a sphere). See
|
||||
# http://en.wikipedia.org/wiki/Great_circle_distance#Formulas and
|
||||
# http://en.wikipedia.org/wiki/Great-circle_navigation and
|
||||
# http://en.wikipedia.org/wiki/Vincenty%27s_formulae
|
||||
d_lon = lon2 - lon1
|
||||
sa = cos(lat2) * sin(d_lon)
|
||||
sb = (cos(lat1) * sin(lat2)) - (sin(lat1) * cos(lat2) * cos(d_lon))
|
||||
bearing = atan2(sa, sb)
|
||||
aa = sqrt((sa ** 2) + (sb ** 2))
|
||||
ab = (sin(lat1) * sin(lat2)) + (cos(lat1) * cos(lat2) * cos(d_lon))
|
||||
angle_at_centre = atan2(aa, ab)
|
||||
great_circle_distance = angle_at_centre * radius
|
||||
|
||||
# Armed with the angle at the centre, calculating the remaining items
|
||||
# is a simple 2D triangley circley problem:
|
||||
|
||||
# Use the triangle with sides (r + alt1), (r + alt2), distance in a
|
||||
# straight line. The angle between (r + alt1) and (r + alt2) is the
|
||||
# angle at the centre. The angle between distance in a straight line and
|
||||
# (r + alt1) is the elevation plus pi/2.
|
||||
|
||||
# Use sum of angle in a triangle to express the third angle in terms
|
||||
# of the other two. Use sine rule on sides (r + alt1) and (r + alt2),
|
||||
# expand with compound angle formulae and solve for tan elevation by
|
||||
# dividing both sides by cos elevation
|
||||
ta = radius + alt1
|
||||
tb = radius + alt2
|
||||
ea = (cos(angle_at_centre) * tb) - ta
|
||||
eb = sin(angle_at_centre) * tb
|
||||
elevation = atan2(ea, eb)
|
||||
|
||||
# Use cosine rule to find unknown side.
|
||||
distance = sqrt((ta ** 2) + (tb ** 2) - 2 * tb * ta * cos(angle_at_centre))
|
||||
|
||||
# Give a bearing in range 0 <= b < 2pi
|
||||
if bearing < 0:
|
||||
bearing += 2 * pi
|
||||
|
||||
return {
|
||||
"listener": listener, "balloon": balloon,
|
||||
"listener_radians": (lat1, lon1, alt1),
|
||||
"balloon_radians": (lat2, lon2, alt2),
|
||||
"angle_at_centre": degrees(angle_at_centre),
|
||||
"angle_at_centre_radians": angle_at_centre,
|
||||
"bearing": degrees(bearing),
|
||||
"bearing_radians": bearing,
|
||||
"great_circle_distance": great_circle_distance,
|
||||
"straight_distance": distance,
|
||||
"elevation": degrees(elevation),
|
||||
"elevation_radians": elevation
|
||||
}
|
||||
|
||||
|
||||
def bearing_to_cardinal(bearing):
|
||||
""" Convert a bearing in degrees to a 16-point cardinal direction """
|
||||
bearing = bearing % 360.0
|
||||
|
||||
if bearing < 11.25:
|
||||
cardinal = "N"
|
||||
elif 11.25 <= bearing < 33.75:
|
||||
cardinal = "NNE"
|
||||
elif 33.75 <= bearing < 56.25:
|
||||
cardinal = "NE"
|
||||
elif 56.25 <= bearing < 78.75:
|
||||
cardinal = "ENE"
|
||||
elif 78.75 <= bearing < 101.25:
|
||||
cardinal = "E"
|
||||
elif 101.25 <= bearing < 123.75:
|
||||
cardinal = "ESE"
|
||||
elif 123.75 <= bearing < 146.25:
|
||||
cardinal = "SE"
|
||||
elif 146.25 <= bearing < 168.75:
|
||||
cardinal = "SSE"
|
||||
elif 168.75 <= bearing < 191.25:
|
||||
cardinal = "S"
|
||||
elif 191.25 <= bearing < 213.75:
|
||||
cardinal = "SSW"
|
||||
elif 213.75 <= bearing < 236.25:
|
||||
cardinal = "SW"
|
||||
elif 236.25 <= bearing < 258.75:
|
||||
cardinal = "WSW"
|
||||
elif 258.75 <= bearing < 281.25:
|
||||
cardinal = "W"
|
||||
elif 281.25 <= bearing < 303.75:
|
||||
cardinal = "WNW"
|
||||
elif 303.75 <= bearing < 326.25:
|
||||
cardinal = "NW"
|
||||
elif 326.25 <= bearing < 348.75:
|
||||
cardinal = "NNW"
|
||||
elif bearing >= 348.75:
|
||||
cardinal = "N"
|
||||
else:
|
||||
cardinal = "?"
|
||||
|
||||
return cardinal
|
||||
|
|
@ -0,0 +1,162 @@
|
|||
#!/usr/bin/env python
|
||||
#
|
||||
# Project Horus - Flight Data to Geometry
|
||||
#
|
||||
# Copyright (C) 2018 Mark Jessop <vk5qi@rfhead.net>
|
||||
# Released under GNU GPL v3 or later
|
||||
#
|
||||
import traceback
|
||||
import logging
|
||||
import numpy as np
|
||||
from .atmosphere import *
|
||||
from .earthmaths import position_info
|
||||
|
||||
|
||||
class GenericTrack(object):
|
||||
"""
|
||||
A Generic 'track' object, which stores track positions for a payload or chase car.
|
||||
Telemetry is added using the add_telemetry method, which takes a dictionary with time/lat/lon/alt keys (at minimum).
|
||||
This object performs a running average of the ascent/descent rate, and calculates the predicted landing rate if the payload
|
||||
is in descent.
|
||||
The track history can be exported to a LineString using the to_line_string method.
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
ascent_averaging = 6,
|
||||
landing_rate = 5.0):
|
||||
''' Create a GenericTrack Object. '''
|
||||
|
||||
# Averaging rate.
|
||||
self.ASCENT_AVERAGING = ascent_averaging
|
||||
# Payload state.
|
||||
self.landing_rate = landing_rate
|
||||
self.ascent_rate = 5.0
|
||||
self.heading = 0.0
|
||||
self.speed = 0.0
|
||||
self.is_descending = False
|
||||
|
||||
# Internal store of track history data.
|
||||
# Data is stored as a list-of-lists, with elements of [datetime, lat, lon, alt, comment]
|
||||
self.track_history = []
|
||||
|
||||
|
||||
def add_telemetry(self,data_dict):
|
||||
'''
|
||||
Accept telemetry data as a dictionary with fields
|
||||
datetime, lat, lon, alt, comment
|
||||
'''
|
||||
|
||||
try:
|
||||
_datetime = data_dict['time']
|
||||
_lat = data_dict['lat']
|
||||
_lon = data_dict['lon']
|
||||
_alt = data_dict['alt']
|
||||
if 'comment' in data_dict.keys():
|
||||
_comment = data_dict['comment']
|
||||
else:
|
||||
_comment = ""
|
||||
|
||||
self.track_history.append([_datetime, _lat, _lon, _alt, _comment])
|
||||
self.update_states()
|
||||
return self.get_latest_state()
|
||||
except:
|
||||
logging.error("Error reading input data: %s" % traceback.format_exc())
|
||||
|
||||
|
||||
def get_latest_state(self):
|
||||
''' Get the latest position of the payload '''
|
||||
|
||||
if len(self.track_history) == 0:
|
||||
return None
|
||||
else:
|
||||
_latest_position = self.track_history[-1]
|
||||
_state = {
|
||||
'time' : _latest_position[0],
|
||||
'lat' : _latest_position[1],
|
||||
'lon' : _latest_position[2],
|
||||
'alt' : _latest_position[3],
|
||||
'ascent_rate': self.ascent_rate,
|
||||
'is_descending': self.is_descending,
|
||||
'landing_rate': self.landing_rate,
|
||||
'heading': self.heading,
|
||||
'speed': self.speed
|
||||
}
|
||||
return _state
|
||||
|
||||
|
||||
def calculate_ascent_rate(self):
|
||||
''' Calculate the ascent/descent rate of the payload based on the available data '''
|
||||
if len(self.track_history) <= 1:
|
||||
return 5.0
|
||||
elif len(self.track_history) == 2:
|
||||
# Basic ascent rate case - only 2 samples.
|
||||
_time_delta = (self.track_history[-1][0] - self.track_history[-2][0]).total_seconds()
|
||||
_altitude_delta = self.track_history[-1][3] - self.track_history[-2][3]
|
||||
return _altitude_delta/_time_delta
|
||||
|
||||
else:
|
||||
_num_samples = min(len(self.track_history), self.ASCENT_AVERAGING)
|
||||
_asc_rates = []
|
||||
|
||||
for _i in range(-1*(_num_samples-1), 0):
|
||||
_time_delta = (self.track_history[_i][0] - self.track_history[_i-1][0]).total_seconds()
|
||||
_altitude_delta = self.track_history[_i][3] - self.track_history[_i-1][3]
|
||||
_asc_rates.append(_altitude_delta/_time_delta)
|
||||
|
||||
return np.mean(_asc_rates)
|
||||
|
||||
def calculate_heading(self):
|
||||
''' Calculate the heading of the payload '''
|
||||
if len(self.track_history) <= 1:
|
||||
return 0.0
|
||||
else:
|
||||
_pos_1 = self.track_history[-2]
|
||||
_pos_2 = self.track_history[-1]
|
||||
|
||||
_pos_info = position_info((_pos_1[1],_pos_1[2],_pos_1[3]), (_pos_2[1],_pos_2[2],_pos_2[3]))
|
||||
|
||||
return _pos_info['bearing']
|
||||
|
||||
def calculate_speed(self):
|
||||
""" Calculate Payload Speed in metres per second """
|
||||
if len(self.track_history)<=1:
|
||||
return 0.0
|
||||
else:
|
||||
_time_delta = (self.track_history[-1][0] - self.track_history[-2][0]).total_seconds()
|
||||
_pos_1 = self.track_history[-2]
|
||||
_pos_2 = self.track_history[-1]
|
||||
|
||||
_pos_info = position_info((_pos_1[1],_pos_1[2],_pos_1[3]), (_pos_2[1],_pos_2[2],_pos_2[3]))
|
||||
|
||||
_speed = _pos_info['great_circle_distance']/_time_delta
|
||||
|
||||
return _speed
|
||||
|
||||
|
||||
def update_states(self):
|
||||
''' Update internal states based on the current data '''
|
||||
self.ascent_rate = self.calculate_ascent_rate()
|
||||
self.heading = self.calculate_heading()
|
||||
self.speed = self.calculate_speed()
|
||||
self.is_descending = self.ascent_rate < 0.0
|
||||
|
||||
if self.is_descending:
|
||||
_current_alt = self.track_history[-1][3]
|
||||
self.landing_rate = seaLevelDescentRate(self.ascent_rate, _current_alt)
|
||||
|
||||
|
||||
def to_polyline(self):
|
||||
''' Generate and return a Leaflet PolyLine compatible array '''
|
||||
# Copy array into a numpy representation for easier slicing.
|
||||
if len(self.track_history) == 0:
|
||||
return []
|
||||
elif len(self.track_history) == 1:
|
||||
# LineStrings need at least 2 points. If we only have a single point,
|
||||
# fudge it by duplicating the single point.
|
||||
_track_data_np = np.array([self.track_history[0], self.track_history[0]])
|
||||
else:
|
||||
_track_data_np = np.array(self.track_history)
|
||||
# Produce new array
|
||||
_track_points = np.column_stack((_track_data_np[:,1], _track_data_np[:,2], _track_data_np[:,3]))
|
||||
|
||||
return _track_points.tolist()
|
||||
|
|
@ -0,0 +1,243 @@
|
|||
#!/usr/bin/env python
|
||||
#
|
||||
# Project Horus - Browser-Based Chase Mapper
|
||||
# GPS Communication Classes
|
||||
#
|
||||
# Copyright (C) 2018 Mark Jessop <vk5qi@rfhead.net>
|
||||
# Released under GNU GPL v3 or later
|
||||
#
|
||||
import logging
|
||||
import re
|
||||
import traceback
|
||||
from datetime import datetime
|
||||
from threading import Thread
|
||||
|
||||
class SerialGPS(object):
|
||||
'''
|
||||
Read NMEA strings from a serial-connected GPS receiver
|
||||
'''
|
||||
|
||||
def __init__(self,
|
||||
serial_port = '/dev/ttyUSB0',
|
||||
serial_baud = 9600,
|
||||
timeout = 5,
|
||||
callback = None):
|
||||
'''
|
||||
Initialise a SerialGPS object.
|
||||
|
||||
This class assumes the serial-connected GPS outputs GPRMC and GPGGA NMEA strings
|
||||
using 8N1 RS232 framing.
|
||||
|
||||
Args:
|
||||
serial_port (str): Serial port (i.e. '/dev/ttyUSB0', or 'COM1') to receive data from.
|
||||
serial_baud (int): Baud rate.
|
||||
timeout (int): Serial port readline timeout (Seconds)
|
||||
callback (function): function to pass valid GPS positions to.
|
||||
GPS data is passed as a dictionary with fields matching the Horus UDP GPS message:
|
||||
packet = {
|
||||
'type' : 'GPS',
|
||||
'latitude': lat,
|
||||
'longitude': lon,
|
||||
'altitude': alt,
|
||||
'speed': speed*3.6, # Convert speed to kph.
|
||||
'valid': position_valid
|
||||
}
|
||||
'''
|
||||
|
||||
self.serial_port = serial_port
|
||||
self.serial_baud = serial_baud
|
||||
self.timeout = timeout
|
||||
self.callback = callback
|
||||
|
||||
# Current GPS state, in a format which matches the Horus UDP
|
||||
# 'Chase Car Position' message.
|
||||
self.gps_state = {
|
||||
'type': 'GPS',
|
||||
'latitude': 0.0,
|
||||
'longitude': 0.0,
|
||||
'altitude': 0.0,
|
||||
'speed': 0.0,
|
||||
'valid': False
|
||||
}
|
||||
|
||||
self.serial_thread_running = False
|
||||
self.serial_thread = None
|
||||
self.ser = None
|
||||
|
||||
self.start()
|
||||
|
||||
|
||||
def start(self):
|
||||
''' Start the GPS thread '''
|
||||
if self.serial_thread != None:
|
||||
return
|
||||
else:
|
||||
self.serial_thread_running = True
|
||||
self.serial_thread = Thread(target=self.gps_thread)
|
||||
self.serial_thread.start()
|
||||
|
||||
|
||||
def close(self):
|
||||
''' Stop the GPS thread. '''
|
||||
self.serial_thread_running = False
|
||||
# Wait for the thread to close.
|
||||
if self.serial_thread != None:
|
||||
self.serial_thread.join()
|
||||
|
||||
|
||||
def gps_thread(self):
|
||||
'''
|
||||
Attempt to connect to a serial port and read lines of text from it.
|
||||
Pass all lines on to the NMEA parser function.
|
||||
'''
|
||||
|
||||
try:
|
||||
import serial
|
||||
except ImportError:
|
||||
logging.critical("Could not import pyserial library!")
|
||||
return
|
||||
|
||||
|
||||
while self.serial_thread_running:
|
||||
# Attempt to connect to the serial port.
|
||||
while self.ser == None and self.serial_thread_running:
|
||||
try:
|
||||
self.ser = serial.Serial(port=self.serial_port,baudrate=self.serial_baud,timeout=self.timeout)
|
||||
logging.info("SerialGPS - Connected to serial port.")
|
||||
except Exception as e:
|
||||
# Continue re-trying until we can connect to the serial port.
|
||||
# This should let the user connect the gps *after* this object if instantiated if required.
|
||||
logging.error("SerialGPS - Serial Port Error: %s" % e)
|
||||
logging.error("SerialGPS - Sleeping 10s before attempting re-connect.")
|
||||
time.sleep(10)
|
||||
self.ser = None
|
||||
continue
|
||||
|
||||
# Read a line of (hopefully) NMEA from the serial port.
|
||||
try:
|
||||
data = self.ser.readline()
|
||||
except:
|
||||
# If we hit a serial read error, attempt to reconnect.
|
||||
logging.error("SerialGPS - Error reading from serial device! Attempting to reconnect.")
|
||||
self.ser = None
|
||||
continue
|
||||
|
||||
# Attempt to parse data.
|
||||
try:
|
||||
self.parse_nmea(data.decode('ascii'))
|
||||
except:
|
||||
pass
|
||||
|
||||
# Clean up before exiting thread.
|
||||
try:
|
||||
self.ser.close()
|
||||
except:
|
||||
pass
|
||||
logging.info("SerialGPS - Closing Thread.")
|
||||
|
||||
|
||||
def dm_to_sd(self, dm):
|
||||
'''
|
||||
Converts a geographic coordiante given in "degres/minutes" dddmm.mmmm
|
||||
format (ie, "12319.943281" = 123 degrees, 19.953281 minutes) to a signed
|
||||
decimal (python float) format.
|
||||
Courtesy of https://github.com/Knio/pynmea2/
|
||||
'''
|
||||
# '12319.943281'
|
||||
if not dm or dm == '0':
|
||||
return 0.
|
||||
|
||||
d, m = re.match(r'^(\d+)(\d\d\.\d+)$', dm).groups()
|
||||
return float(d) + float(m) / 60
|
||||
|
||||
|
||||
def parse_nmea(self, data):
|
||||
'''
|
||||
Attempt to parse a line of NMEA data.
|
||||
If we have received a GPGGA string containing a position valid flag,
|
||||
send the data on to the callback function.
|
||||
'''
|
||||
if "$GPRMC" in data:
|
||||
logging.debug("SerialGPS - Got GPRMC.")
|
||||
gprmc = data.split(",")
|
||||
gprmc_lat = self.dm_to_sd(gprmc[3])
|
||||
gprmc_latns = gprmc[4]
|
||||
gprmc_lon = self.dm_to_sd(gprmc[5])
|
||||
gprmc_lonew = gprmc[6]
|
||||
gprmc_speed = float(gprmc[7])
|
||||
|
||||
if gprmc_latns == "S":
|
||||
self.gps_state['latitude'] = gprmc_lat*-1.0
|
||||
else:
|
||||
self.gps_state['latitude'] = gprmc_lat
|
||||
|
||||
if gprmc_lon == "W":
|
||||
self.gps_state['longitude'] = gprmc_lon*-1.0
|
||||
else:
|
||||
self.gps_state['longitude'] = gprmc_lon
|
||||
|
||||
self.gps_state['speed'] = gprmc_speed*0.51444*3.6
|
||||
|
||||
elif "$GPGGA" in data:
|
||||
logging.debug("SerialGPS - Got GPGGA.")
|
||||
gpgga = data.split(",")
|
||||
gpgga_lat = self.dm_to_sd(gpgga[2])
|
||||
gpgga_latns = gpgga[3]
|
||||
gpgga_lon = self.dm_to_sd(gpgga[4])
|
||||
gpgga_lonew = gpgga[5]
|
||||
gpgga_fixstatus = gpgga[6]
|
||||
self.gps_state['altitude'] = float(gpgga[9])
|
||||
|
||||
|
||||
if gpgga_latns == "S":
|
||||
self.gps_state['latitude'] = gpgga_lat*-1.0
|
||||
else:
|
||||
self.gps_state['latitude'] = gpgga_lat
|
||||
|
||||
if gpgga_lon == "W":
|
||||
self.gps_state['longitude'] = gpgga_lon*-1.0
|
||||
else:
|
||||
self.gps_state['longitude'] = gpgga_lon
|
||||
|
||||
if gpgga_fixstatus == 0:
|
||||
self.gps_state['valid'] = False
|
||||
else:
|
||||
self.gps_state['valid'] = True
|
||||
self.send_to_callback()
|
||||
|
||||
else:
|
||||
# Discard all other lines
|
||||
pass
|
||||
|
||||
|
||||
def send_to_callback(self):
|
||||
'''
|
||||
Send the current GPS data snapshot onto the callback function,
|
||||
if one exists.
|
||||
'''
|
||||
# Generate a copy of the gps state
|
||||
_state = self.gps_state.copy()
|
||||
|
||||
# Attempt to pass it onto the callback function.
|
||||
if self.callback != None:
|
||||
try:
|
||||
self.callback(_state)
|
||||
except Exception as e:
|
||||
traceback.print_exc()
|
||||
logging.error("SerialGPS - Error Passing data to callback - %s" % str(e))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
import sys, time
|
||||
logging.basicConfig(format='%(asctime)s %(levelname)s:%(message)s', level=logging.DEBUG)
|
||||
_port = sys.argv[1]
|
||||
_baud = 9600
|
||||
|
||||
def print_data(data):
|
||||
print(data)
|
||||
|
||||
_gps = SerialGPS(serial_port=_port, serial_baud=_baud, callback=print_data)
|
||||
|
||||
time.sleep(20)
|
||||
_gps.close()
|
||||
|
||||
|
|
@ -0,0 +1,251 @@
|
|||
#!/usr/bin/env python
|
||||
#
|
||||
# Project Horus - Browser-Based Chase Mapper
|
||||
# Listeners
|
||||
#
|
||||
# Copyright (C) 2018 Mark Jessop <vk5qi@rfhead.net>
|
||||
# Released under GNU GPL v3 or later
|
||||
#
|
||||
# These classes have been pulled in from the horuslib library, to avoid
|
||||
# requiring horuslib (hopefully soon-to-be retired) as a dependency.
|
||||
|
||||
import socket, json, sys, traceback
|
||||
from threading import Thread
|
||||
from dateutil.parser import parse
|
||||
from datetime import datetime
|
||||
|
||||
MAX_JSON_LEN = 2048
|
||||
|
||||
class UDPListener(object):
|
||||
''' UDP Broadcast Packet Listener
|
||||
Listens for Horuslib UDP broadcast packets, and passes them onto a callback function
|
||||
'''
|
||||
|
||||
def __init__(self,
|
||||
callback=None,
|
||||
summary_callback = None,
|
||||
gps_callback = None,
|
||||
port=55672):
|
||||
|
||||
self.udp_port = port
|
||||
self.callback = callback
|
||||
self.summary_callback = summary_callback
|
||||
self.gps_callback = gps_callback
|
||||
|
||||
self.listener_thread = None
|
||||
self.s = None
|
||||
self.udp_listener_running = False
|
||||
|
||||
|
||||
def handle_udp_packet(self, packet):
|
||||
''' Process a received UDP packet '''
|
||||
try:
|
||||
packet_dict = json.loads(packet)
|
||||
|
||||
if self.callback is not None:
|
||||
self.callback(packet_dict)
|
||||
|
||||
if packet_dict['type'] == 'PAYLOAD_SUMMARY':
|
||||
if self.summary_callback is not None:
|
||||
self.summary_callback(packet_dict)
|
||||
|
||||
if packet_dict['type'] == 'GPS':
|
||||
if self.gps_callback is not None:
|
||||
self.gps_callback(packet_dict)
|
||||
|
||||
except Exception as e:
|
||||
print("Could not parse packet: %s" % str(e))
|
||||
traceback.print_exc()
|
||||
|
||||
|
||||
def udp_rx_thread(self):
|
||||
''' Listen for Broadcast UDP packets '''
|
||||
|
||||
self.s = socket.socket(socket.AF_INET,socket.SOCK_DGRAM)
|
||||
self.s.settimeout(1)
|
||||
self.s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
||||
try:
|
||||
self.s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1)
|
||||
except:
|
||||
pass
|
||||
self.s.bind(('',self.udp_port))
|
||||
print("Started UDP Listener Thread.")
|
||||
self.udp_listener_running = True
|
||||
|
||||
while self.udp_listener_running:
|
||||
try:
|
||||
m = self.s.recvfrom(MAX_JSON_LEN)
|
||||
except socket.timeout:
|
||||
m = None
|
||||
except:
|
||||
traceback.print_exc()
|
||||
|
||||
if m != None:
|
||||
self.handle_udp_packet(m[0])
|
||||
|
||||
print("Closing UDP Listener")
|
||||
self.s.close()
|
||||
|
||||
|
||||
def start(self):
|
||||
if self.listener_thread is None:
|
||||
self.listener_thread = Thread(target=self.udp_rx_thread)
|
||||
self.listener_thread.start()
|
||||
|
||||
|
||||
def close(self):
|
||||
self.udp_listener_running = False
|
||||
self.listener_thread.join()
|
||||
|
||||
|
||||
class OziListener(object):
|
||||
"""
|
||||
Listen on a supplied UDP port for OziPlotter-compatible telemetry data.
|
||||
|
||||
Incoming sentences are of the form:
|
||||
TELEMETRY.HH:MM:SS,latitude,longitude,altitude\n
|
||||
WAYPOINT,waypoint_name,latitude,longitude,comment\n
|
||||
"""
|
||||
|
||||
allowed_sentences = ['TELEMETRY', 'WAYPOINT']
|
||||
|
||||
def __init__(self,
|
||||
hostname = '',
|
||||
port = 8942,
|
||||
telemetry_callback = None,
|
||||
waypoint_callback = None):
|
||||
|
||||
self.input_host = hostname
|
||||
self.input_port = port
|
||||
self.telemetry_callback = telemetry_callback
|
||||
self.waypoint_callback = waypoint_callback
|
||||
|
||||
self.start()
|
||||
|
||||
|
||||
def start(self):
|
||||
''' Start the UDP Listener Thread. '''
|
||||
self.udp_listener_running = True
|
||||
|
||||
self.t = Thread(target=self.udp_rx_thread)
|
||||
self.t.start()
|
||||
|
||||
|
||||
def udp_rx_thread(self):
|
||||
"""
|
||||
Listen for incoming UDP packets, and pass them off to another function to be processed.
|
||||
"""
|
||||
|
||||
self.s = socket.socket(socket.AF_INET,socket.SOCK_DGRAM)
|
||||
self.s.settimeout(1)
|
||||
self.s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
||||
try:
|
||||
self.s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1)
|
||||
except:
|
||||
pass
|
||||
self.s.bind((self.input_host, self.input_port))
|
||||
|
||||
while self.udp_listener_running:
|
||||
try:
|
||||
m = self.s.recvfrom(1024)
|
||||
except socket.timeout:
|
||||
m = None
|
||||
except:
|
||||
traceback.print_exc()
|
||||
|
||||
if m != None:
|
||||
try:
|
||||
self.handle_packet(m[0])
|
||||
except:
|
||||
traceback.print_exc()
|
||||
print("ERROR: Couldn't handle packet correctly.")
|
||||
pass
|
||||
|
||||
print("INFO: Closing UDP Listener Thread")
|
||||
self.s.close()
|
||||
|
||||
|
||||
def close(self):
|
||||
"""
|
||||
Close the UDP listener thread.
|
||||
"""
|
||||
self.udp_listener_running = False
|
||||
try:
|
||||
self.t.join()
|
||||
except:
|
||||
pass
|
||||
|
||||
|
||||
def handle_telemetry_packet(self, packet):
|
||||
''' Split a telemetry packet into time/lat/lon/alt, and pass it onto a callback '''
|
||||
|
||||
_fields = packet.split(',')
|
||||
_short_time = _fields[1]
|
||||
_lat = float(_fields[2])
|
||||
_lon = float(_fields[3])
|
||||
_alt = float(_fields[4])
|
||||
|
||||
# Timestamp Handling
|
||||
# The 'short' timestamp (HH:MM:SS) is always assumed to be in UTC time.
|
||||
# To build up a complete datetime object, we use the system's current UTC time, and replace the HH:MM:SS part.
|
||||
_full_time = datetime.utcnow().strftime("%Y-%m-%dT") + _short_time + "Z"
|
||||
_time_dt = parse(_full_time)
|
||||
|
||||
_output = {
|
||||
'time' : _time_dt,
|
||||
'lat' : _lat,
|
||||
'lon' : _lon,
|
||||
'alt' : _alt,
|
||||
'comment' : 'Telemetry Data'
|
||||
}
|
||||
|
||||
self.telemetry_callback(_output)
|
||||
|
||||
|
||||
def handle_waypoint_packet(self, packet):
|
||||
''' Split a 'Waypoint' packet into fields, and pass onto a callback '''
|
||||
|
||||
_fields = packet.split(',')
|
||||
_waypoint_name = _fields[1]
|
||||
_lat = float(_fields[2])
|
||||
_lon = float(_fields[3])
|
||||
_comment = _fields[4]
|
||||
|
||||
_time_dt = datetime.utcnow()
|
||||
|
||||
_output = {
|
||||
'time' : _time_dt,
|
||||
'name' : _waypoint_name,
|
||||
'lat' : _lat,
|
||||
'lon' : _lon,
|
||||
'comment' : _comment
|
||||
}
|
||||
|
||||
self.waypoint_callback(_output)
|
||||
|
||||
|
||||
def handle_packet(self, packet):
|
||||
"""
|
||||
Check an incoming packet matches a valid type, and then forward it on.
|
||||
"""
|
||||
|
||||
# Extract header (first field)
|
||||
packet_type = packet.split(',')[0]
|
||||
|
||||
if packet_type not in self.allowed_sentences:
|
||||
print("ERROR: Got unknown packet: %s" % packet)
|
||||
return
|
||||
|
||||
try:
|
||||
# Now send on the packet if we are allowed to.
|
||||
if packet_type == "TELEMETRY" and (self.telemetry_callback != None):
|
||||
self.handle_telemetry_packet(packet)
|
||||
|
||||
# Generally we always want to pass on waypoint data.
|
||||
if packet_type == "WAYPOINT" and (self.waypoint_callback != None):
|
||||
self.handle_waypoint_packet(packet)
|
||||
|
||||
except:
|
||||
print("ERROR: Error when handling packet.")
|
||||
traceback.print_exc()
|
||||
|
||||
|
|
@ -16,12 +16,12 @@ import traceback
|
|||
from threading import Thread
|
||||
from datetime import datetime, timedelta
|
||||
from dateutil.parser import parse
|
||||
from horuslib import *
|
||||
from horuslib.geometry import *
|
||||
from horuslib.atmosphere import time_to_landing
|
||||
from horuslib.listener import OziListener, UDPListener
|
||||
from horuslib.earthmaths import *
|
||||
|
||||
from chasemapper.config import *
|
||||
from chasemapper.earthmaths import *
|
||||
from chasemapper.geometry import *
|
||||
from chasemapper.atmosphere import time_to_landing
|
||||
from chasemapper.listeners import OziListener, UDPListener
|
||||
from chasemapper.predictor import predictor_spawn_download, model_download_running
|
||||
|
||||
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue