Better support of supplied heading data. Handle identical sequential positions better. Optionally display heading on display

chasemapper/__init__.py

@@ -8,4 +8,4 @@
 
 # Now using Semantic Versioning (https://semver.org/)  MAJOR.MINOR.PATCH
 
-__version__ = "1.3.2"
+__version__ = "1.3.3"

chasemapper/geometry.py

@@ -43,6 +43,7 @@ class GenericTrack(object):
         self.is_descending = False
 
         self.supplied_heading = False
+        self.heading_status = None
 
 
         self.prev_heading = 0.0
@@ -82,6 +83,9 @@ class GenericTrack(object):
                 self.heading = data_dict["heading"]
                 self.supplied_heading = True
 
+            if "heading_status" in data_dict:
+                self.heading_status = data_dict["heading_status"]
+
             self.update_states()
 
             return self.get_latest_state()
@@ -105,6 +109,7 @@ class GenericTrack(object):
                 "landing_rate": self.landing_rate,
                 "heading": self.heading,
                 "heading_valid": self.heading_valid,
+                "heading_status": "Unknown",
                 "turn_rate": self.turn_rate,
                 "speed": self.speed,
             }
@@ -163,9 +168,13 @@ class GenericTrack(object):
             self.prev_time = _pos_1[0]
 
             # Calculate new heading
-            _pos_info = position_info(
+            try:
-                (_pos_1[1], _pos_1[2], _pos_1[3]), (_pos_2[1], _pos_2[2], _pos_2[3])
+                _pos_info = position_info(
-            )
+                    (_pos_1[1], _pos_1[2], _pos_1[3]), (_pos_2[1], _pos_2[2], _pos_2[3])
+                )
+            except ValueError:
+                logging.debug("Math Domain Error in heading calculation - Identical Sequential Positions")
+                return self.heading
 
             self.heading = _pos_info["bearing"]
 
@@ -183,9 +192,14 @@ class GenericTrack(object):
             _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])
+            try:
-            )
+                _pos_info = position_info(
+                    (_pos_1[1], _pos_1[2], _pos_1[3]), (_pos_2[1], _pos_2[2], _pos_2[3])
+                )
+            except ValueError:
+                logging.debug("Math Domain Error in speed calculation - Identical Sequential Positions")
+                return 0.0
 
             try:
                 _speed = _pos_info["great_circle_distance"] / _time_delta

chasemapper/gps.py

@@ -57,6 +57,9 @@ class SerialGPS(object):
         self.callback = callback
         self.uberdebug = uberdebug
 
+        # Indication of what the last expected string is.
+        self.last_string = "GGA"
+
         # Current GPS state, in a format which matches the Horus UDP
         # 'Chase Car Position' message.
         # Note that these packets do not contain a timestamp.
@@ -66,6 +69,8 @@ class SerialGPS(object):
             "longitude": 0.0,
             "altitude": 0.0,
             "speed": 0.0,
+            "fix_status": 0,
+            "heading": None,
             "valid": False,
         }
 
@@ -207,7 +212,8 @@ class SerialGPS(object):
             gpgga_latns = gpgga[3]
             gpgga_lon = self.dm_to_sd(gpgga[4])
             gpgga_lonew = gpgga[5]
-            gpgga_fixstatus = gpgga[6]
+            gpgga_fixstatus = int(gpgga[6])
+            self.gps_state["fix_status"] = gpgga_fixstatus
             self.gps_state["altitude"] = float(gpgga[9])
 
             if gpgga_latns == "S":
@@ -224,7 +230,49 @@ class SerialGPS(object):
                 self.gps_state["valid"] = False
             else:
                 self.gps_state["valid"] = True
+            
+            if self.last_string == "GGA":
                 self.send_to_callback()
+        
+        elif ("$GPTHS" in data) or ("$GNTHS" in data):
+            # Very basic handling of the uBlox NEO-M8U-provided True heading data.
+            # This data *appears* to be the output of the fused solution, once the system
+            # has self-calibrated. 
+            # The GNTHS message can be enabled on the USB port by sending: $PUBX,40,THS,0,0,0,1,0,0*55\r\n
+            # to the GPS.
+            logging.debug("SerialGPS - Got Heading Info (GNTHS).")
+            gnths = data.split(",")
+            try:
+                if len(gnths[1]) > 0:
+                    # Data is present in the heading field, try and parse it.
+                    gnths_heading = float(gnths[1])
+                    # Get the heading validity field.
+                    gnths_valid = gnths[2]
+
+                    if gnths_valid != "V":
+                        # Treat anything other than 'V' as a valid heading
+                        self.gps_state["heading"] = gnths_heading
+                    else:
+                        self.gps_state["heading"] = None
+                else:
+                    # Blank field, which means data is not valid.
+                    self.gps_state["heading"] = None
+
+
+                # Assume that if we are receiving GNTHS strings, that they are the last in the batch.
+                # Stop sending data when we get a GGA string.
+                self.last_string = "THS"
+
+                # Send to callback if we have lock.
+                if self.gps_state["fix_status"] != 0:
+                    self.send_to_callback()
+
+            except:
+                # Failed to parse field, which probably means an invalid heading.
+                logging.debug(f"Failed to parse GNTHS: {data}")
+                # Invalidate the heading data, and revert to emitting messages on GGA strings.
+                self.gps_state["heading"] = None
+                self.last_string = "GGA"
                 
         else:
             # Discard all other lines
