diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 73829001c8771d..eae2b2488da2b1 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -544,6 +544,8 @@ void AP_GPS::send_blob_start(uint8_t instance) #if AP_GPS_SBF_ENABLED case GPS_TYPE_SBF: case GPS_TYPE_SBF_DUAL_ANTENNA: + case GPS_TYPE_SBF_RTK_BASE: + case GPS_TYPE_SBF_RTK_ROVER: #endif //AP_GPS_SBF_ENABLED #if AP_GPS_GSOF_ENABLED case GPS_TYPE_GSOF: @@ -712,6 +714,8 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance) // by default the sbf/trimble gps outputs no data on its port, until configured. case GPS_TYPE_SBF: case GPS_TYPE_SBF_DUAL_ANTENNA: + case GPS_TYPE_SBF_RTK_BASE: + case GPS_TYPE_SBF_RTK_ROVER: return new AP_GPS_SBF(*this, params[instance], state[instance], _port[instance]); #endif //AP_GPS_SBF_ENABLED #if AP_GPS_NOVA_ENABLED @@ -923,21 +927,8 @@ void AP_GPS::update_instance(uint8_t instance) } #if GPS_MAX_RECEIVERS > 1 - if (drivers[instance] && type == GPS_TYPE_UBLOX_RTK_BASE) { - // see if a moving baseline base has some RTCMv3 data - // which we need to pass along to the rover - const uint8_t *rtcm_data; - uint16_t rtcm_len; - if (drivers[instance]->get_RTCMV3(rtcm_data, rtcm_len)) { - for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) { - if (i != instance && params[i].type == GPS_TYPE_UBLOX_RTK_ROVER) { - // pass the data to the rover - inject_data(i, rtcm_data, rtcm_len); - drivers[instance]->clear_RTCMV3(); - break; - } - } - } + if (provides_moving_base_corrections(instance)) { + forward_moving_base_corrections(instance); } #endif @@ -978,6 +969,42 @@ void AP_GPS::update_instance(uint8_t instance) #endif } +/* + check whether receiver managed by driver `instance` is expected to provide RTCM corrections +*/ +bool AP_GPS::provides_moving_base_corrections(uint8_t instance) const +{ + const auto driver_type = params[instance].type; + return driver_type == GPS_TYPE_UBLOX_RTK_BASE || driver_type == GPS_TYPE_SBF_RTK_BASE; +} + +/* + check whether receiver managed by driver `instance` accepts moving base RTCM corrections +*/ +bool AP_GPS::accepts_moving_base_corrections(uint8_t instance) const +{ + const auto driver_type = params[instance].type; + return driver_type == GPS_TYPE_UBLOX_RTK_ROVER || driver_type == GPS_TYPE_SBF_RTK_ROVER; +} + +/* + check if driver `instance` has any moving base RTCMv3 corrections and if so, forward them to the other instances if they accept them +*/ +void AP_GPS::forward_moving_base_corrections(uint8_t instance) +{ + const uint8_t *rtcm_data; + uint16_t rtcm_len; + if (drivers[instance] && drivers[instance]->get_RTCMV3(rtcm_data, rtcm_len)) { + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (i != instance && accepts_moving_base_corrections(i)) { + // pass the data to the rover + inject_data(i, rtcm_data, rtcm_len); + drivers[instance]->clear_RTCMV3(); + break; + } + } + } +} #if GPS_MOVING_BASELINE bool AP_GPS::get_RelPosHeading(uint32_t ×tamp, float &relPosHeading, float &relPosLength, float &relPosD, float &accHeading) @@ -1998,7 +2025,7 @@ bool AP_GPS::gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg, // @Param: _TYPE // @DisplayName: 1st GPS type // @Description: GPS type of 1st GPS - // @Values: 0:None,1:AUTO,2:uBlox,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:DroneCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:DroneCAN-MovingBaseline-Base,23:DroneCAN-MovingBaseline-Rover,24:UnicoreNMEA,25:UnicoreMovingBaselineNMEA,26:SBF-DualAntenna + // @Values: 0:None,1:AUTO,2:uBlox,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:DroneCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:DroneCAN-MovingBaseline-Base,23:DroneCAN-MovingBaseline-Rover,24:UnicoreNMEA,25:UnicoreMovingBaselineNMEA,26:SBF-DualAntenna,27:SBF-MovingBaseline-Base,28,SBF-MovingBaseline-Rover // @RebootRequired: True // @User: Advanced diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index e21bf665246316..6c89e1a7c7146d 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -111,6 +111,8 @@ class AP_GPS GPS_TYPE_UNICORE_NMEA = 24, GPS_TYPE_UNICORE_MOVINGBASE_NMEA = 25, GPS_TYPE_SBF_DUAL_ANTENNA = 26, + GPS_TYPE_SBF_RTK_BASE = 27, + GPS_TYPE_SBF_RTK_ROVER = 28, #if HAL_SIM_GPS_ENABLED GPS_TYPE_SITL = 100, #endif @@ -722,6 +724,15 @@ class AP_GPS void update_instance(uint8_t instance); + // check whether receiver managed by driver `instance` is expected to provide RTCM corrections + bool provides_moving_base_corrections(uint8_t instance) const; + + // check whether receiver managed by driver `instance` accepts moving base RTCM corrections + bool accepts_moving_base_corrections(uint8_t instace) const; + + // check if driver `instance` has any moving base RTCMv3 corrections and if so, forward them to the other instances if they accept them + void forward_moving_base_corrections(uint8_t instance); + /* buffer for re-assembling RTCM data for GPS injection. The 8 bit flags field in GPS_RTCM_DATA is interpreted as: diff --git a/libraries/AP_GPS/AP_GPS_Params.cpp b/libraries/AP_GPS/AP_GPS_Params.cpp index f7fc29ead97478..5212ca0d9e51ef 100644 --- a/libraries/AP_GPS/AP_GPS_Params.cpp +++ b/libraries/AP_GPS/AP_GPS_Params.cpp @@ -25,7 +25,7 @@ const AP_Param::GroupInfo AP_GPS::Params::var_info[] = { // @Param: TYPE // @DisplayName: GPS type // @Description: GPS type - // @Values: 0:None,1:AUTO,2:uBlox,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:DroneCAN,10:SBF,11:GSOF,13:ERB,14:MAVLink,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:DroneCAN-MovingBaseline-Base,23:DroneCAN-MovingBaseline-Rover,24:UnicoreNMEA,25:UnicoreMovingBaselineNMEA,26:SBF-DualAntenna + // @Values: 0:None,1:AUTO,2:uBlox,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:DroneCAN,10:SBF,11:GSOF,13:ERB,14:MAVLink,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:DroneCAN-MovingBaseline-Base,23:DroneCAN-MovingBaseline-Rover,24:UnicoreNMEA,25:UnicoreMovingBaselineNMEA,26:SBF-DualAntenna,27:SBF-MovingBaseline-Base,28:SBF-MovingBaseline-Rover // @RebootRequired: True // @User: Advanced AP_GROUPINFO_FLAGS("TYPE", 1, AP_GPS::Params, type, 0, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index e5a102998bf96f..0b35b33874fc4c 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -71,15 +71,39 @@ AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, // 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; - // yaw available when option bit set or using dual antenna - if (option_set(AP_GPS::DriverOptions::SBF_UseBaseForYaw) || - (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA)) { + // yaw available when option bit set or using moving base/dual-antenna + if (option_set(AP_GPS::DriverOptions::SBF_UseBaseForYaw)) { state.gps_yaw_configured = true; } + + switch (get_type()) { +#if GPS_MOVING_BASELINE + case AP_GPS::GPS_Type::GPS_TYPE_SBF_RTK_BASE: { + rtcm3_parser = new RTCM3_Parser; + if (!rtcm3_parser) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "SBF %d: failed RTCMv3 parser allocation", state.instance + 1); + } + break; + } +#endif // GPS_MOVING_BASELINE + case AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA: + case AP_GPS::GPS_Type::GPS_TYPE_SBF_RTK_ROVER: { + state.gps_yaw_configured = true; + break; + } + default: { + break; + } + } } AP_GPS_SBF::~AP_GPS_SBF (void) { free(config_string); +#if GPS_MOVING_BASELINE + if (rtcm3_parser) { + delete rtcm3_parser; + } +#endif // GPS_MOVING_BASELINE } // Process all bytes available from the stream @@ -112,7 +136,7 @@ AP_GPS_SBF::read(void) } else if (readyForCommand) { if (config_string == nullptr) { switch (config_step) { - case Config_State::Baud_Rate: + case Config_State::Baud_Rate: { if (asprintf(&config_string, "scs,COM%d,baud%d,bits8,No,bit1,%s\n", (int)params.com_port, 230400, @@ -120,16 +144,19 @@ AP_GPS_SBF::read(void) config_string = nullptr; } break; - case Config_State::SSO: + } + case Config_State::SSO: { const char *extra_config; switch (get_type()) { case AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA: + case AP_GPS::GPS_Type::GPS_TYPE_SBF_RTK_ROVER: { extra_config = "+AttCovEuler+AuxAntPositions"; break; - case AP_GPS::GPS_Type::GPS_TYPE_SBF: - default: + } + default: { extra_config = ""; break; + } } if (asprintf(&config_string, "sso,Stream%d,COM%d,PVTGeodetic+DOP+ReceiverStatus+VelCovGeodetic+BaseVectorGeod%s,msec100\n", (int)GPS_SBF_STREAM_NUMBER, @@ -137,13 +164,66 @@ AP_GPS_SBF::read(void) extra_config) == -1) { config_string = nullptr; } + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %hhu SBF: SSO finished", state.instance); + break; + } + case Config_State::SDIO: { +#if GPS_MOVING_BASELINE + switch (get_type()) { + case AP_GPS::GPS_Type::GPS_TYPE_SBF_RTK_BASE: { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %hhu SBF: Enabling RTCMv3 output", state.instance); + if (asprintf(&config_string, "sdio,COM%d,Auto,+RTCMv3\n", (int)params.com_port) == -1) { + config_string = nullptr; + } + break; + } + default: { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %hhu SBF: Enabling SBF output", state.instance); + if (asprintf(&config_string, "sdio,COM%d,Auto,+SBF\n", (int)params.com_port) == -1) { + config_string = nullptr; + } + break; + } + } +#else + if (asprintf(&config_string, "sdio,COM%d,Auto,+SBF\n", (int)params.com_port) == -1) { + config_string = nullptr; + } +#endif // GPS_MOVING_BASE + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %hhu SBF: SDIO finished", state.instance); + break; + } + case Config_State::SGA: + { + const char *targetGA; + + switch (get_type()) { + case AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA: { + targetGA = "MultiAntenna"; + break; + } + case AP_GPS::GPS_Type::GPS_TYPE_SBF_RTK_ROVER: { + targetGA = "MovingBase"; + break; + } + default: { + targetGA = "none"; + break; + } + } + if (asprintf(&config_string, "sga, %s\n", targetGA) == -1) { + config_string = nullptr; + } break; - case Config_State::Blob: + } + case Config_State::Blob: { if (asprintf(&config_string, "%s\n", _initialisation_blob[_init_blob_index]) == -1) { config_string = nullptr; } + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SBF: Configured initialization blob"); break; - case Config_State::SBAS: + } + case Config_State::SBAS: { switch ((AP_GPS::SBAS_Mode)gps._sbas_mode) { case AP_GPS::SBAS_Mode::Disabled: if (asprintf(&config_string, "%s\n", sbas_off) == -1) { @@ -161,21 +241,12 @@ AP_GPS_SBF::read(void) break; } break; - case Config_State::SGA: - { - const char *targetGA = "none"; - if (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA) { - targetGA = "MultiAntenna"; - } - if (asprintf(&config_string, "sga, %s\n", targetGA)) { - config_string = nullptr; - } - break; } - case Config_State::Complete: + case Config_State::Complete: { // should never reach here, why search for a config if we have fully configured already INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); break; + } } } @@ -226,6 +297,12 @@ bool AP_GPS_SBF::logging_healthy(void) const bool AP_GPS_SBF::parse(uint8_t temp) { +#if GPS_MOVING_BASELINE + if (rtcm3_parser && rtcm3_parser->read(temp)) { + sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1; + return false; + } +#endif switch (sbf_msg.sbf_state) { default: @@ -315,6 +392,12 @@ AP_GPS_SBF::parse(uint8_t temp) } sbf_msg.read++; if (sbf_msg.read >= (sbf_msg.length - 8)) { +#if GPS_MOVING_BASELINE + if (rtcm3_parser) { + // it's definitely an SBF message, so reset the RTCMv3 parser + rtcm3_parser->reset(); + } +#endif // GPS_MOVING_BASELINE if (sbf_msg.read > sizeof(sbf_msg.data)) { // not interested in these large messages sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1; @@ -339,7 +422,7 @@ AP_GPS_SBF::parse(uint8_t temp) sbf_msg.data.bytes[sbf_msg.read] = temp; } else { // we don't have enough buffer to compare the commands - // most probable cause is that a user injected a longer command then + // most probable cause is that a user injected a longer command than // we have buffer for, or it could be a corruption, either way we // simply ignore the result sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1; @@ -361,12 +444,23 @@ AP_GPS_SBF::parse(uint8_t temp) config_string = nullptr; switch (config_step) { case Config_State::Baud_Rate: + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SBF: Found Baud_Rate"); config_step = Config_State::SSO; break; case Config_State::SSO: + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SBF: Found SSO"); + config_step = Config_State::SGA; + break; + case Config_State::SGA: + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SBF: Found SGA"); + config_step = Config_State::SDIO; + break; + case Config_State::SDIO: + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SBF: Found SDIO"); config_step = Config_State::Blob; break; case Config_State::Blob: + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SBF: Found Blob"); _init_blob_index++; if (_init_blob_index >= ARRAY_SIZE(_initialisation_blob)) { config_step = Config_State::SBAS; @@ -374,15 +468,13 @@ AP_GPS_SBF::parse(uint8_t temp) } break; case Config_State::SBAS: + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SBF: Found SBAS"); _init_blob_index++; if ((gps._sbas_mode == AP_GPS::SBAS_Mode::Disabled) ||_init_blob_index >= ARRAY_SIZE(sbas_on_blob)) { - config_step = Config_State::SGA; + config_step = Config_State::Complete; } break; - case Config_State::SGA: - config_step = Config_State::Complete; - break; case Config_State::Complete: // should never reach here, this implies that we validated a config string when we hadn't sent any INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); @@ -541,7 +633,7 @@ AP_GPS_SBF::process_message(void) { // yaw accuracy is taken from this message even though we actually calculate the yaw ourself (see AuxAntPositions below) // this is OK based on the assumption that the calculation methods are similar and that inaccuracy arises from the sensor readings - if (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA) { + if (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA || get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_RTK_ROVER) { const msg5939 &temp = sbf_msg.data.msg5939u; check_new_itow(temp.TOW, sbf_msg.length); @@ -566,7 +658,7 @@ AP_GPS_SBF::process_message(void) case AuxAntPositions: { #if GPS_MOVING_BASELINE - if (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA) { + if (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA || get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_RTK_ROVER) { // calculate yaw using reported antenna positions in earth-frame // note that this calculation does not correct for the vehicle's roll and pitch meaning it is inaccurate at very high lean angles const msg5942 &temp = sbf_msg.data.msg5942u; @@ -657,7 +749,36 @@ bool AP_GPS_SBF::is_configured (void) const { (config_step == Config_State::Complete)); } +// support for retrieving RTCMv3 data from a moving baseline base +bool AP_GPS_SBF::get_RTCMV3(const uint8_t *&bytes, uint16_t &len) +{ +#if GPS_MOVING_BASELINE + if (rtcm3_parser) { + len = rtcm3_parser->get_len(bytes); + return len > 0; + } +#endif + return false; +} + +// clear previous RTCM3 packet +void AP_GPS_SBF::clear_RTCMV3(void) +{ +#if GPS_MOVING_BASELINE + if (rtcm3_parser) { + rtcm3_parser->clear_packet(); + } +#endif +} + bool AP_GPS_SBF::is_healthy (void) const { +#if GPS_MOVING_BASELINE + if (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_RTK_BASE && rtcm3_parser == nullptr) { + // we haven't initialised RTCMv3 parser + return false; + } +#endif + return (RxError & RX_ERROR_MASK) == 0; } diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h index 8817653f42ed74..ddc8ad498957e4 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.h +++ b/libraries/AP_GPS/AP_GPS_SBF.h @@ -21,6 +21,7 @@ #include "AP_GPS.h" #include "GPS_Backend.h" +#include "RTCM3_Parser.h" #if AP_GPS_SBF_ENABLED @@ -41,6 +42,10 @@ class AP_GPS_SBF : public AP_GPS_Backend const char *name() const override { return "SBF"; } + // support for retrieving RTCMv3 data from a moving baseline base + bool get_RTCMV3(const uint8_t *&bytes, uint16_t &len) override; + void clear_RTCMV3(void) override; + bool is_configured (void) const override; void broadcast_configuration_failure_reason(void) const override; @@ -73,6 +78,7 @@ class AP_GPS_SBF : public AP_GPS_Backend enum class Config_State { Baud_Rate, SSO, + SDIO, Blob, SBAS, SGA, @@ -307,5 +313,9 @@ class AP_GPS_SBF : public AP_GPS_Backend char portIdentifier[5]; uint8_t portLength; bool readyForCommand; + +#if GPS_MOVING_BASELINE + RTCM3_Parser *rtcm3_parser {nullptr}; +#endif // GPS_MOVING_BASELINE }; -#endif +#endif // AP_GPS_SBF_ENABLED