From db777c56b9451557ba1e92030376dd8080abdc4c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 23 Dec 2019 10:03:58 +1100 Subject: [PATCH] AP_GPS: provide yaw feedback in GPS_RAW_INT and GPS2_RAW allow GPS to display status of GPS yaw for moving baseline and NMEA --- libraries/AP_GPS/AP_GPS.cpp | 28 ++++++++++++++++++++++++++-- libraries/AP_GPS/AP_GPS.h | 15 +++++++++++++-- libraries/AP_GPS/AP_GPS_NMEA.cpp | 5 +++++ libraries/AP_GPS/AP_GPS_UBLOX.cpp | 15 ++++----------- libraries/AP_GPS/AP_GPS_UBLOX.h | 1 - 5 files changed, 48 insertions(+), 16 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 1ee0f236ce..ef30533416 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -1036,6 +1036,28 @@ void AP_GPS::inject_data(uint8_t instance, const uint8_t *data, uint16_t len) } } +/* + get GPS yaw following mavlink GPS_RAW_INT and GPS2_RAW + convention. We return 0 if the GPS is not configured to provide + yaw. We return 65535 for a GPS configured to provide yaw that can't + currently provide it. We return from 1 to 36000 for yaw otherwise + */ +uint16_t AP_GPS::gps_yaw_cdeg(uint8_t instance) const +{ + if (!have_gps_yaw_configured(instance)) { + return 0; + } + float yaw_deg, accuracy_deg; + if (!gps_yaw_deg(instance, yaw_deg, accuracy_deg)) { + return 65535; + } + int yaw_cd = wrap_360_cd(yaw_deg * 100); + if (yaw_cd == 0) { + return 36000; + } + return yaw_cd; +} + void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan) { const Location &loc = location(0); @@ -1061,7 +1083,8 @@ void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan) hacc * 1000, // one-sigma standard deviation in mm vacc * 1000, // one-sigma standard deviation in mm sacc * 1000, // one-sigma standard deviation in mm/s - 0); // TODO one-sigma heading accuracy standard deviation + 0, // TODO one-sigma heading accuracy standard deviation + gps_yaw_cdeg(0)); } #if GPS_MAX_RECEIVERS > 1 @@ -1086,7 +1109,8 @@ void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan) ground_course(1)*100, // 1/100 degrees, num_sats(1), state[1].rtk_num_sats, - state[1].rtk_age_ms); + state[1].rtk_age_ms, + gps_yaw_cdeg(1)); } #endif // GPS_MAX_RECEIVERS diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 8e17467721..e37bdcc81d 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -147,6 +147,7 @@ public: 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) + bool gps_yaw_configured; ///< GPS is configured to provide yaw 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 @@ -367,13 +368,20 @@ public: return have_vertical_velocity(primary_instance); } - // return true if the GPS supports yaw + // return true if the GPS currently has yaw available 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); } + + // return true if the GPS is configured to provide yaw. This will + // be true if we expect the GPS to provide yaw, even if it + // currently is not able to provide yaw + bool have_gps_yaw_configured(uint8_t instance) const { + return state[instance].gps_yaw_configured; + } // 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 @@ -590,7 +598,10 @@ private: /// Update primary instance void update_primary(void); - + + // helper function for mavlink gps yaw + uint16_t gps_yaw_cdeg(uint8_t instance) const; + // Auto configure types enum GPS_AUTO_CONFIG { GPS_AUTO_CONFIG_DISABLE = 0, diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 5e0631ca2c..d61f5aaa10 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -295,6 +295,11 @@ bool AP_GPS_NMEA::_term_complete() _last_HDT_ms = now; state.gps_yaw = wrap_360(_new_gps_yaw*0.01f); state.have_gps_yaw = true; + // remember that we are setup to provide yaw. With + // a NMEA GPS we can only tell if the GPS is + // configured to provide yaw when it first sends a + // HDT sentence. + state.gps_yaw_configured = true; break; } } else { diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index a5b242e11b..c8dd934e44 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -96,11 +96,12 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART if (rtcm3_parser == nullptr) { GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "u-blox %d: failed RTCMv3 parser allocation", state.instance + 1); } - } - if (role == AP_GPS::GPS_ROLE_MB_BASE || - role == AP_GPS::GPS_ROLE_MB_ROVER) { _unconfigured_messages |= CONFIG_RTK_MOVBASE; } + if (role == AP_GPS::GPS_ROLE_MB_ROVER) { + _unconfigured_messages |= CONFIG_RTK_MOVBASE; + state.gps_yaw_configured = true; + } } AP_GPS_UBLOX::~AP_GPS_UBLOX() @@ -1358,17 +1359,9 @@ AP_GPS_UBLOX::_parse_gps(void) state.have_gps_yaw = true; state.gps_yaw_accuracy = _buffer.relposned.accHeading * 1e-5; state.have_gps_yaw_accuracy = true; - if (last_yaw_ms == 0) { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS: got yaw lock"); - } - last_yaw_ms = AP_HAL::millis(); } else { state.have_gps_yaw = false; state.have_gps_yaw_accuracy = false; - if (last_yaw_ms && AP_HAL::millis() - last_yaw_ms > 5000) { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS: lost yaw lock 0x%x", unsigned(_buffer.relposned.flags)); - last_yaw_ms = 0; - } } } break; diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 7e11fa6f24..3d56f422b4 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -769,5 +769,4 @@ private: // RTCM3 parser for when in moving baseline base mode RTCM3_Parser *rtcm3_parser; - uint32_t last_yaw_ms; };