TDOA plot adjustments, turn rate gating

chasemapper/config.py

@@ -170,6 +170,13 @@ def parse_config_file(filename):
         logging.info("Missing Stadia API Key setting, using default (none)")
         chase_config["stadia_api_key"] = "none"
 
+    try:
+        chase_config["turn_rate_threshold"] = config.getfloat("bearings", "turn_rate_threshold")
+    except:
+        logging.info("Missing turn rate gate setting, using default (4m/s)")
+        chase_config["turn_rate_threshold"] = 4.0
+
+
     # Telemetry Source Profiles
 
     _profile_count = config.getint("profile_selection", "profile_count")

chasemapper/geometry.py

@@ -22,7 +22,7 @@ class GenericTrack(object):
     """
 
     def __init__(
-        self, ascent_averaging=6, landing_rate=5.0, heading_gate_threshold=0.0
+        self, ascent_averaging=6, landing_rate=5.0, heading_gate_threshold=0.0, turn_rate_threshold=4.0
     ):
         """ Create a GenericTrack Object. """
 
@@ -32,13 +32,22 @@ class GenericTrack(object):
         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
+        # Turn rate threshold - only gate headings if turning *slower* than this value in degrees/sec
+        self.turn_rate_threshold = turn_rate_threshold
 
         self.ascent_rate = 0.0
         self.heading = 0.0
+        self.turn_rate = 100.0
         self.heading_valid = False
         self.speed = 0.0
         self.is_descending = False
 
+        self.supplied_heading = False
+
+
+        self.prev_heading = 0.0
+        self.prev_time = 0.0
+
         # 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 = []
@@ -60,12 +69,20 @@ class GenericTrack(object):
                 _comment = ""
 
             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.
+            # In this case we are assuming that the heading is being provided by some form of magnetic compass,
+            # and is valid even when the car is stationary.
             if "heading" in data_dict:
+                # Rotate heading data if we have enough data
+                if len(self.track_history) >=2:
+                    self.prev_time = self.track_history[-2][0]
+                    self.prev_heading = self.heading
+
                 self.heading = data_dict["heading"]
-                self.heading_valid = True
+                self.supplied_heading = True
+
+            self.update_states()
 
             return self.get_latest_state()
         except:
@@ -88,6 +105,7 @@ class GenericTrack(object):
                 "landing_rate": self.landing_rate,
                 "heading": self.heading,
                 "heading_valid": self.heading_valid,
+                "turn_rate": self.turn_rate,
                 "speed": self.speed,
             }
             return _state
@@ -139,12 +157,20 @@ class GenericTrack(object):
         else:
             _pos_1 = self.track_history[-2]
             _pos_2 = self.track_history[-1]
+            
+            # Save previous heading.
+            self.prev_heading = self.heading
+            self.prev_time = _pos_1[0]
 
+            # Calculate new heading
             _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"]
+            self.heading = _pos_info["bearing"]
+
+            return self.heading
+
 
     def calculate_speed(self):
         """ Calculate Payload Speed in metres per second """
@@ -171,16 +197,49 @@ class GenericTrack(object):
 
             return _speed
 
+
+    def calculate_turn_rate(self):
+        """ Calculate heading rate based on previous heading and current heading """
+        if len(self.track_history) > 2:
+            # Grab current time
+            _current_time = self.track_history[-1][0]
+
+            _time_delta = (_current_time - self.prev_time).total_seconds()
+
+            _heading_delta = (self.heading - self.prev_heading) % 360.0
+            if _heading_delta >= 180.0:
+                _heading_delta -= 360.0
+            
+            self.turn_rate = abs(_heading_delta)/_time_delta
+
+            return self.turn_rate
+
+
     def update_states(self):
         """ Update internal states based on the current data """
         self.ascent_rate = self.calculate_ascent_rate()
         self.speed = self.calculate_speed()
-        self.heading = self.calculate_heading()
 
-        if self.speed > self.heading_gate_threshold:
+        # If we haven't been supplied a heading, calculate one
-            self.heading_valid = True
+        if not self.supplied_heading:
+            self.heading = self.calculate_heading()
+
+        # Calculate the turn rate
+        self.calculate_turn_rate()
+
+        if self.supplied_heading:
+            # Heading supplied - only threshold on turn rate.
+            if self.turn_rate < self.turn_rate_threshold:
+                self.heading_valid = True
+            else:
+                self.heading_valid = False
+
         else:
-            self.heading_valid = False
+            # Heading calculated - threshold on speed and turn rate.
+            if (self.speed > self.heading_gate_threshold) and (self.turn_rate < self.turn_rate_threshold):
+                self.heading_valid = True
+            else:
+                self.heading_valid = False
 
         self.is_descending = self.ascent_rate < 0.0
 

horusmapper.cfg.example

@@ -234,17 +234,24 @@ max_bearing_age = 10
 # Only consider car headings to be valid if the car speed is greater than this value in *kph*
 car_speed_gate = 10
 
+# Turn rate threshold
+# Only plot bearings if the turn rate of the vehicle is less than this value, in degrees/second
+# This helps avoid plotting bearings when the heading and bearind data might be misaligned during
+# a turn (e.g. around a roundabout)
+# 4 degrees/second seems to work fairly well.
+turn_rate_threshold = 4.0
+
 # 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 = 0.5
+bearing_weight = 1.0
 
 # Color of the bearings.
-# Valid options are: red, black, blue, green, custom
+# Valid options are: red, black, blue, green, white, custom
-bearing_color = black
+bearing_color = red
 
 # Custom bearing color, in hexadecimal #RRGGBB
 bearing_custom_color = #FF0000

horusmapper.py

@@ -1176,6 +1176,7 @@ if __name__ == "__main__":
 
     # Set speed gate for car position object
     car_track.heading_gate_threshold = chasemapper_config["car_speed_gate"]
+    car_track.turn_rate_threshold = chasemapper_config["turn_rate_threshold"]
 
     # Start listeners using the default profile selection.
     start_listeners(

static/js/bearings.js

@@ -28,6 +28,8 @@ var bearing_color = "#000000";
 var bearing_max_opacity = 0.8;
 var bearing_min_opacity = 0.1;
 
+var bearing_large_plot = false;
+
 // Store for the latest server timestamp.
 // Start out with just our own local timestamp.
 var latest_server_timestamp = Date.now()/1000.0;
@@ -51,9 +53,11 @@ function updateBearingSettings(){
 	} else if (_bearing_color == "blue"){
 		bearing_color = "#0000FF";
 	} else if (_bearing_color == "green"){
-		bearing__color = "#00FF00";
+		bearing_color = "#00AA00";
+	} else if (_bearing_color == "white"){
+		bearing_color = "#FFFFFF";
 	} else if (_bearing_color == "custom"){
-		bearing_color = _ring_custom_color;
+		bearing_color = _bearing_custom_color;
 	}
 }
 
@@ -115,19 +119,25 @@ function addBearing(timestamp, bearing, live){
 			opacity: _opacity
 		});
 
-
+	_bearing_valid = bearingValid(bearing_store[timestamp]);
-	if ( (bearingValid(bearing_store[timestamp]) == true) && (document.getElementById("bearingsEnabled").checked == true) ){
+	if ( (_bearing_valid == true) && (document.getElementById("bearingsEnabled").checked == true) ){
 		bearing_store[timestamp].line.addTo(map);
 	}
 
 	if ( (live == true) && (document.getElementById("bearingsEnabled").checked == true) ){
 		
 		if(_raw_bearing_angles.length > 0){
-			$("#bearing_table").tabulator("setData", [{id:1, bearing: bearing_store[timestamp].raw_bearing.toFixed(0), confidence: bearing_store[timestamp].confidence.toFixed(0), power: bearing_store[timestamp].power.toFixed(0)}]);
+			if (bearing_store[timestamp].confidence > bearing_confidence_threshold){
+				_valid_text = "YES";
+			}else {
+				_valid_text = "NO";
+			}
+			$("#bearing_table").tabulator("setData", [{id:1, valid_bearing:_valid_text, bearing: bearing_store[timestamp].raw_bearing.toFixed(0), confidence: bearing_store[timestamp].confidence.toFixed(0), power: bearing_store[timestamp].power.toFixed(0)}]);
 			$("#bearing_table").show();
 
 			if(document.getElementById("tdoaEnabled").checked == true){
-				bearingPlotRender(_raw_bearing_angles, _raw_doa);
+				_valid_tdoa = bearing_store[timestamp].confidence > bearing_confidence_threshold;
+				bearingPlotRender(_raw_bearing_angles, _raw_doa, _valid_tdoa);
 				$('#bearing_plot').show();
 			}else{
 				$('#bearing_plot').hide();
@@ -273,6 +283,8 @@ function toggleBearingsOnlyMode(){
 		$("#summary_table").hide();
 		$("#telem_table_btn").hide();
 		$("#telem_table").hide();
+		$("#payload_age").hide();
+		$("#pred_age").hide();
 
 		bearings_only_mode = true;
 
@@ -283,6 +295,8 @@ function toggleBearingsOnlyMode(){
 		$("#summary_table").show();
 		$("#telem_table_btn").show();
 		$("#telem_table").show();
+		$("#payload_age").show();
+		$("#pred_age").show();
 
 		bearings_only_mode = false;
 
@@ -302,45 +316,78 @@ function flushBearings(){
 }
 
 
-function bearingPlotRender(angles, doa){
 
-var _config = {
+function bearingPlotRender(angles, doa, data_valid){
-    "data": [{
+
-        "t": angles,// [0,45,90,135,180,215,270,315], // theta values (x axis)
+	// Trying a colorblind-friendly color scheme.
-        "r": doa,//[-4,-3,-2,-1,0,-1,-2,-3,-4], // radial values (y axis)
+	if(data_valid == true){
-        "name": "DOA", // name for the legend
+		_stroke_color = "#1A85FF";
-        "visible": true,
+	} else {
-        "color": "blue", // color of data element
+		_stroke_color = "#D41159";
-        "opacity": 0.8,
+	}
-        "strokeColor": "blue",
+
-        "strokeDash": "solid", // solid, dot, dash (default)
+	if(document.getElementById("bigTDOAEnabled").checked){
-        "strokeSize": 2,
+		_plot_dim = 400;
-        "visibleInLegend": false,
+	}else{
-        "geometry": "LinePlot" // AreaChart, BarChart, DotPlot, LinePlot (default)
+		_plot_dim = 250;
-    }],
+	}
-    "layout": {
+
-        "height": 250, // (default: 450)
+	if(dark_mode == true){
-        "width": 250,
+		_bg_color = "none";
-        "orientation":-90,
+	} else {
-        "showlegend": false,
+		_bg_color = "ghostwhite";
-        "backgroundColor": "ghostwhite",
+	}
-        "radialAxis": {
+
-            "domain": µ.DATAEXTENT,
+	var _config = {
-            "visible": true
+		"data": [{
-        },
+			"t": angles,// [0,45,90,135,180,215,270,315], // theta values (x axis)
-        "margin": { 
+			"r": doa,//[-4,-3,-2,-1,0,-1,-2,-3,-4], // radial values (y axis)
-            "top": 20,
+			"name": "DOA", // name for the legend
-            "right": 20,
+			"visible": true,
-            "bottom": 20,
+			"color": _stroke_color, // color of data element
-            "left": 20
+			"opacity": 1,
-        },
+			"strokeColor": _stroke_color,
-    }};
+			"strokeDash": "solid", // solid, dot, dash (default)
+			"strokeSize": 2,
+			"visibleInLegend": false,
+			"geometry": "AreaChart" // AreaChart, BarChart, DotPlot, LinePlot (default)
+		}],
+		"layout": {
+			"height": _plot_dim, // (default: 450)
+			"width": _plot_dim,
+			"orientation":-90,
+			"showlegend": false,
+			"backgroundColor": _bg_color, // "ghostwhite",
+			"radialAxis": {
+				"domain": µ.DATAEXTENT,
+				"visible": true
+			},
+			"margin": { 
+				"top": 20,
+				"right": 20,
+				"bottom": 20,
+				"left": 20
+			},
+		}};
 
     micropolar.Axis() // instantiate a new axis
   .config(_config) // configure it
   .render(d3.select('#bearing_plot'));
 }
 
+function toggle_bearing_plot_size(){
+	if(bearing_large_plot == true){
+		bearing_large_plot = false;
+	}else{
+		bearing_large_plot = true;
+	}
+
+	console.log(bearing_large_plot);
+};
+
+// TODO: This is not working
+$("#bearing_plot").click(toggle_bearing_plot_size);
+
 /**
 	Returns the point that is a distance and heading away from
 	the given origin point.

static/js/tables.js

@@ -202,11 +202,12 @@ function initTables(){
         layoutColumnsOnNewData:true,
         //selectable:1, // TODO...
         columns:[ //Define Table Columns
+            {title:"Valid", field:'valid_bearing', headerSort:false},
             {title:"Bearing", field:"bearing", headerSort:false},
             {title:"Score", field:'confidence', headerSort:false},
             {title:"Power", field:'power', headerSort:false}
         ],
-        data:[{id: 1, bearing:0.0, confidence:0.0}]
+        data:[{id: 1, valid_bearing:"NO", bearing:0.0, confidence:0.0, power:0.0}]
     });
 
     $("#bearing_table").hide();
@@ -260,11 +261,12 @@ function initTablesImperial(){
         layoutColumnsOnNewData:true,
         //selectable:1, // TODO...
         columns:[ //Define Table Columns
+            {title:"Valid", field:'valid_bearing', headerSort:false},
             {title:"Bearing", field:"bearing", headerSort:false},
             {title:"Score", field:'confidence', headerSort:false},
             {title:"Power", field:'power', headerSort:false}
         ],
-        data:[{id: 1, bearing:0.0, confidence:0.0}]
+        data:[{id: 1, valid_bearing:"NO", bearing:0.0, confidence:0.0, power:0.0}]
     });
 
     $("#bearing_table").hide();

templates/index.html

@@ -96,6 +96,10 @@
         // Functions defined later.
         var setChaseCarTrack;
 
+        // Global dark mode setting
+        var dark_mode = false;
+        var dark_mode_layers = ["Alidade Smooth Dark", "Dark Matter"];
+
         // Leaflet map instance.
         var map;
         // Routing Engine
@@ -463,6 +467,14 @@
             map.on('dragend',mapMovedEvent);
 
 
+            map.on('baselayerchange', function (e) {
+                if(dark_mode_layers.includes(e.name)){
+                    dark_mode = true;
+                }else {
+                    dark_mode = false;
+                }
+            });
+
             initTables();
 
             
@@ -867,6 +879,10 @@
                     <b>Enable TDOA Plot</b> <input type="checkbox" class="paramSelector" id="tdoaEnabled" checked>
                 </div>
 
+                <div class="paramRow">
+                    <b>Big TDOA Plot</b> <input type="checkbox" class="paramSelector" id="bigTDOAEnabled">
+                </div>
+
                 <div class="paramRow">
                     <b>Confidence Threshold</b><input type="text" class="paramEntry" id="bearingConfidenceThreshold" value="50"><br/>
                 </div>
@@ -892,7 +908,7 @@
                     <b>Bearing Length (km)</b><input type="text" class="paramEntry" id="bearingLength" value="10"><br/>
                 </div>
                 <div class="paramRow">
-                    <b>Bearing Weight (px)</b><input type="text" class="paramEntry" id="bearingWeight" value="0.5"><br/>
+                    <b>Bearing Weight (px)</b><input type="text" class="paramEntry" id="bearingWeight" value="1.0"><br/>
                 </div>
                 <div class="paramRow">
                     <b>Bearing Min Opacity</b><input type="text" class="paramEntry" id="bearingMinOpacity" value="0.1"><br/>
@@ -904,6 +920,7 @@
                     <b>Bearing Color</b>
                     <select class="paramSelector" id="bearingColorSelect" name="bearingColorSelect">
                     <option value='black'>Black</option>
+                    <option value='white'>White</option>
                     <option value='red'>Red</option>
                     <option value='green'>Green</option>
                     <option value='blue'>Blue</option>