Skip to content

Commit

Permalink
AP_GPS: SBF supports yaw from dual antennas
Browse files Browse the repository at this point in the history 
Co-authored-by: Andrew Tridgell <[email protected]>
Co-authored-by: Randy Mackay <[email protected]>
  • Loading branch information
@WickedShell @tridge @rmackay9
3 people committed Nov 23, 2023
1 parent 14aeb83 commit 7a3f6eb
Show file tree
Hide file tree
Showing 4 changed files with 133 additions and 12 deletions.
8 changes: 4 additions & 4 deletions libraries/AP_GPS/AP_GPS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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
Expand Down
1 change: 1 addition & 0 deletions libraries/AP_GPS/AP_GPS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
92 changes: 85 additions & 7 deletions libraries/AP_GPS/AP_GPS_SBF.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
}
Expand All @@ -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);
Expand All @@ -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;
Expand All @@ -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);
Expand All @@ -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;
Expand All @@ -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;
}

Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;
Expand Down
44 changes: 43 additions & 1 deletion libraries/AP_GPS/AP_GPS_SBF.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,7 @@ class AP_GPS_SBF : public AP_GPS_Backend
SSO,
Blob,
SBAS,
SGA,
Complete
};
Config_State config_step;
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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];
};

Expand Down

0 comments on commit 7a3f6eb

Please sign in to comment.