From 64ed76326b7af9ab065be0fa992f398f3f3976c7 Mon Sep 17 00:00:00 2001 From: Grant Morphett Date: Sun, 28 May 2017 14:11:31 +1000 Subject: [PATCH] AP_GPS: Adding support for the NMEA GPHDT sentence The NMEA GPHDT sentence can be used to determine the vehicles bearing instead of a compass even when the vehicle is stationary. This type of GPS is normally very expensive and does the bearing using some sort of phase ambituity algorithm. --- libraries/AP_GPS/AP_GPS.cpp | 13 +++++++++---- libraries/AP_GPS/AP_GPS.h | 30 +++++++++++++++++++++++++++++- libraries/AP_GPS/AP_GPS_NMEA.cpp | 20 +++++++++++++++++++- libraries/AP_GPS/AP_GPS_NMEA.h | 11 +++++++++++ 4 files changed, 68 insertions(+), 6 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 476c4e9628..179057ad47 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -72,7 +72,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Param: TYPE // @DisplayName: GPS type // @Description: GPS type - // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA + // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA // @RebootRequired: True // @User: Advanced AP_GROUPINFO("TYPE", 0, AP_GPS, _type[0], HAL_GPS_TYPE_DEFAULT), @@ -80,7 +80,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Param: TYPE2 // @DisplayName: 2nd GPS type // @Description: GPS type of 2nd GPS - // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA + // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA // @RebootRequired: True // @User: Advanced AP_GROUPINFO("TYPE2", 1, AP_GPS, _type[1], 0), @@ -488,7 +488,11 @@ void AP_GPS::detect_instance(uint8_t instance) dstate->last_baud_change_ms = now; if (_auto_config == GPS_AUTO_CONFIG_ENABLE && new_gps == nullptr) { - send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob)); + if (_type[instance] == GPS_TYPE_HEMI) { + send_blob_start(instance, AP_GPS_NMEA_HEMISPHERE_INIT_STRING, strlen(AP_GPS_NMEA_HEMISPHERE_INIT_STRING)); + } else { + send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob)); + } } } @@ -540,7 +544,8 @@ void AP_GPS::detect_instance(uint8_t instance) else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_ERB) && AP_GPS_ERB::_detect(dstate->erb_detect_state, data)) { new_gps = new AP_GPS_ERB(*this, state[instance], _port[instance]); - } else if (_type[instance] == GPS_TYPE_NMEA && + } else if ((_type[instance] == GPS_TYPE_NMEA || + _type[instance] == GPS_TYPE_HEMI) && AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) { new_gps = new AP_GPS_NMEA(*this, state[instance], _port[instance]); } diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 9af6b368e5..dcf662887a 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -84,7 +84,8 @@ public: GPS_TYPE_GSOF = 11, GPS_TYPE_ERB = 13, GPS_TYPE_MAV = 14, - GPS_TYPE_NOVA = 15 + GPS_TYPE_NOVA = 15, + GPS_TYPE_HEMI = 16, // hemisphere NMEA }; /// GPS status codes @@ -130,6 +131,7 @@ public: Location location; ///< last fix location float ground_speed; ///< ground speed in m/sec float ground_course; ///< ground course in degrees + float gps_yaw; ///< GPS derived yaw information, if available (degrees) uint16_t hdop; ///< horizontal dilution of precision in cm uint16_t vdop; ///< vertical dilution of precision in cm uint8_t num_sats; ///< Number of visible satellites @@ -141,6 +143,7 @@ public: bool have_speed_accuracy; ///< does GPS give speed accuracy? Set to true only once available. bool have_horizontal_accuracy; ///< does GPS give horizontal position accuracy? Set to true only once available. bool have_vertical_accuracy; ///< does GPS give vertical position accuracy? Set to true only once available. + bool have_gps_yaw; ///< does GPS give yaw? Set to true only once available. uint32_t last_gps_time_ms; ///< the system time we got the last GPS timestamp, milliseconds uint32_t uart_timestamp_ms; ///< optional timestamp from set_uart_timestamp() @@ -255,6 +258,23 @@ public: return ground_course_cd(primary_instance); } + // yaw in degrees if available + bool gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg) const { + if (!have_gps_yaw(instance)) { + return false; + } + yaw_deg = state[instance].gps_yaw; + // None of the GPS backends can provide this yet, so we hard + // code a fixed value of 10 degrees, which seems like a + // reasonable guess. Once a backend can provide a proper + // estimate we can implement it + accuracy_deg = 10; + return true; + } + bool gps_yaw_deg(float &yaw_deg, float &accuracy_deg) const { + return gps_yaw_deg(primary_instance, yaw_deg, accuracy_deg); + } + // number of locked satellites uint8_t num_sats(uint8_t instance) const { return state[instance].num_sats; @@ -329,6 +349,14 @@ public: return have_vertical_velocity(primary_instance); } + // return true if the GPS supports yaw + bool have_gps_yaw(uint8_t instance) const { + return state[instance].have_gps_yaw; + } + bool have_gps_yaw(void) const { + return have_gps_yaw(primary_instance); + } + // the expected lag (in seconds) in the position and velocity readings from the gps // return true if the GPS hardware configuration is known or the lag parameter has been set manually bool get_lag(uint8_t instance, float &lag_sec) const; diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 1ec6120f1d..c4fcdde8fb 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -223,6 +223,12 @@ bool AP_GPS_NMEA::_have_new_message() if (_last_VTG_ms != 0) { _last_VTG_ms = 1; } + + if (now - _last_HDT_ms > 300) { + // we have lost GPS yaw + state.have_gps_yaw = false; + } + _last_GGA_ms = 1; _last_RMC_ms = 1; return true; @@ -293,6 +299,11 @@ bool AP_GPS_NMEA::_term_complete() fill_3d_velocity(); // VTG has no fix indicator, can't change fix status break; + case _GPS_SENTENCE_HDT: + _last_HDT_ms = now; + state.gps_yaw = wrap_360(_new_gps_yaw*0.01f); + state.have_gps_yaw = true; + break; } } else { switch (_sentence_type) { @@ -327,6 +338,10 @@ bool AP_GPS_NMEA::_term_complete() _sentence_type = _GPS_SENTENCE_RMC; } else if (strcmp(term_type, "GGA") == 0) { _sentence_type = _GPS_SENTENCE_GGA; + } else if (strcmp(term_type, "HDT") == 0) { + _sentence_type = _GPS_SENTENCE_HDT; + // HDT doesn't have a data qualifier + _gps_data_good = true; } else if (strcmp(term_type, "VTG") == 0) { _sentence_type = _GPS_SENTENCE_VTG; // VTG may not contain a data qualifier, presume the solution is good @@ -338,7 +353,7 @@ bool AP_GPS_NMEA::_term_complete() return false; } - // 32 = RMC, 64 = GGA, 96 = VTG + // 32 = RMC, 64 = GGA, 96 = VTG, 128 = HDT if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) { switch (_sentence_type + _term_number) { // operational status @@ -400,6 +415,9 @@ bool AP_GPS_NMEA::_term_complete() case _GPS_SENTENCE_VTG + 5: // Speed (VTG) _new_speed = (_parse_decimal_100(_term) * 514) / 1000; // knots-> m/sec, approximiates * 0.514 break; + case _GPS_SENTENCE_HDT + 1: // Course (HDT) + _new_gps_yaw = _parse_decimal_100(_term); + break; case _GPS_SENTENCE_RMC + 8: // Course (GPRMC) case _GPS_SENTENCE_VTG + 1: // Course (VTG) _new_course = _parse_decimal_100(_term); diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h index a7f982902e..a7725950a3 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.h +++ b/libraries/AP_GPS/AP_GPS_NMEA.h @@ -71,6 +71,7 @@ private: _GPS_SENTENCE_RMC = 32, _GPS_SENTENCE_GGA = 64, _GPS_SENTENCE_VTG = 96, + _GPS_SENTENCE_HDT = 128, _GPS_SENTENCE_OTHER = 0 }; @@ -139,6 +140,7 @@ private: int32_t _new_altitude; ///< altitude parsed from a term int32_t _new_speed; ///< speed parsed from a term int32_t _new_course; ///< course parsed from a term + float _new_gps_yaw; ///< yaw parsed from a term uint16_t _new_hdop; ///< HDOP parsed from a term uint8_t _new_satellite_count; ///< satellite count parsed from a term uint8_t _new_quality_indicator; ///< GPS quality indicator parsed from a term @@ -146,6 +148,7 @@ private: uint32_t _last_RMC_ms = 0; uint32_t _last_GGA_ms = 0; uint32_t _last_VTG_ms = 0; + uint32_t _last_HDT_ms = 0; /// @name Init strings /// In ::init, an attempt is made to configure the GPS @@ -159,3 +162,11 @@ private: static const char _initialisation_blob[]; }; + +#define AP_GPS_NMEA_HEMISPHERE_INIT_STRING \ + "$JATT,NMEAHE,0\r\n" /* Prefix of GP on the HDT message */ \ + "$JASC,GPGGA,5\r\n" /* GGA at 5Hz */ \ + "$JASC,GPRMC,5\r\n" /* RMC at 5Hz */ \ + "$JASC,GPVTG,5\r\n" /* VTG at 5Hz */ \ + "$JASC,GPHDT,5\r\n" /* HDT at 5Hz */ \ + "$JMODE,SBASR,YES\r\n" /* Enable SBAS */