diff --git a/libraries/AP_GPS/AP_GPS_ERB.h b/libraries/AP_GPS/AP_GPS_ERB.h index 69fde51d69..449b2e4b1a 100644 --- a/libraries/AP_GPS/AP_GPS_ERB.h +++ b/libraries/AP_GPS/AP_GPS_ERB.h @@ -35,7 +35,7 @@ public: AP_GPS::GPS_Status highest_supported_status(void) override { return AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; } - bool supports_mavlink_gps_rtk_message() override { return true; } + bool supports_mavlink_gps_rtk_message() const override { return true; } static bool _detect(struct ERB_detect_state &state, uint8_t data); diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 6eb4990d4d..5a620c4917 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -62,6 +62,9 @@ AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state, port->write((const uint8_t*)_port_enable, strlen(_port_enable)); _config_last_ack_time = AP_HAL::millis(); + + // if we ever parse RTK observations it will always be of type NED, so set it once + state.rtk_baseline_coords_type = RTK_BASELINE_COORDINATE_SYSTEM_NED; } AP_GPS_SBF::~AP_GPS_SBF (void) { @@ -411,6 +414,37 @@ AP_GPS_SBF::process_message(void) } break; } + case BaseVectorGeod: + { +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" // suppress -Wfloat-equal as it's false positive when testing for DNU values + const msg4028 &temp = sbf_msg.data.msg4028u; + + // just breakout any consts we need for Do Not Use (DNU) reasons + constexpr double doubleDNU = -2e-10; + + check_new_itow(temp.TOW, sbf_msg.length); + + if (temp.N == 0) { // no sub blocks so just bail, we can't do anything useful here + state.rtk_num_sats = 0; + state.rtk_age_ms = 0; + state.rtk_baseline_y_mm = 0; + state.rtk_baseline_x_mm = 0; + state.rtk_baseline_z_mm = 0; + break; + } + + state.rtk_num_sats = temp.info.NrSV; + + state.rtk_age_ms = (temp.info.CorrAge != 65535) ? ((uint32_t)temp.info.CorrAge) * 10 : 0; + + // copy the position as long as the data isn't DNU + state.rtk_baseline_y_mm = (temp.info.DeltaEast != doubleDNU) ? temp.info.DeltaEast * 1e3 : 0; + state.rtk_baseline_x_mm = (temp.info.DeltaNorth != doubleDNU) ? temp.info.DeltaNorth * 1e3 : 0; + state.rtk_baseline_z_mm = (temp.info.DeltaUp != doubleDNU) ? temp.info.DeltaUp * -1e3 : 0; +#pragma GCC diagnostic pop + break; + } } return false; diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h index 50f935e2ad..f52a3d4713 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.h +++ b/libraries/AP_GPS/AP_GPS_SBF.h @@ -43,6 +43,8 @@ public: void broadcast_configuration_failure_reason(void) const override; + bool supports_mavlink_gps_rtk_message(void) const override { return true; }; + // get the velocity lag, returns true if the driver is confident in the returned value bool get_lag(float &lag_sec) const override { lag_sec = 0.08f; return true; } ; @@ -65,7 +67,7 @@ private: uint8_t _init_blob_index; uint32_t _init_blob_time; char *_initial_sso; - const char* _sso_normal = ", PVTGeodetic+DOP+ReceiverStatus+VelCovGeodetic, msec100\n"; + const char* _sso_normal = ", PVTGeodetic+DOP+ReceiverStatus+VelCovGeodetic+BaseVectorGeod, msec100\n"; const char* _initialisation_blob[4] = { "srd, Moderate, UAV\n", "sem, PVT, 5\n", @@ -87,6 +89,7 @@ private: DOP = 4001, PVTGeodetic = 4007, ReceiverStatus = 4014, + BaseVectorGeod = 4028, VelCovGeodetic = 5908 }; @@ -123,7 +126,7 @@ private: uint16_t VAccuracy; uint8_t Misc; }; - + struct PACKED msg4001 // DOP { uint32_t TOW; @@ -150,6 +153,33 @@ private: // remaining data is AGCData, which we don't have a use for, don't extract the data }; + struct PACKED VectorInfoGeod { + uint8_t NrSV; + uint8_t Error; + uint8_t Mode; + uint8_t Misc; + double DeltaEast; + double DeltaNorth; + double DeltaUp; + float DeltaVe; + float DeltaVn; + float DeltaVu; + uint16_t Azimuth; + int16_t Elevation; + uint8_t ReferenceID; + uint16_t CorrAge; + uint32_t SignalInfo; + }; + + struct PACKED msg4028 // BaseVectorGeod + { + uint32_t TOW; + uint16_t WNc; + uint8_t N; // number of baselines + uint8_t SBLength; + VectorInfoGeod info; // there can be multiple baselines here, but we will only consume the first one, so don't worry about anything after + }; + struct PACKED msg5908 // VelCovGeodetic { uint32_t TOW; @@ -172,6 +202,7 @@ private: msg4007 msg4007u; msg4001 msg4001u; msg4014 msg4014u; + msg4028 msg4028u; msg5908 msg5908u; uint8_t bytes[256]; }; diff --git a/libraries/AP_GPS/AP_GPS_SBP.h b/libraries/AP_GPS/AP_GPS_SBP.h index 6384c92cd8..1c37b18d4c 100644 --- a/libraries/AP_GPS/AP_GPS_SBP.h +++ b/libraries/AP_GPS/AP_GPS_SBP.h @@ -31,7 +31,7 @@ public: AP_GPS::GPS_Status highest_supported_status(void) override { return AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; } - bool supports_mavlink_gps_rtk_message() override { return true; } + bool supports_mavlink_gps_rtk_message() const override { return true; } // Methods bool read() override; diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index edf2534c5d..2c3812f1e3 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -45,7 +45,7 @@ public: virtual void inject_data(const uint8_t *data, uint16_t len); //MAVLink methods - virtual bool supports_mavlink_gps_rtk_message() { return false; } + virtual bool supports_mavlink_gps_rtk_message() const { return false; } virtual void send_mavlink_gps_rtk(mavlink_channel_t chan); virtual void broadcast_configuration_failure_reason(void) const { return ; }