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 #19165

Closed
wants to merge 2 commits into from
Closed

AP_GPS: Support integrated headings from SBF #19165

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
b703e46
AP_GPS: Support integrated headings from SBF
WickedShell Nov 5, 2021
046b393
AP_GPS: Switch to MultiAntenna rather then MovingBase on SBF dual ant…
WickedShell Feb 1, 2022
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 @@ -114,18 +114,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: _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 @@ -631,6 +629,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 @@ -780,6 +779,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_GSOF_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 @@ -134,6 +134,7 @@ class AP_GPS
#if HAL_SIM_GPS_ENABLED
GPS_TYPE_SITL = 100,
#endif
GPS_TYPE_SBF_DUAL_ANTENNA = 26,
};

/// GPS status codes. These are kept aligned with MAVLink by
Expand Down
99 changes: 94 additions & 5 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 @@ -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,23 @@ 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";
}
// We could set the UseBaseForYaw option to set the attitude here, and the attitude offset
// which would generate an AttEuler message for us, and wouldn't require any extra handling
// but by doing it in ArduPilot we can apply our constraints on baseline distances
// if (driver_options() & DriverOptions::SBF_UseBaseForYaw) {
// targetGA = "MovingBase";
// }
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 +200,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 +208,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 +381,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 +539,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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,6 +73,7 @@ class AP_GPS_SBF : public AP_GPS_Backend
SSO,
Blob,
SBAS,
SGA,
Complete
};
Config_State config_step;
Expand Down Expand Up @@ -109,7 +110,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 @@ -217,12 +220,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