Skip to content

Commit

Permalink
AP_GPS: Support integrated headings from SBF
Browse files Browse the repository at this point in the history
  • Loading branch information
@WickedShell @rmackay9
WickedShell authored and rmackay9 committed Nov 23, 2023
1 parent 4fd54dc commit 2e75b36
Show file tree
Hide file tree
Showing 4 changed files with 129 additions and 10 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-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

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
93 changes: 88 additions & 5 deletions libraries/AP_GPS/AP_GPS_SBF.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -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);
Expand All @@ -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;
Expand All @@ -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);
Expand All @@ -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;
Expand All @@ -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;
}

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,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
Expand Down
37 changes: 36 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,
AttEuler = 5938,
AttEulerCov = 5939,
};

struct PACKED msg4007 // PVTGeodetic
Expand Down Expand Up @@ -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];
};

Expand Down

0 comments on commit 2e75b36

Please sign in to comment.