Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: use KSXT for position and alt when available

Browse Source 
this gets velocity and position from the same message for more
consistency
This commit is contained in:
Andrew Tridgell 2022-01-03 11:01:05 +11:00
parent 9d21c0b6c1
commit 135c88d474
2 changed files with 19 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -229,6 +229,11 @@ bool AP_GPS_NMEA::_have_new_message()
state.have_gps_yaw = false; state.have_gps_yaw = false;
} }
if (now - _last_KSXT_pos_ms > 500) {
// we have lost KSXT
_last_KSXT_pos_ms = 0;
}
// special case for fixing low output rate of ALLYSTAR GPS modules // special case for fixing low output rate of ALLYSTAR GPS modules
const int32_t dt_ms = now - _last_fix_ms; const int32_t dt_ms = now - _last_fix_ms;
if (labs(dt_ms - gps._rate_ms[state.instance]) > 50 && if (labs(dt_ms - gps._rate_ms[state.instance]) > 50 &&
@ -266,8 +271,10 @@ bool AP_GPS_NMEA::_term_complete()
_last_RMC_ms = now; _last_RMC_ms = now;
//time = _new_time; //time = _new_time;
//date = _new_date; //date = _new_date;
state.location.lat = _new_latitude; if (_last_KSXT_pos_ms == 0) {
state.location.lng = _new_longitude; state.location.lat = _new_latitude;
state.location.lng = _new_longitude;
}
if (_last_3D_velocity_ms == 0 || if (_last_3D_velocity_ms == 0 ||
now - _last_3D_velocity_ms > 1000) { now - _last_3D_velocity_ms > 1000) {
state.ground_speed = _new_speed*0.01f; state.ground_speed = _new_speed*0.01f;
@ -283,9 +290,11 @@ bool AP_GPS_NMEA::_term_complete()
break; break;
case _GPS_SENTENCE_GGA: case _GPS_SENTENCE_GGA:
_last_GGA_ms = now; _last_GGA_ms = now;
state.location.alt = _new_altitude; if (_last_KSXT_pos_ms == 0) {
state.location.lat = _new_latitude; state.location.alt = _new_altitude;
state.location.lng = _new_longitude; state.location.lat = _new_latitude;
state.location.lng = _new_longitude;
}
state.num_sats = _new_satellite_count; state.num_sats = _new_satellite_count;
state.hdop = _new_hdop; state.hdop = _new_hdop;
switch(_new_quality_indicator) { switch(_new_quality_indicator) {
@ -362,6 +371,10 @@ bool AP_GPS_NMEA::_term_complete()
} }
break; break;
case _GPS_SENTENCE_KSXT: case _GPS_SENTENCE_KSXT:
state.location.lat = _ksxt.fields[2]*1.0e7;
state.location.lng = _ksxt.fields[1]*1.0e7;
state.location.alt = _ksxt.fields[3]*1.0e2;
_last_KSXT_pos_ms = now;
if (_ksxt.fields[9] >= 1) { if (_ksxt.fields[9] >= 1) {
// we have 3D fix // we have 3D fix
constexpr float kmh_to_mps = 1.0 / 3.6; constexpr float kmh_to_mps = 1.0 / 3.6;

1
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -149,6 +149,7 @@ private:
uint32_t _last_vvelocity_ms; uint32_t _last_vvelocity_ms;
uint32_t _last_vaccuracy_ms; uint32_t _last_vaccuracy_ms;
uint32_t _last_3D_velocity_ms; uint32_t _last_3D_velocity_ms;
uint32_t _last_KSXT_pos_ms;
uint32_t _last_fix_ms; uint32_t _last_fix_ms;
/// @name Init strings /// @name Init strings