From 7a3f6eba0cf3bc7395abe01b8652a1171820cf8c Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Fri, 5 Nov 2021 13:00:51 -0700 Subject: [PATCH] AP_GPS: SBF supports yaw from dual antennas Co-authored-by: Andrew Tridgell Co-authored-by: Randy Mackay --- libraries/AP_GPS/AP_GPS.cpp | 8 +-- libraries/AP_GPS/AP_GPS.h | 1 + libraries/AP_GPS/AP_GPS_SBF.cpp | 92 ++++++++++++++++++++++++++++++--- libraries/AP_GPS/AP_GPS_SBF.h | 44 +++++++++++++++- 4 files changed, 133 insertions(+), 12 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 0b984bf1df709..b947175b0bf62 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-DualAntenna // @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..569d5b6910ec4 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -68,7 +68,10 @@ AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state, // 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; - if (option_set(AP_GPS::DriverOptions::SBF_UseBaseForYaw)) { + + // 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)) { state.gps_yaw_configured = true; } } @@ -92,9 +95,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 +119,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 = "+AttCovEuler+AuxAntPositions"; + 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 +159,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 +193,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 +201,12 @@ AP_GPS_SBF::read(void) } } + // yaw timeout after 300 milliseconds + 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,51 @@ AP_GPS_SBF::process_message(void) } break; } + case AttEulerCov: + { + // 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) { + 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 AuxAntPositions: + { +#if GPS_MOVING_BASELINE + if (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA) { + // 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; + check_new_itow(temp.TOW, sbf_msg.length); + if (temp.N > 0 && temp.ant1.Error == 0 && temp.ant1.AmbiguityType == 0) { + // valid RTK integer fix + const float rel_heading_deg = degrees(atan2f(temp.ant1.DeltaEast, temp.ant1.DeltaNorth)); + calculate_moving_base_yaw(rel_heading_deg, + Vector3f(temp.ant1.DeltaNorth, temp.ant1.DeltaEast, temp.ant1.DeltaUp).length(), + -temp.ant1.DeltaUp); + } + } +#endif + break; + } case BaseVectorGeod: { #pragma GCC diagnostic push @@ -542,7 +620,7 @@ AP_GPS_SBF::process_message(void) } #endif // GPS_MOVING_BASELINE - } else { + } else if (option_set(AP_GPS::DriverOptions::SBF_UseBaseForYaw)) { state.rtk_baseline_y_mm = 0; state.rtk_baseline_x_mm = 0; state.rtk_baseline_z_mm = 0; diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h index 2d8d47baafc50..e8773cf42cdef 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, + AttEulerCov = 5939, + AuxAntPositions = 5942, }; struct PACKED msg4007 // PVTGeodetic @@ -219,12 +222,51 @@ class AP_GPS_SBF : public AP_GPS_Backend float Cov_VuDt; }; + struct PACKED msg5939 // AttEulerCoV + { + uint32_t TOW; // receiver time stamp, 0.001s + uint16_t WNc; // receiver time stamp, 1 week + uint8_t Reserved; // unused + uint8_t Error; // error code. bit 0-1:antenna 1, bit 2-3:antenna2, bit 7: when att not requested + // 00b:no error, 01b:not enough meausurements, 10b:antennas are on one line, 11b:inconsistent with manual anntena pos info + float Cov_HeadHead; // heading estimate variance + float Cov_PitchPitch; // pitch estimate variance + float Cov_RollRoll; // roll estimate variance + float Cov_HeadPitch; // covariance between Euler angle estimates. Always set to Do-No-Use values + float Cov_HeadRoll; + float Cov_PitchRoll; + }; + + struct PACKED AuxAntPositionSubBlock { + uint8_t NrSV; // total number of satellites tracked by the antenna + uint8_t Error; // aux antenna position error code + uint8_t AmbiguityType; // aux antenna positions obtained with 0: fixed ambiguities, 1: float ambiguities + uint8_t AuxAntID; // aux antenna ID: 1 for the first auxiliary antenna, 2 for the second, etc. + double DeltaEast; // position in East direction (relative to main antenna) + double DeltaNorth; // position in North direction (relative to main antenna) + double DeltaUp; // position in Up direction (relative to main antenna) + double EastVel; // velocity in East direction (relative to main antenna) + double NorthVel; // velocity in North direction (relative to main antenna) + double UpVel; // velocity in Up direction (relative to main antenna) + }; + + struct PACKED msg5942 // AuxAntPositions + { + uint32_t TOW; + uint16_t WNc; + uint8_t N; // number of AuxAntPosition sub-blocks in this AuxAntPositions block + uint8_t SBLength; // length of one sub-block in bytes + AuxAntPositionSubBlock ant1; // first aux antennas position + }; + union PACKED msgbuffer { msg4007 msg4007u; msg4001 msg4001u; msg4014 msg4014u; msg4028 msg4028u; msg5908 msg5908u; + msg5939 msg5939u; + msg5942 msg5942u; uint8_t bytes[256]; };