diff --git a/chasemapper/__init__.py b/chasemapper/__init__.py index 4226042..6bdf969 100644 --- a/chasemapper/__init__.py +++ b/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" diff --git a/chasemapper/geometry.py b/chasemapper/geometry.py index 3291c40..8b3a5ae 100644 --- a/chasemapper/geometry.py +++ b/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( - (_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 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 diff --git a/chasemapper/gps.py b/chasemapper/gps.py index 2f273f7..3e36932 100644 --- a/chasemapper/gps.py +++ b/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: diff --git a/horusmapper.py b/horusmapper.py index 5cf72f6..996daef 100644 --- a/horusmapper.py +++ b/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 - # (Which will be the case once I end up building a better car GPS...) + # Add in true heading data if we have been supplied it (e.g. from a uBlox NEO-M8U device) 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, }, ) diff --git a/static/js/balloon.js b/static/js/balloon.js index 0db00d5..81fc5e9 100644 --- a/static/js/balloon.js +++ b/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); diff --git a/templates/index.html b/templates/index.html index ffcb1e3..fe604f7 100644 --- a/templates/index.html +++ b/templates/index.html @@ -382,6 +382,20 @@ } }) .addTo(map); + // Chase Car Heading Display + L.control.custom({ + position: 'bottomleft', + content : "
", + 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 @@ Custom Color
-

Speed Display

+

Speed/Heading Display

Show Chase Car Speed:
+
+ Show Chase Car Heading: +