From 2e75b367caecd8c4c38066e1db81da6f413947d9 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Fri, 5 Nov 2021 13:00:51 -0700 Subject: [PATCH] AP_GPS: Support integrated headings from SBF --- libraries/AP_GPS/AP_GPS.cpp | 8 +-- libraries/AP_GPS/AP_GPS.h | 1 + libraries/AP_GPS/AP_GPS_SBF.cpp | 93 +++++++++++++++++++++++++++++++-- libraries/AP_GPS/AP_GPS_SBF.h | 37 ++++++++++++- 4 files changed, 129 insertions(+), 10 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 0b984bf1df709..25b824b4b6d25 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -129,18 +129,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: GPS_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 @@ -645,6 +643,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: @@ -806,6 +805,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_NOVA_ENABLED diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 4b184dcde0a82..eedc1a0c5a43a 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -131,6 +131,7 @@ class AP_GPS GPS_TYPE_UAVCAN_RTK_ROVER = 23, GPS_TYPE_UNICORE_NMEA = 24, GPS_TYPE_UNICORE_MOVINGBASE_NMEA = 25, + GPS_TYPE_SBF_DUAL_ANTENNA = 26, #if HAL_SIM_GPS_ENABLED GPS_TYPE_SITL = 100, #endif diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 3b1d8d70095c9..6701b84b0e53d 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,17 @@ 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: // 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 +194,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 +202,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 +375,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 +533,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 2d8d47baafc50..407745d50c6e0 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.h +++ b/libraries/AP_GPS/AP_GPS_SBF.h @@ -75,6 +75,7 @@ class AP_GPS_SBF : public AP_GPS_Backend SSO, Blob, SBAS, + SGA, Complete }; Config_State config_step; @@ -111,7 +112,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 @@ -219,12 +222,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]; };