diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 6f0b6c90e450e..c921220d083df 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -114,18 +114,16 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @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 + // @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-Heading // @RebootRequired: True // @User: Advanced AP_GROUPINFO("_TYPE", 0, AP_GPS, _type[0], HAL_GPS_TYPE_DEFAULT), #if GPS_MAX_RECEIVERS > 1 // @Param: _TYPE2 + // @CopyFieldsFrom: _TYPE // @DisplayName: 2nd GPS type // @Description: GPS type of 2nd 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 - // @RebootRequired: True - // @User: Advanced AP_GROUPINFO("_TYPE2", 1, AP_GPS, _type[1], 0), #endif @@ -631,6 +629,7 @@ void AP_GPS::send_blob_start(uint8_t instance) switch (_type[instance]) { #if AP_GPS_SBF_ENABLED case GPS_TYPE_SBF: + case GPS_TYPE_SBF_DUAL_ANTENNA: #endif //AP_GPS_SBF_ENABLED #if AP_GPS_GSOF_ENABLED case GPS_TYPE_GSOF: @@ -780,6 +779,7 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance) #if AP_GPS_SBF_ENABLED // by default the sbf/trimble gps outputs no data on its port, until configured. case GPS_TYPE_SBF: + case GPS_TYPE_SBF_DUAL_ANTENNA: return new AP_GPS_SBF(*this, state[instance], _port[instance]); #endif //AP_GPS_SBF_ENABLED #if AP_GPS_GSOF_ENABLED diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 9cdca1326b737..d2a496d72f1fe 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -134,6 +134,7 @@ class AP_GPS #if HAL_SIM_GPS_ENABLED GPS_TYPE_SITL = 100, #endif + GPS_TYPE_SBF_DUAL_ANTENNA = 26, }; /// GPS status codes. These are kept aligned with MAVLink by diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 4da9e4e41866c..0bcf2e6ef6ed0 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -71,6 +71,10 @@ AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state, if (option_set(AP_GPS::DriverOptions::SBF_UseBaseForYaw)) { state.gps_yaw_configured = true; } + + if (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA) { + state.gps_yaw_configured = true; + } } AP_GPS_SBF::~AP_GPS_SBF (void) { @@ -92,9 +96,9 @@ AP_GPS_SBF::read(void) ret |= parse(temp); } + const uint32_t now = AP_HAL::millis(); if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE) { if (config_step != Config_State::Complete) { - uint32_t now = AP_HAL::millis(); if (now > _init_blob_time) { if (now > _config_last_ack_time + 2000) { const size_t port_enable_len = strlen(_port_enable); @@ -116,9 +120,20 @@ AP_GPS_SBF::read(void) } break; case Config_State::SSO: - if (asprintf(&config_string, "sso,Stream%d,COM%d,PVTGeodetic+DOP+ReceiverStatus+VelCovGeodetic+BaseVectorGeod,msec100\n", + const char *extra_config; + switch (get_type()) { + case AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA: + extra_config = "+AttEuler+AttCovEuler"; + break; + case AP_GPS::GPS_Type::GPS_TYPE_SBF: + 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, - (int)gps._com_port[state.instance]) == -1) { + (int)gps._com_port[state.instance], + extra_config) == -1) { config_string = nullptr; } break; @@ -145,6 +160,23 @@ 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"; + } + // We could set the UseBaseForYaw option to set the attitude here, and the attitude offset + // which would generate an AttEuler message for us, and wouldn't require any extra handling + // but by doing it in ArduPilot we can apply our constraints on baseline distances + // if (driver_options() & DriverOptions::SBF_UseBaseForYaw) { + // targetGA = "MovingBase"; + // } + if (asprintf(&config_string, "sga, %s\n", targetGA)) { + config_string = nullptr; + } + break; + } 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); @@ -168,7 +200,6 @@ AP_GPS_SBF::read(void) } else if (_has_been_armed && (RxState & SBF_DISK_MOUNTED)) { // since init is done at this point and unmounting should be rate limited, // take over the _init_blob_time variable - uint32_t now = AP_HAL::millis(); if (now > _init_blob_time) { unmount_disk(); _init_blob_time = now + 1000; @@ -177,6 +208,11 @@ AP_GPS_SBF::read(void) } } + if ((now - state.gps_yaw_time_ms) > 300) { + state.have_gps_yaw = false; + state.have_gps_yaw_accuracy = false; + } + return ret; } @@ -345,9 +381,12 @@ AP_GPS_SBF::parse(uint8_t temp) _init_blob_index++; if ((gps._sbas_mode == AP_GPS::SBAS_Mode::Disabled) ||_init_blob_index >= ARRAY_SIZE(sbas_on_blob)) { - config_step = Config_State::Complete; + config_step = Config_State::SGA; } 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); @@ -500,6 +539,56 @@ AP_GPS_SBF::process_message(void) } break; } + case AttEuler: + { + if (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA) { + const msg5938 &temp = sbf_msg.data.msg5938u; + + check_new_itow(temp.TOW, sbf_msg.length); + + constexpr double floatDNU = -2e-10f; + constexpr uint8_t errorBits = 0x8F; // Bits 0-1 are aux 1 baseline + // Bits 2-3 are aux 2 baseline + // Bit 7 is attitude not requested +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" // suppress -Wfloat-equal as it's false positive when testing for DNU values + if (((temp.Error & errorBits) == 0) + && (temp.Mode > 0) + && (temp.Heading != floatDNU)) { +#pragma GCC diagnostic pop + state.gps_yaw = temp.Heading; + state.gps_yaw_time_ms = AP_HAL::millis(); + state.have_gps_yaw = true; + } else { + state.gps_yaw = false; + } + } + break; + } + case AttEulerCov: + { + if (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA) { + const msg5939 &temp = sbf_msg.data.msg5939u; + + check_new_itow(temp.TOW, sbf_msg.length); + + constexpr double floatDNU = -2e-10f; + constexpr uint8_t errorBits = 0x8F; // Bits 0-1 are aux 1 baseline + // Bits 2-3 are aux 2 baseline + // Bit 7 is attitude not requested +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" // suppress -Wfloat-equal as it's false positive when testing for DNU values + if (((temp.Error & errorBits) == 0) + && (temp.Cov_HeadHead != floatDNU)) { +#pragma GCC diagnostic pop + state.gps_yaw_accuracy = sqrtf(temp.Cov_HeadHead); + state.have_gps_yaw_accuracy = true; + } else { + state.gps_yaw_accuracy = false; + } + } + break; + } case BaseVectorGeod: { #pragma GCC diagnostic push diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h index ac4944a9ccb8a..d5a5ce1f8021f 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.h +++ b/libraries/AP_GPS/AP_GPS_SBF.h @@ -73,6 +73,7 @@ class AP_GPS_SBF : public AP_GPS_Backend SSO, Blob, SBAS, + SGA, Complete }; Config_State config_step; @@ -109,7 +110,9 @@ class AP_GPS_SBF : public AP_GPS_Backend PVTGeodetic = 4007, ReceiverStatus = 4014, BaseVectorGeod = 4028, - VelCovGeodetic = 5908 + VelCovGeodetic = 5908, + AttEuler = 5938, + AttEulerCov = 5939, }; struct PACKED msg4007 // PVTGeodetic @@ -217,12 +220,44 @@ class AP_GPS_SBF : public AP_GPS_Backend float Cov_VuDt; }; + struct PACKED msg5938 // AttEuler + { + uint32_t TOW; + uint16_t WNc; + uint8_t NrSV; + uint8_t Error; + uint16_t Mode; + uint16_t Reserved; + float Heading; + float Pitch; + float Roll; + float PitchDot; + float RollDot; + float HeadingDot; + }; + + struct PACKED msg5939 + { + uint32_t TOW; + uint16_t WNc; + uint8_t Reserved; + uint8_t Error; + float Cov_HeadHead; + float Cov_PitchPitch; + float Cov_RollRoll; + float Cov_HeadPitch; + float Cov_HeadRoll; + float Cov_PitchRoll; + }; + union PACKED msgbuffer { msg4007 msg4007u; msg4001 msg4001u; msg4014 msg4014u; msg4028 msg4028u; msg5908 msg5908u; + msg5938 msg5938u; + msg5939 msg5939u; uint8_t bytes[256]; };