Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_GPS: Support integrated headings from SBF #25601

Merged
merged 1 commit into from
Nov 28, 2023
Merged

AP_GPS: Support integrated headings from SBF #25601

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 4 additions & 4 deletions libraries/AP_GPS/AP_GPS.cpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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