kopia lustrzana https://github.com/projecthorus/chasemapper
Backend bearing processing
Mark Jessop
rodzic
0bb7a273e0
commit
307c0ef667
|
|
@ -0,0 +1,242 @@
|
|||
#!/usr/bin/env python
|
||||
#
|
||||
# Project Horus - Bearing Handler
|
||||
#
|
||||
# Copyright (C) 2019 Mark Jessop <vk5qi@rfhead.net>
|
||||
# Released under GNU GPL v3 or later
|
||||
#
|
||||
#
|
||||
# TODO:
|
||||
# [ ] Store a rolling buffer of car positions, to enable fusing of 'old' bearings with previous car positions.
|
||||
#
|
||||
#
|
||||
|
||||
import logging
|
||||
import time
|
||||
|
||||
|
||||
class Bearings(object):
|
||||
|
||||
|
||||
def __init__(self,
|
||||
socketio_instance = None,
|
||||
max_bearings = 300,
|
||||
max_bearing_age = 30*60
|
||||
):
|
||||
|
||||
|
||||
# Reference to the socketio instance which will be used to pass data onto web clients
|
||||
self.sio = socketio_instance
|
||||
self.max_bearings = max_bearings
|
||||
self.max_age = max_bearing_age
|
||||
|
||||
|
||||
# Bearing store
|
||||
# Bearings are stored as a dict, with the key being the timestamp (time.time())
|
||||
# when the bearing arrived in the system.
|
||||
# Each record contains:
|
||||
# {
|
||||
# 'timestamp': time.time(), # A copy of the arrival timestamp
|
||||
# 'src_timestamp': time.time(), # Optional timestamp provided by the source
|
||||
# 'lat': 0.0, # Bearing start latitude
|
||||
# 'lon': 0.0, # Bearing start longitude
|
||||
# 'speed': 0.0, # Car speed at time of bearing arrival
|
||||
# 'heading': 0.0, # Car heading at time of bearing arrival
|
||||
# 'heading_valid': False, # Indicates if the car heading is considered valid (i.e. was captured while moving)
|
||||
# 'raw_bearing': 0.0, # Raw bearing value
|
||||
# 'true_bearing': 0.0, # Bearing converted to degrees true
|
||||
# 'confidence': 0.0, # Arbitrary confidence value - TBD what ranges this will take.
|
||||
# }
|
||||
self.bearings = {}
|
||||
|
||||
|
||||
# Internal record of the chase car position, which is updated with incoming GPS data.
|
||||
# If incoming bearings do not contain lat/lon information, we fuse them with this position,
|
||||
# as long as it is valid.
|
||||
self.current_car_position = {
|
||||
'timestamp': None, # System timestamp from time.time()
|
||||
'datetime': None, # Datetime object from data source.
|
||||
'lat': 0.0,
|
||||
'lon': 0.0,
|
||||
'alt': 0.0,
|
||||
'heading': 0.0,
|
||||
'heading_valid': False,
|
||||
'position_valid': False
|
||||
}
|
||||
|
||||
|
||||
def update_car_position(self, position):
|
||||
""" Accept a new car position, in the form of a dictionary produced by a GenericTrack object
|
||||
(refer geometry.py). This is of the form:
|
||||
|
||||
_state = {
|
||||
'time' : _latest_position[0], # Datetime object, with timezone info
|
||||
'lat' : _latest_position[1],
|
||||
'lon' : _latest_position[2],
|
||||
'alt' : _latest_position[3],
|
||||
'ascent_rate': self.ascent_rate, # Not used here
|
||||
'is_descending': self.is_descending, # Not used here
|
||||
'landing_rate': self.landing_rate, # Not used here
|
||||
'heading': self.heading, # Movement heading in degrees true
|
||||
'heading_valid': self.heading_valid, # Indicates if heading was calculated when the car was moving
|
||||
'speed': self.speed # Speed in m/s
|
||||
}
|
||||
|
||||
"""
|
||||
|
||||
# Attempt to build up new chase car position dict
|
||||
try:
|
||||
_car_pos = {
|
||||
'timestamp': time.time(),
|
||||
'datetime': position['time'],
|
||||
'lat': position['lat'],
|
||||
'lon': position['lon'],
|
||||
'alt': position['alt'],
|
||||
'heading': position['heading'],
|
||||
'heading_valid': position['heading_valid'],
|
||||
'speed': position['speed'],
|
||||
'position_valid': True # Should we be taking this from upstream somewhere?
|
||||
}
|
||||
|
||||
# Mark position as invalid if we have zero lat/lon values
|
||||
if (_car_pos['lat'] == 0.0) and (_car_pos['lon'] == 0.0):
|
||||
_car_pos['position_valid'] = False
|
||||
|
||||
# Replace car position state with new data
|
||||
self.current_car_position = _car_pos
|
||||
|
||||
except Exception as e:
|
||||
logging.error("Bearing Handler - Invalid car position: %s" % str(e))
|
||||
|
||||
|
||||
def add_bearing(self, bearing):
|
||||
""" Add a bearing into the store, fusing incoming data with the latest car position as required.
|
||||
|
||||
bearing must be a dictionary with the following keys:
|
||||
|
||||
# Absolute bearings - lat/lon and true bearing provided
|
||||
{'type': 'BEARING', 'bearing_type': 'absolute', 'latitude': latitude, 'longitude': longitude, 'bearing': bearing}
|
||||
|
||||
# Relative bearings - only relative bearing is provided.
|
||||
{'type': 'BEARING', 'bearing_type': 'relative', 'bearing': bearing}
|
||||
|
||||
The following optional fields can be provided:
|
||||
'timestamp': A timestamp of the bearing provided by the source.
|
||||
'confidence': A confidence value for the bearing, from 0 to [MAX VALUE ??]
|
||||
|
||||
"""
|
||||
|
||||
# Should never be passed a non-bearing dict, but check anyway,
|
||||
if bearing['type'] != 'BEARING':
|
||||
return
|
||||
|
||||
_arrival_time = time.time()
|
||||
|
||||
# Get a copy of the current car position, in case it is updated
|
||||
_current_car_pos = self.current_car_position.copy()
|
||||
|
||||
|
||||
if 'timestamp' in bearing:
|
||||
_src_timestamp = bearing['timestamp']
|
||||
else:
|
||||
_src_timestamp = _arrival_time
|
||||
|
||||
if 'confidence' in bearing:
|
||||
_confidence = bearing['confidence']
|
||||
else:
|
||||
_confidence = 100.0
|
||||
|
||||
try:
|
||||
if bearing['bearing_type'] == 'relative':
|
||||
# Relative bearing - we need to fuse this with the current car position.
|
||||
|
||||
_new_bearing = {
|
||||
'timestamp': _arrival_time,
|
||||
'src_timestamp': _src_timestamp,
|
||||
'lat': _current_car_pos['lat'],
|
||||
'lon': _current_car_pos['lon'],
|
||||
'speed': _current_car_pos['speed'],
|
||||
'heading': _current_car_pos['heading'],
|
||||
'heading_valid': _current_car_pos['heading_valid'],
|
||||
'raw_bearing': bearing['bearing'],
|
||||
'true_bearing': (bearing['bearing'] + _current_car_pos['heading']) % 360.0,
|
||||
'confidence': _confidence
|
||||
}
|
||||
|
||||
elif bearing['bearing_type'] == 'absolute':
|
||||
# Absolute bearing - use the provided data as-is
|
||||
|
||||
_new_bearing = {
|
||||
'timestamp': _arrival_time,
|
||||
'src_timestamp': _src_timestamp,
|
||||
'lat': bearing['latitude'],
|
||||
'lon': bearing['longitude'],
|
||||
'speed': 0.0,
|
||||
'heading': 0.0,
|
||||
'heading_valid': True,
|
||||
'raw_bearing': bearing['bearing'],
|
||||
'true_bearing': bearing['bearing'],
|
||||
'confidence': _confidence
|
||||
}
|
||||
|
||||
|
||||
else:
|
||||
return
|
||||
|
||||
except Exception as e:
|
||||
logging.error("Bearing Handler - Invalid input bearing: %s" % str(e))
|
||||
return
|
||||
|
||||
# We now have our bearing - now we need to store it
|
||||
self.bearings["%.4f" % _arrival_time] = _new_bearing
|
||||
|
||||
# Now we need to do a clean-up of our bearing list.
|
||||
# At this point, we should always have at least 2 bearings in our store
|
||||
if len(self.bearings) == 1:
|
||||
return
|
||||
|
||||
# Keep a list of what we remove, so we can pass it on to the web clients.
|
||||
_removal_list = []
|
||||
|
||||
# Grab the list of bearing entries, and sort them by time
|
||||
_bearing_list = self.bearings.keys()
|
||||
_bearing_list.sort()
|
||||
|
||||
# First remove any excess entries - we only get one bearing at a time, so we can do this simply:
|
||||
if len(_bearing_list) > self.max_bearings:
|
||||
self.bearings.pop(_bearing_list[0])
|
||||
_removal_list.append(_bearing_list[0])
|
||||
_bearing_list = _bearing_list[1:]
|
||||
|
||||
# Now we need to remove *old* bearings.
|
||||
_min_time = time.time() - self.max_age
|
||||
|
||||
_curr_time = float(_bearing_list[0])
|
||||
|
||||
while _curr_time < _min_time:
|
||||
# Current entry is older than our limit, remove it.
|
||||
self.bearings.pop(_bearing_list[0])
|
||||
_removal_list.append(_bearing_list[0])
|
||||
_bearing_list = _bearing_list[1:]
|
||||
|
||||
# Advance to the next entry in the list.
|
||||
_curr_time = float(_bearing_list[0])
|
||||
|
||||
|
||||
# Now we need to update the web clients on what has changed.
|
||||
_client_update = {
|
||||
'add': _new_bearing,
|
||||
'remove': _removal_list
|
||||
}
|
||||
|
||||
self.sio.emit('bearing_change', _client_update, namespace='/chasemapper')
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -39,7 +39,16 @@ default_config = {
|
|||
'range_ring_spacing': 1000,
|
||||
'range_ring_weight': 1.5,
|
||||
'range_ring_color': 'red',
|
||||
'range_ring_custom_color': '#FF0000'
|
||||
'range_ring_custom_color': '#FF0000',
|
||||
|
||||
# Bearing processing
|
||||
'max_bearings': 300,
|
||||
'max_bearing_age': 30,
|
||||
'car_speed_gate': 10,
|
||||
'bearing_length': 10,
|
||||
'bearing_weight': 1.0,
|
||||
'bearing_color': 'black',
|
||||
'bearing_custom_color': '#FF0000',
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -89,6 +98,17 @@ def parse_config_file(filename):
|
|||
chase_config['range_ring_color'] = config.get('range_rings', 'range_ring_color')
|
||||
chase_config['range_ring_custom_color'] = config.get('range_rings', 'range_ring_custom_color')
|
||||
|
||||
# Bearing Processing
|
||||
chase_config['max_bearings'] = config.getint('bearings', 'max_bearings')
|
||||
chase_config['max_bearing_age'] = config.getint('bearings', 'max_bearing_age')*60 # Convert to seconds
|
||||
if chase_config['max_bearing_age'] < 60:
|
||||
chase_config['max_bearing_age'] = 60 # Make sure this number is something sane, otherwise things will break
|
||||
chase_config['car_speed_gate'] = config.getfloat('bearings', 'car_speed_gate')*3.6 # Convert to m/s
|
||||
chase_config['bearing_length'] = config.getfloat('bearings', 'bearing_length')
|
||||
chase_config['bearing_weight'] = config.getfloat('bearings', 'bearing_weight')
|
||||
chase_config['bearing_color'] = config.get('bearings', 'bearing_color')
|
||||
chase_config['bearing_custom_color'] = config.get('bearings', 'bearing_custom_color')
|
||||
|
||||
# Offline Map Settings
|
||||
chase_config['tile_server_enabled'] = config.getboolean('offline_maps', 'tile_server_enabled')
|
||||
chase_config['tile_server_path'] = config.get('offline_maps', 'tile_server_path')
|
||||
|
|
|
|||
|
|
@ -23,15 +23,20 @@ class GenericTrack(object):
|
|||
|
||||
def __init__(self,
|
||||
ascent_averaging = 6,
|
||||
landing_rate = 5.0):
|
||||
landing_rate = 5.0,
|
||||
heading_gate_threshold = 0.0):
|
||||
''' Create a GenericTrack Object. '''
|
||||
|
||||
# Averaging rate.
|
||||
self.ASCENT_AVERAGING = ascent_averaging
|
||||
# Payload state.
|
||||
self.landing_rate = landing_rate
|
||||
# Heading gate threshold (only gate headings if moving faster than this value in m/s)
|
||||
self.heading_gate_threshold = heading_gate_threshold
|
||||
|
||||
self.ascent_rate = 0.0
|
||||
self.heading = 0.0
|
||||
self.heading_valid = False
|
||||
self.speed = 0.0
|
||||
self.is_descending = False
|
||||
|
||||
|
|
@ -58,6 +63,12 @@ class GenericTrack(object):
|
|||
|
||||
self.track_history.append([_datetime, _lat, _lon, _alt, _comment])
|
||||
self.update_states()
|
||||
|
||||
# If we have been supplied a 'true' heading with the position, override the state to use that.
|
||||
if 'heading' in data_dict:
|
||||
self.heading = data_dict['heading']
|
||||
self.heading_valid = True
|
||||
|
||||
return self.get_latest_state()
|
||||
except:
|
||||
logging.error("Error reading input data: %s" % traceback.format_exc())
|
||||
|
|
@ -79,6 +90,7 @@ class GenericTrack(object):
|
|||
'is_descending': self.is_descending,
|
||||
'landing_rate': self.landing_rate,
|
||||
'heading': self.heading,
|
||||
'heading_valid': self.heading_valid,
|
||||
'speed': self.speed
|
||||
}
|
||||
return _state
|
||||
|
|
@ -138,8 +150,14 @@ class GenericTrack(object):
|
|||
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.heading = self.calculate_heading()
|
||||
|
||||
if self.speed > self.heading_gate_threshold:
|
||||
self.heading_valid = True
|
||||
else:
|
||||
self.heading_valid = False
|
||||
|
||||
self.is_descending = self.ascent_rate < 0.0
|
||||
|
||||
if self.is_descending:
|
||||
|
|
|
|||
|
|
@ -69,12 +69,14 @@ class UDPListener(object):
|
|||
callback=None,
|
||||
summary_callback = None,
|
||||
gps_callback = None,
|
||||
bearing_callback = None,
|
||||
port=55672):
|
||||
|
||||
self.udp_port = port
|
||||
self.callback = callback
|
||||
self.summary_callback = summary_callback
|
||||
self.gps_callback = gps_callback
|
||||
self.bearing_callback = bearing_callback
|
||||
|
||||
self.listener_thread = None
|
||||
self.s = None
|
||||
|
|
@ -97,6 +99,10 @@ class UDPListener(object):
|
|||
if self.gps_callback is not None:
|
||||
self.gps_callback(packet_dict)
|
||||
|
||||
if packet_dict['type'] == 'BEARING':
|
||||
if self.bearing_callback is not None:
|
||||
self.bearing_callback(packet_dict)
|
||||
|
||||
except Exception as e:
|
||||
print("Could not parse packet: %s" % str(e))
|
||||
traceback.print_exc()
|
||||
|
|
|
|||
|
|
@ -163,3 +163,34 @@ range_ring_color = red
|
|||
|
||||
# Custom range ring color, in hexadecimal #RRGGBB
|
||||
range_ring_custom_color = #FF0000
|
||||
|
||||
|
||||
#
|
||||
# Bearing Processing
|
||||
#
|
||||
[bearings]
|
||||
|
||||
# Number of bearings to store
|
||||
max_bearings = 300
|
||||
|
||||
# Maximum age of bearings, in *minutes*.
|
||||
max_bearing_age = 30
|
||||
|
||||
# Car heading speed gate
|
||||
# Only consider car headings to be valid if the car speed is greater than this value in *kph*
|
||||
car_speed_gate = 10
|
||||
|
||||
# Visual Settings - these can be adjust in the Web GUI during runtime
|
||||
|
||||
# Bearing length in km
|
||||
bearing_length = 10
|
||||
|
||||
# Weight of the bearing lines, in pixels.
|
||||
bearing_weight = 1.0
|
||||
|
||||
# Color of the bearings.
|
||||
# Valid options are: red, black, blue, green, custom
|
||||
bearing_color = black
|
||||
|
||||
# Custom bearing color, in hexadecimal #RRGGBB
|
||||
bearing_custom_color = #FF0000
|
||||
|
|
|
|||
|
|
@ -28,6 +28,7 @@ from chasemapper.listeners import OziListener, UDPListener, fix_datetime
|
|||
from chasemapper.predictor import predictor_spawn_download, model_download_running
|
||||
from chasemapper.habitat import HabitatChaseUploader, initListenerCallsign, uploadListenerPosition
|
||||
from chasemapper.logger import ChaseLogger
|
||||
from chasemapper.bearings import Bearings
|
||||
|
||||
|
||||
# Define Flask Application, and allow automatic reloading of templates for dev work
|
||||
|
|
@ -66,10 +67,12 @@ current_payload_tracks = {} # Store of payload Track objects which are used to c
|
|||
# Chase car position
|
||||
car_track = GenericTrack()
|
||||
|
||||
# Bearing store
|
||||
bearing_store = None
|
||||
|
||||
# Habitat Chase-Car uploader object
|
||||
habitat_uploader = None
|
||||
|
||||
|
||||
# Copy out any extra fields from incoming telemetry that we want to pass on to the GUI.
|
||||
# At the moment we're really only using the burst timer field.
|
||||
EXTRA_FIELDS = ['bt', 'temp', 'humidity', 'sats']
|
||||
|
|
@ -84,7 +87,6 @@ def flask_index():
|
|||
""" Render main index page """
|
||||
return flask.render_template('index.html')
|
||||
|
||||
|
||||
@app.route("/get_telemetry_archive")
|
||||
def flask_get_telemetry_archive():
|
||||
return json.dumps(current_payloads)
|
||||
|
|
@ -94,6 +96,18 @@ def flask_get_telemetry_archive():
|
|||
def flask_get_config():
|
||||
return json.dumps(chasemapper_config)
|
||||
|
||||
|
||||
@app.route("/get_bearings")
|
||||
def flask_get_bearings():
|
||||
return json.dumps(bearing_store.bearings)
|
||||
|
||||
# Some features of the web interface require comparisons with server time,
|
||||
# so provide a route to grab it.
|
||||
@app.route('/server_time')
|
||||
def flask_get_server_time():
|
||||
return json.dumps(time.time())
|
||||
|
||||
|
||||
@app.route("/tiles/<path:filename>")
|
||||
def flask_server_tiles(filename):
|
||||
""" Serve up a file from the tile server location """
|
||||
|
|
@ -594,7 +608,7 @@ def udp_listener_car_callback(data):
|
|||
''' Handle car position data '''
|
||||
# TODO: Make a generic car position function, and have this function pass data into it
|
||||
# so we can add support for other chase car position inputs.
|
||||
global car_track, habitat_uploader
|
||||
global car_track, habitat_uploader, bearing_store
|
||||
_lat = data['latitude']
|
||||
_lon = data['longitude']
|
||||
_alt = data['altitude']
|
||||
|
|
@ -610,6 +624,10 @@ def udp_listener_car_callback(data):
|
|||
'alt' : _alt,
|
||||
'comment': _comment
|
||||
}
|
||||
# Add in true heading data if we have been supplied it
|
||||
# (Which will be the case once I end up building a better car GPS...)
|
||||
if 'heading' in data:
|
||||
_car_position_update['heading'] = data['heading']
|
||||
|
||||
car_track.add_telemetry(_car_position_update)
|
||||
|
||||
|
|
@ -624,14 +642,22 @@ def udp_listener_car_callback(data):
|
|||
if habitat_uploader != None:
|
||||
habitat_uploader.update_position(data)
|
||||
|
||||
# Update the bearing store with the current car state (position & bearing)
|
||||
if bearing_store != None:
|
||||
bearing_store.update_car_position(_state)
|
||||
|
||||
# Add the car position to the logger, but only if we are moving (>10kph = ~3m/s)
|
||||
if _speed > 3.0:
|
||||
_car_position_update['speed'] = _speed
|
||||
_car_position_update['heading'] = _heading
|
||||
chase_logger.add_car_position(_car_position_update)
|
||||
|
||||
# Add other listeners here...
|
||||
|
||||
def udp_listener_bearing_callback(data):
|
||||
global bearing_store
|
||||
|
||||
if bearing_store != None:
|
||||
bearing_store.add_bearing(data)
|
||||
|
||||
|
||||
# Data Age Monitoring Thread
|
||||
|
|
@ -701,6 +727,7 @@ def start_listeners(profile):
|
|||
logging.info("Starting single Horus UDP listener on port %d" % profile['telemetry_source_port'])
|
||||
_telem_horus_udp_listener = UDPListener(summary_callback=udp_listener_summary_callback,
|
||||
gps_callback=udp_listener_car_callback,
|
||||
bearing_callback=udp_listener_bearing_callback,
|
||||
port=profile['telemetry_source_port'])
|
||||
_telem_horus_udp_listener.start()
|
||||
data_listeners.append(_telem_horus_udp_listener)
|
||||
|
|
@ -711,6 +738,7 @@ def start_listeners(profile):
|
|||
logging.info("Starting Telemetry Horus UDP listener on port %d" % profile['telemetry_source_port'])
|
||||
_telem_horus_udp_listener = UDPListener(summary_callback=udp_listener_summary_callback,
|
||||
gps_callback=None,
|
||||
bearing_callback=udp_listener_bearing_callback,
|
||||
port=profile['telemetry_source_port'])
|
||||
_telem_horus_udp_listener.start()
|
||||
data_listeners.append(_telem_horus_udp_listener)
|
||||
|
|
@ -720,6 +748,7 @@ def start_listeners(profile):
|
|||
logging.info("Starting Car Position Horus UDP listener on port %d" % profile['car_source_port'])
|
||||
_car_horus_udp_listener = UDPListener(summary_callback=None,
|
||||
gps_callback=udp_listener_car_callback,
|
||||
bearing_callback=udp_listener_bearing_callback,
|
||||
port=profile['car_source_port'])
|
||||
_car_horus_udp_listener.start()
|
||||
data_listeners.append(_car_horus_udp_listener)
|
||||
|
|
@ -824,6 +853,15 @@ if __name__ == "__main__":
|
|||
'tile_server_path': chasemapper_config['tile_server_path']
|
||||
}
|
||||
|
||||
# Initialise Bearing store
|
||||
bearing_store = Bearings(
|
||||
socketio_instance = socketio,
|
||||
max_bearings = chasemapper_config['max_bearings'],
|
||||
max_bearing_age = chasemapper_config['max_bearing_age'])
|
||||
|
||||
# Set speed gate for car position object
|
||||
car_track.heading_gate_threshold = chasemapper_config['car_speed_gate']
|
||||
|
||||
# Start listeners using the default profile selection.
|
||||
start_listeners(chasemapper_config['profiles'][chasemapper_config['selected_profile']])
|
||||
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue