diff --git a/chasemapper/bearings.py b/chasemapper/bearings.py new file mode 100644 index 0000000..9de3c2b --- /dev/null +++ b/chasemapper/bearings.py @@ -0,0 +1,242 @@ +#!/usr/bin/env python +# +# Project Horus - Bearing Handler +# +# Copyright (C) 2019 Mark Jessop +# 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') + + + + + + + + + + diff --git a/chasemapper/config.py b/chasemapper/config.py index 43af74b..8e738a4 100644 --- a/chasemapper/config.py +++ b/chasemapper/config.py @@ -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') diff --git a/chasemapper/geometry.py b/chasemapper/geometry.py index 736cb96..0b8c302 100644 --- a/chasemapper/geometry.py +++ b/chasemapper/geometry.py @@ -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: diff --git a/chasemapper/listeners.py b/chasemapper/listeners.py index 620e18f..0d6ad3c 100644 --- a/chasemapper/listeners.py +++ b/chasemapper/listeners.py @@ -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() diff --git a/horusmapper.cfg.example b/horusmapper.cfg.example index e5a1eda..4d6f430 100644 --- a/horusmapper.cfg.example +++ b/horusmapper.cfg.example @@ -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 diff --git a/horusmapper.py b/horusmapper.py index 990e693..85adb6c 100644 --- a/horusmapper.py +++ b/horusmapper.py @@ -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/") 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']])