@@ -238,6 +286,9 @@ class SerialGPS(object):
         # Generate a copy of the gps state
         _state = self.gps_state.copy()
 
+        if _state["heading"] is None:
+            _state.pop("heading")
+
         # Attempt to pass it onto the callback function.
         if self.callback != None:
             try:

horusmapper.py

@@ -835,15 +835,19 @@ def udp_listener_car_callback(data):
         "alt": _alt,
         "comment": _comment,
     }
-    # Add in true heading data if we have been supplied it
+    # Add in true heading data if we have been supplied it (e.g. from a uBlox NEO-M8U device)
-    # (Which will be the case once I end up building a better car GPS...)
     if "heading" in data:
         _car_position_update["heading"] = data["heading"]
 
+    if "heading_status" in data:
+        _car_position_update["heading_status"] = data["heading_status"]
+
     car_track.add_telemetry(_car_position_update)
 
     _state = car_track.get_latest_state()
     _heading = _state["heading"]
+    _heading_status = _state["heading_status"]
+    _heading_valid = _state["heading_valid"]
     _speed = _state["speed"]
 
     # Push the new car position to the web client
@@ -854,6 +858,8 @@ def udp_listener_car_callback(data):
             "position": [_lat, _lon, _alt],
             "vel_v": 0.0,
             "heading": _heading,
+            "heading_valid": _heading_valid,
+            "heading_status": _heading_status,
             "speed": _speed,
         },
     )

static/js/balloon.js

@@ -239,6 +239,7 @@ function handleTelemetry(data){
         // Update car position.
         chase_car_position.latest_data = data.position;
         chase_car_position.heading = data.heading; // degrees true
+        chase_car_position.heading_valid = data.heading_valid;
         chase_car_position.speed = data.speed; // m/s
 
         // Update range rings, if they are enabled.
@@ -257,6 +258,19 @@ function handleTelemetry(data){
             $("#chase_car_speed_header").text("");
         }
 
+        // Update heading information
+        if (document.getElementById("showCarHeading").checked){
+            if(chase_car_position.heading_valid){
+                $("#chase_car_heading").text(chase_car_position.heading.toFixed(0) + "˚");
+            }else{
+                $("#chase_car_heading").text("---˚");
+            }
+            $("#chase_car_heading_header").text("Heading");
+        } else {
+            $("#chase_car_heading").text("");
+            $("#chase_car_heading_header").text("");
+        }
+
         if (chase_car_position.marker == 'NONE'){
             // Create marker!
             chase_car_position.marker = L.marker(chase_car_position.latest_data,{title:"Chase Car", icon: carIcon, rotationOrigin: "center center"})
@@ -270,8 +284,8 @@ function handleTelemetry(data){
             chase_car_position.path.addLatLng(chase_car_position.latest_data);
             chase_car_position.marker.setLatLng(chase_car_position.latest_data).update();
         }
-        // Rotate car icon based on heading, but only if we're going faster than 20kph (5.5m/s).
+
-        if(chase_car_position.speed > 5.5){ // TODO: Remove magic number!
+        if(chase_car_position.heading_valid){
             var _car_heading = chase_car_position.heading - 90.0;
             if (_car_heading<=90.0){
                 chase_car_position.marker.setIcon(carIcon);

templates/index.html

@@ -382,6 +382,20 @@
                 }
             })
             .addTo(map);
+            // Chase Car Heading Display
+            L.control.custom({
+                position: 'bottomleft',
+                content : "<div class='dataAgeHeader' id='chase_car_heading_header'></div><div id='chase_car_heading' class='chaseCarSpeed'></div>",
+                classes : 'btn-group-vertical btn-group-sm',
+                id: 'heading_display',
+                style   :
+                {
+                    margin: '5px',
+                    padding: '0px 0 0 0',
+                    cursor: 'pointer',
+                }
+            })
+            .addTo(map);
 
             // Time-to-landing display - shows the time until landing for the currently tracked payload.
             L.control.custom({
@@ -856,10 +870,13 @@
                     <b>Custom Color</b><input type="text" class="paramEntry" id="ringCustomColor" value="#FF0000"><br/>
                 </div>
 
-                <h3>Speed Display</h3>
+                <h3>Speed/Heading Display</h3>
                 <div class="paramRow">
                     <b>Show Chase Car Speed:</b> <input type="checkbox" class="paramSelector" id="showCarSpeed">
                 </div>
+                <div class="paramRow">
+                    <b>Show Chase Car Heading:</b> <input type="checkbox" class="paramSelector" id="showCarHeading">
+                </div>
 
 
             </div>