kopia lustrzana https://github.com/projecthorus/chasemapper
Better support of supplied heading data. Handle identical sequential positions better. Optionally display heading on display
Mark Jessop
rodzic
25f5510b64
commit
e03e6d15e1
|
|
@ -8,4 +8,4 @@
|
|||
|
||||
# Now using Semantic Versioning (https://semver.org/) MAJOR.MINOR.PATCH
|
||||
|
||||
__version__ = "1.3.2"
|
||||
__version__ = "1.3.3"
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
},
|
||||
)
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue