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

AP_GPS: Add NMEA THS message.

Browse Source
This commit is contained in:
Philippe Hamelin 2020-07-07 09:16:22 -04:00 committed by Andrew Tridgell
parent ff072c5215
commit 738d000f86
2 changed files with 18 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -236,7 +236,7 @@ bool AP_GPS_NMEA::_have_new_message()
_last_VTG_ms = 1; _last_VTG_ms = 1;
} }
if (now - _last_HDT_ms > 300) { if (now - _last_HDT_THS_ms > 300) {
// we have lost GPS yaw // we have lost GPS yaw
state.have_gps_yaw = false; state.have_gps_yaw = false;
} }
@ -335,7 +335,8 @@ bool AP_GPS_NMEA::_term_complete()
// VTG has no fix indicator, can't change fix status // VTG has no fix indicator, can't change fix status
break; break;
case _GPS_SENTENCE_HDT: case _GPS_SENTENCE_HDT:
_last_HDT_ms = now; case _GPS_SENTENCE_THS:
_last_HDT_THS_ms = now;
state.gps_yaw = wrap_360(_new_gps_yaw*0.01f); state.gps_yaw = wrap_360(_new_gps_yaw*0.01f);
state.have_gps_yaw = true; state.have_gps_yaw = true;
// remember that we are setup to provide yaw. With // remember that we are setup to provide yaw. With
@ -368,6 +369,10 @@ bool AP_GPS_NMEA::_term_complete()
// Only these sentences give us information about // Only these sentences give us information about
// fix status. // fix status.
state.status = AP_GPS::NO_FIX; state.status = AP_GPS::NO_FIX;
break;
case _GPS_SENTENCE_THS:
state.have_gps_yaw = false;
break;
} }
} }
// see if we got a good message // see if we got a good message
@ -405,6 +410,8 @@ bool AP_GPS_NMEA::_term_complete()
_sentence_type = _GPS_SENTENCE_HDT; _sentence_type = _GPS_SENTENCE_HDT;
// HDT doesn't have a data qualifier // HDT doesn't have a data qualifier
_gps_data_good = true; _gps_data_good = true;
} else if (strcmp(term_type, "THS") == 0) {
_sentence_type = _GPS_SENTENCE_THS;
} else if (strcmp(term_type, "VTG") == 0) { } else if (strcmp(term_type, "VTG") == 0) {
_sentence_type = _GPS_SENTENCE_VTG; _sentence_type = _GPS_SENTENCE_VTG;
// VTG may not contain a data qualifier, presume the solution is good // VTG may not contain a data qualifier, presume the solution is good
@ -416,7 +423,7 @@ bool AP_GPS_NMEA::_term_complete()
return false; return false;
} }
// 32 = RMC, 64 = GGA, 96 = VTG, 128 = HDT // 32 = RMC, 64 = GGA, 96 = VTG, 128 = HDT, 160 = THS
if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) { if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) {
switch (_sentence_type + _term_number) { switch (_sentence_type + _term_number) {
// operational status // operational status
@ -428,6 +435,9 @@ bool AP_GPS_NMEA::_term_complete()
_gps_data_good = _term[0] > '0'; _gps_data_good = _term[0] > '0';
_new_quality_indicator = _term[0] - '0'; _new_quality_indicator = _term[0] - '0';
break; break;
case _GPS_SENTENCE_THS + 2: // validity (THS)
_gps_data_good = _term[0] == 'A';
break;
case _GPS_SENTENCE_VTG + 9: // validity (VTG) (we may not see this field) case _GPS_SENTENCE_VTG + 9: // validity (VTG) (we may not see this field)
_gps_data_good = _term[0] != 'N'; _gps_data_good = _term[0] != 'N';
break; break;
@ -481,6 +491,9 @@ bool AP_GPS_NMEA::_term_complete()
case _GPS_SENTENCE_HDT + 1: // Course (HDT) case _GPS_SENTENCE_HDT + 1: // Course (HDT)
_new_gps_yaw = _parse_decimal_100(_term); _new_gps_yaw = _parse_decimal_100(_term);
break; break;
case _GPS_SENTENCE_THS + 1: // Course (THS)
_new_gps_yaw = _parse_decimal_100(_term);
break;
case _GPS_SENTENCE_RMC + 8: // Course (GPRMC) case _GPS_SENTENCE_RMC + 8: // Course (GPRMC)
case _GPS_SENTENCE_VTG + 1: // Course (VTG) case _GPS_SENTENCE_VTG + 1: // Course (VTG)
_new_course = _parse_decimal_100(_term); _new_course = _parse_decimal_100(_term);

3
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -73,6 +73,7 @@ private:
_GPS_SENTENCE_VTG = 96, _GPS_SENTENCE_VTG = 96,
_GPS_SENTENCE_HDT = 128, _GPS_SENTENCE_HDT = 128,
_GPS_SENTENCE_PHD = 138, // extension for AllyStar GPS modules _GPS_SENTENCE_PHD = 138, // extension for AllyStar GPS modules
_GPS_SENTENCE_THS = 160, // True heading with quality indicator, available on Trimble MB-Two
_GPS_SENTENCE_OTHER = 0 _GPS_SENTENCE_OTHER = 0
}; };
@ -143,7 +144,7 @@ private:
uint32_t _last_RMC_ms; uint32_t _last_RMC_ms;
uint32_t _last_GGA_ms; uint32_t _last_GGA_ms;
uint32_t _last_VTG_ms; uint32_t _last_VTG_ms;
uint32_t _last_HDT_ms; uint32_t _last_HDT_THS_ms;
uint32_t _last_PHD_12_ms; uint32_t _last_PHD_12_ms;
uint32_t _last_PHD_26_ms; uint32_t _last_PHD_26_ms;
uint32_t _last_fix_ms; uint32_t _last_fix_ms;