AP_GPS: sync with master for new yaw handling
This commit is contained in:
Andrew Tridgell
parent
8c9efec4e3
commit
4c27e07d06
@ -60,7 +60,7 @@
|
||||
extern const AP_HAL::HAL &hal;
|
||||
|
||||
// baudrates to try to detect GPSes with
|
||||
const uint32_t AP_GPS::_baudrates[] = {9600U, 115200U, 4800U, 19200U, 38400U, 57600U, 230400U};
|
||||
const uint32_t AP_GPS::_baudrates[] = {9600U, 115200U, 4800U, 19200U, 38400U, 57600U, 230400U, 460800U};
|
||||
|
||||
// initialisation blobs to send to the GPS to try to get it into the
|
||||
// right mode
|
||||
@ -73,7 +73,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
|
||||
// @Param: TYPE
|
||||
// @DisplayName: GPS type
|
||||
// @Description: GPS type
|
||||
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA
|
||||
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover
|
||||
// @RebootRequired: True
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("TYPE", 0, AP_GPS, _type[0], HAL_GPS_TYPE_DEFAULT),
|
||||
@ -82,7 +82,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
|
||||
// @Param: TYPE2
|
||||
// @DisplayName: 2nd GPS type
|
||||
// @Description: GPS type of 2nd GPS
|
||||
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA
|
||||
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover
|
||||
// @RebootRequired: True
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("TYPE2", 1, AP_GPS, _type[1], 0),
|
||||
@ -205,44 +205,50 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
|
||||
// @DisplayName: Antenna X position offset
|
||||
// @Description: X position of the first GPS antenna in body frame. Positive X is forward of the origin. Use antenna phase centroid location if provided by the manufacturer.
|
||||
// @Units: m
|
||||
// @Range: -10 10
|
||||
// @Range: -5 5
|
||||
// @Increment: 0.01
|
||||
// @User: Advanced
|
||||
|
||||
// @Param: POS1_Y
|
||||
// @DisplayName: Antenna Y position offset
|
||||
// @Description: Y position of the first GPS antenna in body frame. Positive Y is to the right of the origin. Use antenna phase centroid location if provided by the manufacturer.
|
||||
// @Units: m
|
||||
// @Range: -10 10
|
||||
// @Range: -5 5
|
||||
// @Increment: 0.01
|
||||
// @User: Advanced
|
||||
|
||||
// @Param: POS1_Z
|
||||
// @DisplayName: Antenna Z position offset
|
||||
// @Description: Z position of the first GPS antenna in body frame. Positive Z is down from the origin. Use antenna phase centroid location if provided by the manufacturer.
|
||||
// @Units: m
|
||||
// @Range: -10 10
|
||||
// @Range: -5 5
|
||||
// @Increment: 0.01
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("POS1", 16, AP_GPS, _antenna_offset[0], 0.0f),
|
||||
|
||||
#if GPS_MAX_RECEIVERS > 1
|
||||
// @Param: POS2_X
|
||||
// @DisplayName: Antenna X position offset
|
||||
// @Description: X position of the second GPS antenna in body frame. Positive X is forward of the origin. Use antenna phase centroid location if provided by the manufacturer.
|
||||
// @Units: m
|
||||
// @Range: -10 10
|
||||
// @Range: -5 5
|
||||
// @Increment: 0.01
|
||||
// @User: Advanced
|
||||
|
||||
// @Param: POS2_Y
|
||||
// @DisplayName: Antenna Y position offset
|
||||
// @Description: Y position of the second GPS antenna in body frame. Positive Y is to the right of the origin. Use antenna phase centroid location if provided by the manufacturer.
|
||||
// @Units: m
|
||||
// @Range: -10 10
|
||||
// @Range: -5 5
|
||||
// @Increment: 0.01
|
||||
// @User: Advanced
|
||||
|
||||
#if GPS_MAX_RECEIVERS > 1
|
||||
// @Param: POS2_Z
|
||||
// @DisplayName: Antenna Z position offset
|
||||
// @Description: Z position of the second GPS antenna in body frame. Positive Z is down from the origin. Use antenna phase centroid location if provided by the manufacturer.
|
||||
// @Units: m
|
||||
// @Range: -10 10
|
||||
// @Range: -5 5
|
||||
// @Increment: 0.01
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("POS2", 17, AP_GPS, _antenna_offset[1], 0.0f),
|
||||
#endif
|
||||
@ -284,6 +290,15 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
|
||||
AP_GROUPINFO("BLEND_TC", 21, AP_GPS, _blend_tc, 10.0f),
|
||||
#endif
|
||||
|
||||
#if GPS_UBLOX_MOVING_BASELINE
|
||||
// @Param: DRV_OPTIONS
|
||||
// @DisplayName: driver options
|
||||
// @Description: Additional backend specific options
|
||||
// @Bitmask: 0:Use UART2 for moving baseline on ublox
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("DRV_OPTIONS", 22, AP_GPS, _driver_options, 0),
|
||||
#endif
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
@ -529,6 +544,10 @@ void AP_GPS::detect_instance(uint8_t instance)
|
||||
if (_auto_config == GPS_AUTO_CONFIG_ENABLE && new_gps == nullptr) {
|
||||
if (_type[instance] == GPS_TYPE_HEMI) {
|
||||
send_blob_start(instance, AP_GPS_NMEA_HEMISPHERE_INIT_STRING, strlen(AP_GPS_NMEA_HEMISPHERE_INIT_STRING));
|
||||
} else if (_type[instance] == GPS_TYPE_UBLOX_RTK_BASE ||
|
||||
_type[instance] == GPS_TYPE_UBLOX_RTK_ROVER) {
|
||||
static const char blob[] = UBLOX_SET_BINARY_460800;
|
||||
send_blob_start(instance, blob, sizeof(blob));
|
||||
} else {
|
||||
send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));
|
||||
}
|
||||
@ -549,11 +568,25 @@ void AP_GPS::detect_instance(uint8_t instance)
|
||||
the uBlox into 115200 no matter what rate it is configured
|
||||
for.
|
||||
*/
|
||||
if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_UBLOX) &&
|
||||
if ((_type[instance] == GPS_TYPE_AUTO ||
|
||||
_type[instance] == GPS_TYPE_UBLOX) &&
|
||||
((!_auto_config && _baudrates[dstate->current_baud] >= 38400) ||
|
||||
_baudrates[dstate->current_baud] == 115200) &&
|
||||
AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) {
|
||||
new_gps = new AP_GPS_UBLOX(*this, state[instance], _port[instance]);
|
||||
new_gps = new AP_GPS_UBLOX(*this, state[instance], _port[instance], GPS_ROLE_NORMAL);
|
||||
}
|
||||
|
||||
if ((_type[instance] == GPS_TYPE_UBLOX_RTK_BASE ||
|
||||
_type[instance] == GPS_TYPE_UBLOX_RTK_ROVER) &&
|
||||
_baudrates[dstate->current_baud] == 460800 &&
|
||||
AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) {
|
||||
GPS_Role role;
|
||||
if (_type[instance] == GPS_TYPE_UBLOX_RTK_BASE) {
|
||||
role = GPS_ROLE_MB_BASE;
|
||||
} else {
|
||||
role = GPS_ROLE_MB_ROVER;
|
||||
}
|
||||
new_gps = new AP_GPS_UBLOX(*this, state[instance], _port[instance], role);
|
||||
}
|
||||
#ifndef HAL_BUILD_AP_PERIPH
|
||||
#if !HAL_MINIMIZE_FEATURES
|
||||
@ -602,6 +635,9 @@ found_gps:
|
||||
timing[instance].last_message_time_ms = now;
|
||||
timing[instance].delta_time_ms = GPS_TIMEOUT_MS;
|
||||
new_gps->broadcast_gps_type();
|
||||
if (instance == 1) {
|
||||
has_had_second_instance = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -712,6 +748,44 @@ void AP_GPS::update_instance(uint8_t instance)
|
||||
data_should_be_logged = true;
|
||||
}
|
||||
|
||||
#if GPS_MAX_RECEIVERS > 1
|
||||
if (drivers[instance] && _type[instance] == GPS_TYPE_UBLOX_RTK_BASE) {
|
||||
// see if a moving baseline base has some RTCMv3 data
|
||||
// which we need to pass along to the rover
|
||||
const uint8_t *rtcm_data;
|
||||
uint16_t rtcm_len;
|
||||
if (drivers[instance]->get_RTCMV3(rtcm_data, rtcm_len)) {
|
||||
for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) {
|
||||
if (i != instance && _type[i] == GPS_TYPE_UBLOX_RTK_ROVER) {
|
||||
// pass the data to the rover
|
||||
inject_data(i, rtcm_data, rtcm_len);
|
||||
drivers[instance]->clear_RTCMV3();
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
if (data_should_be_logged) {
|
||||
// keep count of delayed frames and average frame delay for health reporting
|
||||
const uint16_t gps_max_delta_ms = 245; // 200 ms (5Hz) + 45 ms buffer
|
||||
GPS_timing &t = timing[instance];
|
||||
|
||||
if (t.delta_time_ms > gps_max_delta_ms) {
|
||||
t.delayed_count++;
|
||||
} else {
|
||||
t.delayed_count = 0;
|
||||
}
|
||||
if (t.delta_time_ms < 2000) {
|
||||
if (t.average_delta_ms <= 0) {
|
||||
t.average_delta_ms = t.delta_time_ms;
|
||||
} else {
|
||||
t.average_delta_ms = 0.98f * t.average_delta_ms + 0.02f * t.delta_time_ms;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#ifndef HAL_BUILD_AP_PERIPH
|
||||
if (data_should_be_logged &&
|
||||
(should_log() || AP::ahrs().have_ekf_logging())) {
|
||||
@ -734,6 +808,8 @@ void AP_GPS::update_instance(uint8_t instance)
|
||||
*/
|
||||
void AP_GPS::update(void)
|
||||
{
|
||||
WITH_SEMAPHORE(rsem);
|
||||
|
||||
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
|
||||
update_instance(i);
|
||||
}
|
||||
@ -745,6 +821,20 @@ void AP_GPS::update(void)
|
||||
}
|
||||
}
|
||||
|
||||
update_primary();
|
||||
|
||||
#ifndef HAL_BUILD_AP_PERIPH
|
||||
// update notify with gps status. We always base this on the primary_instance
|
||||
AP_Notify::flags.gps_status = state[primary_instance].status;
|
||||
AP_Notify::flags.gps_num_sats = state[primary_instance].num_sats;
|
||||
#endif
|
||||
}
|
||||
|
||||
/*
|
||||
update primary GPS instance
|
||||
*/
|
||||
void AP_GPS::update_primary(void)
|
||||
{
|
||||
#if defined(GPS_BLENDED_INSTANCE)
|
||||
// if blending is requested, attempt to calculate weighting for each GPS
|
||||
if (_auto_switch == 2) {
|
||||
@ -769,72 +859,84 @@ void AP_GPS::update(void)
|
||||
calc_blended_state();
|
||||
// set primary to the virtual instance
|
||||
primary_instance = GPS_BLENDED_INSTANCE;
|
||||
} else {
|
||||
// use switch logic to find best GPS
|
||||
uint32_t now = AP_HAL::millis();
|
||||
if (_auto_switch == 3) {
|
||||
// select the second GPS instance
|
||||
primary_instance = 1;
|
||||
} else if (_auto_switch >= 1) {
|
||||
// handling switching away from blended GPS
|
||||
if (primary_instance == GPS_BLENDED_INSTANCE) {
|
||||
primary_instance = 0;
|
||||
for (uint8_t i=1; i<GPS_MAX_RECEIVERS; i++) {
|
||||
// choose GPS with highest state or higher number of satellites
|
||||
if ((state[i].status > state[primary_instance].status) ||
|
||||
((state[i].status == state[primary_instance].status) && (state[i].num_sats > state[primary_instance].num_sats))) {
|
||||
primary_instance = i;
|
||||
_last_instance_swap_ms = now;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// handle switch between real GPSs
|
||||
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
|
||||
if (i == primary_instance) {
|
||||
continue;
|
||||
}
|
||||
if (state[i].status > state[primary_instance].status) {
|
||||
// we have a higher status lock, or primary is set to the blended GPS, change GPS
|
||||
primary_instance = i;
|
||||
_last_instance_swap_ms = now;
|
||||
continue;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
bool another_gps_has_1_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 1);
|
||||
if (_auto_switch == 0) {
|
||||
// AUTO_SWITCH is 0 so no switching of GPSs, always use first instance
|
||||
primary_instance = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
if (state[i].status == state[primary_instance].status && another_gps_has_1_or_more_sats) {
|
||||
if (_auto_switch == 3) {
|
||||
// always select the second GPS instance
|
||||
primary_instance = 1;
|
||||
return;
|
||||
}
|
||||
|
||||
bool another_gps_has_2_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 2);
|
||||
uint32_t now = AP_HAL::millis();
|
||||
|
||||
if ((another_gps_has_1_or_more_sats && (now - _last_instance_swap_ms) >= 20000) ||
|
||||
(another_gps_has_2_or_more_sats && (now - _last_instance_swap_ms) >= 5000)) {
|
||||
// this GPS has more satellites than the
|
||||
// current primary, switch primary. Once we switch we will
|
||||
// then tend to stick to the new GPS as primary. We don't
|
||||
// want to switch too often as it will look like a
|
||||
// position shift to the controllers.
|
||||
primary_instance = i;
|
||||
_last_instance_swap_ms = now;
|
||||
}
|
||||
}
|
||||
}
|
||||
// special handling of RTK moving baseline pair. If a rover has a
|
||||
// RTK fixed lock and yaw available then always select it as
|
||||
// primary. This ensures that the yaw data and position/velocity
|
||||
// data is time aligned whenever we provide yaw to the EKF
|
||||
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
|
||||
if (_type[i] == GPS_TYPE_UBLOX_RTK_ROVER &&
|
||||
state[i].status == GPS_OK_FIX_3D_RTK_FIXED &&
|
||||
state[i].have_gps_yaw) {
|
||||
if (primary_instance != i) {
|
||||
_last_instance_swap_ms = now;
|
||||
primary_instance = i;
|
||||
}
|
||||
} else {
|
||||
// AUTO_SWITCH is 0 so no switching of GPSs
|
||||
primary_instance = 0;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// handling switching away from blended GPS
|
||||
if (primary_instance == GPS_BLENDED_INSTANCE) {
|
||||
primary_instance = 0;
|
||||
for (uint8_t i=1; i<GPS_MAX_RECEIVERS; i++) {
|
||||
// choose GPS with highest state or higher number of satellites
|
||||
if ((state[i].status > state[primary_instance].status) ||
|
||||
((state[i].status == state[primary_instance].status) && (state[i].num_sats > state[primary_instance].num_sats))) {
|
||||
primary_instance = i;
|
||||
_last_instance_swap_ms = now;
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// handle switch between real GPSs
|
||||
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
|
||||
if (i == primary_instance) {
|
||||
continue;
|
||||
}
|
||||
if (state[i].status > state[primary_instance].status) {
|
||||
// we have a higher status lock, or primary is set to the blended GPS, change GPS
|
||||
primary_instance = i;
|
||||
_last_instance_swap_ms = now;
|
||||
continue;
|
||||
}
|
||||
|
||||
// copy the primary instance to the blended instance in case it is enabled later
|
||||
state[GPS_BLENDED_INSTANCE] = state[primary_instance];
|
||||
_blended_antenna_offset = _antenna_offset[primary_instance];
|
||||
bool another_gps_has_1_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 1);
|
||||
|
||||
if (state[i].status == state[primary_instance].status && another_gps_has_1_or_more_sats) {
|
||||
|
||||
bool another_gps_has_2_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 2);
|
||||
|
||||
if ((another_gps_has_1_or_more_sats && (now - _last_instance_swap_ms) >= 20000) ||
|
||||
(another_gps_has_2_or_more_sats && (now - _last_instance_swap_ms) >= 5000)) {
|
||||
// this GPS has more satellites than the
|
||||
// current primary, switch primary. Once we switch we will
|
||||
// then tend to stick to the new GPS as primary. We don't
|
||||
// want to switch too often as it will look like a
|
||||
// position shift to the controllers.
|
||||
primary_instance = i;
|
||||
_last_instance_swap_ms = now;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif // GPS_BLENDED_INSTANCE
|
||||
|
||||
#ifndef HAL_BUILD_AP_PERIPH
|
||||
// update notify with gps status. We always base this on the primary_instance
|
||||
AP_Notify::flags.gps_status = state[primary_instance].status;
|
||||
AP_Notify::flags.gps_num_sats = state[primary_instance].num_sats;
|
||||
#endif
|
||||
}
|
||||
|
||||
void AP_GPS::handle_gps_inject(const mavlink_message_t &msg)
|
||||
@ -948,6 +1050,10 @@ void AP_GPS::inject_data(const uint8_t *data, uint16_t len)
|
||||
//Support broadcasting to all GPSes.
|
||||
if (_inject_to == GPS_RTK_INJECT_TO_ALL) {
|
||||
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
|
||||
if (_type[i] == GPS_TYPE_UBLOX_RTK_ROVER) {
|
||||
// we don't externally inject to moving baseline rover
|
||||
continue;
|
||||
}
|
||||
inject_data(i, data, len);
|
||||
}
|
||||
} else {
|
||||
@ -962,23 +1068,30 @@ void AP_GPS::inject_data(uint8_t instance, const uint8_t *data, uint16_t len)
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
get GPS yaw following mavlink GPS_RAW_INT and GPS2_RAW
|
||||
convention. We return 0 if the GPS is not configured to provide
|
||||
yaw. We return 65535 for a GPS configured to provide yaw that can't
|
||||
currently provide it. We return from 1 to 36000 for yaw otherwise
|
||||
*/
|
||||
uint16_t AP_GPS::gps_yaw_cdeg(uint8_t instance) const
|
||||
{
|
||||
if (!have_gps_yaw_configured(instance)) {
|
||||
return 0;
|
||||
}
|
||||
float yaw_deg, accuracy_deg;
|
||||
if (!gps_yaw_deg(instance, yaw_deg, accuracy_deg)) {
|
||||
return 65535;
|
||||
}
|
||||
int yaw_cd = wrap_360_cd(yaw_deg * 100);
|
||||
if (yaw_cd == 0) {
|
||||
return 36000;
|
||||
}
|
||||
return yaw_cd;
|
||||
}
|
||||
|
||||
void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
|
||||
{
|
||||
static uint32_t last_send_time_ms[MAVLINK_COMM_NUM_BUFFERS];
|
||||
if (status(0) > AP_GPS::NO_GPS) {
|
||||
// when we have a GPS then only send new data
|
||||
if (last_send_time_ms[chan] == last_message_time_ms(0)) {
|
||||
return;
|
||||
}
|
||||
last_send_time_ms[chan] = last_message_time_ms(0);
|
||||
} else {
|
||||
// when we don't have a GPS then send at 1Hz
|
||||
uint32_t now = AP_HAL::millis();
|
||||
if (now - last_send_time_ms[chan] < 1000) {
|
||||
return;
|
||||
}
|
||||
last_send_time_ms[chan] = now;
|
||||
}
|
||||
const Location &loc = location(0);
|
||||
float hacc = 0.0f;
|
||||
float vacc = 0.0f;
|
||||
@ -1002,21 +1115,17 @@ void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
|
||||
hacc * 1000, // one-sigma standard deviation in mm
|
||||
vacc * 1000, // one-sigma standard deviation in mm
|
||||
sacc * 1000, // one-sigma standard deviation in mm/s
|
||||
0); // TODO one-sigma heading accuracy standard deviation
|
||||
0, // TODO one-sigma heading accuracy standard deviation
|
||||
gps_yaw_cdeg(0));
|
||||
}
|
||||
|
||||
#if GPS_MAX_RECEIVERS > 1
|
||||
void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan)
|
||||
{
|
||||
static uint32_t last_send_time_ms[MAVLINK_COMM_NUM_BUFFERS];
|
||||
if (num_instances < 2 || status(1) <= AP_GPS::NO_GPS) {
|
||||
// always send the message once we've ever seen the GPS
|
||||
if (!has_had_second_instance) {
|
||||
return;
|
||||
}
|
||||
// when we have a GPS then only send new data
|
||||
if (last_send_time_ms[chan] == last_message_time_ms(1)) {
|
||||
return;
|
||||
}
|
||||
last_send_time_ms[chan] = last_message_time_ms(1);
|
||||
|
||||
const Location &loc = location(1);
|
||||
mavlink_msg_gps2_raw_send(
|
||||
@ -1032,7 +1141,8 @@ void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan)
|
||||
ground_course(1)*100, // 1/100 degrees,
|
||||
num_sats(1),
|
||||
state[1].rtk_num_sats,
|
||||
state[1].rtk_age_ms);
|
||||
state[1].rtk_age_ms,
|
||||
gps_yaw_cdeg(1));
|
||||
}
|
||||
#endif // GPS_MAX_RECEIVERS
|
||||
|
||||
@ -1062,7 +1172,7 @@ void AP_GPS::broadcast_first_configuration_failure_reason(void) const
|
||||
uint8_t unconfigured;
|
||||
if (first_unconfigured_gps(unconfigured)) {
|
||||
if (drivers[unconfigured] == nullptr) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: was not found", unconfigured + 1);
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %d: was not found", unconfigured + 1);
|
||||
} else {
|
||||
drivers[unconfigured]->broadcast_configuration_failure_reason();
|
||||
}
|
||||
@ -1540,42 +1650,6 @@ void AP_GPS::calc_blended_state(void)
|
||||
state[GPS_BLENDED_INSTANCE].ground_speed = norm(state[GPS_BLENDED_INSTANCE].velocity.x, state[GPS_BLENDED_INSTANCE].velocity.y);
|
||||
state[GPS_BLENDED_INSTANCE].ground_course = wrap_360(degrees(atan2f(state[GPS_BLENDED_INSTANCE].velocity.y, state[GPS_BLENDED_INSTANCE].velocity.x)));
|
||||
|
||||
/*
|
||||
* The blended location in _output_state.location is not stable enough to be used by the autopilot
|
||||
* as it will move around as the relative accuracy changes. To mitigate this effect a low-pass filtered
|
||||
* offset from each GPS location to the blended location is calculated and the filtered offset is applied
|
||||
* to each receiver.
|
||||
*/
|
||||
|
||||
// Calculate filter coefficients to be applied to the offsets for each GPS position and height offset
|
||||
// A weighting of 1 will make the offset adjust the slowest, a weighting of 0 will make it adjust with zero filtering
|
||||
float alpha[GPS_MAX_RECEIVERS] = {};
|
||||
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
|
||||
if (state[i].last_gps_time_ms - _last_time_updated[i] > 0) {
|
||||
float min_alpha = constrain_float(_omega_lpf * 0.001f * (float)(state[i].last_gps_time_ms - _last_time_updated[i]), 0.0f, 1.0f);
|
||||
if (_blend_weights[i] > min_alpha) {
|
||||
alpha[i] = min_alpha / _blend_weights[i];
|
||||
} else {
|
||||
alpha[i] = 1.0f;
|
||||
}
|
||||
_last_time_updated[i] = state[i].last_gps_time_ms;
|
||||
}
|
||||
}
|
||||
|
||||
// Calculate the offset from each GPS solution to the blended solution
|
||||
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
|
||||
_NE_pos_offset_m[i] = state[i].location.get_distance_NE(state[GPS_BLENDED_INSTANCE].location) * alpha[i] + _NE_pos_offset_m[i] * (1.0f - alpha[i]);
|
||||
_hgt_offset_cm[i] = (float)(state[GPS_BLENDED_INSTANCE].location.alt - state[i].location.alt) * alpha[i] + _hgt_offset_cm[i] * (1.0f - alpha[i]);
|
||||
}
|
||||
|
||||
// Calculate a corrected location for each GPS
|
||||
Location corrected_location[GPS_MAX_RECEIVERS];
|
||||
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
|
||||
corrected_location[i] = state[i].location;
|
||||
corrected_location[i].offset(_NE_pos_offset_m[i].x, _NE_pos_offset_m[i].y);
|
||||
corrected_location[i].alt += (int)(_hgt_offset_cm[i]);
|
||||
}
|
||||
|
||||
// If the GPS week is the same then use a blended time_week_ms
|
||||
// If week is different, then use time stamp from GPS with largest weighting
|
||||
// detect inconsistent week data
|
||||
@ -1631,18 +1705,24 @@ bool AP_GPS::is_healthy(uint8_t instance) const
|
||||
return false;
|
||||
}
|
||||
|
||||
const uint16_t gps_max_delta_ms = 245; // 200 ms (5Hz) + 45 ms buffer
|
||||
|
||||
bool last_msg_valid = last_message_delta_time_ms(instance) < gps_max_delta_ms;
|
||||
/*
|
||||
allow two lost frames before declaring the GPS unhealthy, but
|
||||
require the average frame rate to be close to 5Hz. We allow for
|
||||
a bit higher average for a rover due to the packet loss that
|
||||
happens with the RTCMv3 data
|
||||
*/
|
||||
const uint8_t delay_threshold = 2;
|
||||
const float delay_avg_max = _type[instance] == GPS_TYPE_UBLOX_RTK_ROVER?245:215;
|
||||
const GPS_timing &t = timing[instance];
|
||||
bool delay_ok = (t.delayed_count < delay_threshold) && t.average_delta_ms < delay_avg_max;
|
||||
|
||||
#if defined(GPS_BLENDED_INSTANCE)
|
||||
if (instance == GPS_BLENDED_INSTANCE) {
|
||||
return last_msg_valid && blend_health_check();
|
||||
return delay_ok && blend_health_check();
|
||||
}
|
||||
#endif
|
||||
|
||||
return last_msg_valid &&
|
||||
drivers[instance] != nullptr &&
|
||||
return delay_ok && drivers[instance] != nullptr &&
|
||||
drivers[instance]->is_healthy();
|
||||
}
|
||||
|
||||
@ -1656,6 +1736,20 @@ bool AP_GPS::prepare_for_arming(void) {
|
||||
return all_passed;
|
||||
}
|
||||
|
||||
bool AP_GPS::logging_failed(void) const {
|
||||
if (!logging_enabled()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
|
||||
if ((drivers[i] != nullptr) && !(drivers[i]->logging_healthy())) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
namespace AP {
|
||||
|
||||
AP_GPS &gps()
|
||||
|
||||
@ -42,6 +42,10 @@
|
||||
|
||||
#define UNIX_OFFSET_MSEC (17000ULL * 86400ULL + 52ULL * 10ULL * AP_MSEC_PER_WEEK - GPS_LEAPSECONDS_MILLIS)
|
||||
|
||||
#ifndef GPS_UBLOX_MOVING_BASELINE
|
||||
#define GPS_UBLOX_MOVING_BASELINE !HAL_MINIMIZE_FEATURES && GPS_MAX_RECEIVERS>1
|
||||
#endif
|
||||
|
||||
class AP_GPS_Backend;
|
||||
|
||||
/// @class AP_GPS
|
||||
@ -74,6 +78,11 @@ public:
|
||||
return _singleton;
|
||||
}
|
||||
|
||||
// allow threads to lock against GPS update
|
||||
HAL_Semaphore &get_semaphore(void) {
|
||||
return rsem;
|
||||
}
|
||||
|
||||
// GPS driver types
|
||||
enum GPS_Type {
|
||||
GPS_TYPE_NONE = 0,
|
||||
@ -92,6 +101,8 @@ public:
|
||||
GPS_TYPE_MAV = 14,
|
||||
GPS_TYPE_NOVA = 15,
|
||||
GPS_TYPE_HEMI = 16, // hemisphere NMEA
|
||||
GPS_TYPE_UBLOX_RTK_BASE = 17,
|
||||
GPS_TYPE_UBLOX_RTK_ROVER = 18,
|
||||
};
|
||||
|
||||
/// GPS status codes
|
||||
@ -119,9 +130,16 @@ public:
|
||||
GPS_ENGINE_AIRBORNE_4G = 8
|
||||
};
|
||||
|
||||
enum GPS_Config {
|
||||
enum GPS_Config {
|
||||
GPS_ALL_CONFIGURED = 255
|
||||
};
|
||||
};
|
||||
|
||||
// role for auto-config
|
||||
enum GPS_Role {
|
||||
GPS_ROLE_NORMAL,
|
||||
GPS_ROLE_MB_BASE,
|
||||
GPS_ROLE_MB_ROVER,
|
||||
};
|
||||
|
||||
/*
|
||||
The GPS_State structure is filled in by the backend driver as it
|
||||
@ -138,6 +156,7 @@ public:
|
||||
float ground_speed; ///< ground speed in m/sec
|
||||
float ground_course; ///< ground course in degrees
|
||||
float gps_yaw; ///< GPS derived yaw information, if available (degrees)
|
||||
bool gps_yaw_configured; ///< GPS is configured to provide yaw
|
||||
uint16_t hdop; ///< horizontal dilution of precision in cm
|
||||
uint16_t vdop; ///< vertical dilution of precision in cm
|
||||
uint8_t num_sats; ///< Number of visible satellites
|
||||
@ -358,13 +377,20 @@ public:
|
||||
return have_vertical_velocity(primary_instance);
|
||||
}
|
||||
|
||||
// return true if the GPS supports yaw
|
||||
// return true if the GPS currently has yaw available
|
||||
bool have_gps_yaw(uint8_t instance) const {
|
||||
return state[instance].have_gps_yaw;
|
||||
}
|
||||
bool have_gps_yaw(void) const {
|
||||
return have_gps_yaw(primary_instance);
|
||||
}
|
||||
|
||||
// return true if the GPS is configured to provide yaw. This will
|
||||
// be true if we expect the GPS to provide yaw, even if it
|
||||
// currently is not able to provide yaw
|
||||
bool have_gps_yaw_configured(uint8_t instance) const {
|
||||
return state[instance].gps_yaw_configured;
|
||||
}
|
||||
|
||||
// the expected lag (in seconds) in the position and velocity readings from the gps
|
||||
// return true if the GPS hardware configuration is known or the lag parameter has been set manually
|
||||
@ -431,6 +457,12 @@ public:
|
||||
// returns true if all GPS instances have passed all final arming checks/state changes
|
||||
bool prepare_for_arming(void);
|
||||
|
||||
// returns false if any GPS drivers are not performing their logging appropriately
|
||||
bool logging_failed(void) const;
|
||||
|
||||
bool logging_present(void) const { return _raw_data != 0; }
|
||||
bool logging_enabled(void) const { return _raw_data != 0; }
|
||||
|
||||
// used to disable GPS for GPS failure testing in flight
|
||||
void force_disable(bool disable) {
|
||||
_force_disable_gps = disable;
|
||||
@ -439,6 +471,11 @@ public:
|
||||
// handle possibly fragmented RTCM injection data
|
||||
void handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_t len);
|
||||
|
||||
// get configured type by instance
|
||||
GPS_Type get_type(uint8_t instance) const {
|
||||
return instance>=GPS_MAX_RECEIVERS? GPS_Type::GPS_TYPE_NONE : GPS_Type(_type[instance].get());
|
||||
}
|
||||
|
||||
protected:
|
||||
|
||||
// configuration parameters
|
||||
@ -460,11 +497,13 @@ protected:
|
||||
AP_Int16 _delay_ms[GPS_MAX_RECEIVERS];
|
||||
AP_Int8 _blend_mask;
|
||||
AP_Float _blend_tc;
|
||||
AP_Int16 _driver_options;
|
||||
|
||||
uint32_t _log_gps_bit = -1;
|
||||
|
||||
private:
|
||||
static AP_GPS *_singleton;
|
||||
HAL_Semaphore rsem;
|
||||
|
||||
// returns the desired gps update rate in milliseconds
|
||||
// this does not provide any guarantee that the GPS is updating at the requested
|
||||
@ -481,6 +520,12 @@ private:
|
||||
|
||||
// delta time between the last pair of GPS updates in system milliseconds
|
||||
uint16_t delta_time_ms;
|
||||
|
||||
// count of delayed frames
|
||||
uint8_t delayed_count;
|
||||
|
||||
// the average time delta
|
||||
float average_delta_ms;
|
||||
};
|
||||
// Note allowance for an additional instance to contain blended data
|
||||
GPS_timing timing[GPS_MAX_INSTANCES];
|
||||
@ -553,12 +598,9 @@ private:
|
||||
void inject_data(uint8_t instance, const uint8_t *data, uint16_t len);
|
||||
|
||||
// GPS blending and switching
|
||||
Vector2f _NE_pos_offset_m[GPS_MAX_RECEIVERS]; // Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m)
|
||||
float _hgt_offset_cm[GPS_MAX_RECEIVERS]; // Filtered height offset from GPS instance relative to blended solution in _output_state.location (cm)
|
||||
Vector3f _blended_antenna_offset; // blended antenna offset
|
||||
float _blended_lag_sec; // blended receiver lag in seconds
|
||||
float _blend_weights[GPS_MAX_RECEIVERS]; // blend weight for each GPS. The blend weights must sum to 1.0 across all instances.
|
||||
uint32_t _last_time_updated[GPS_MAX_RECEIVERS]; // the last value of state.last_gps_time_ms read for that GPS instance - used to detect new data.
|
||||
float _omega_lpf; // cutoff frequency in rad/sec of LPF applied to position offsets
|
||||
bool _output_is_blended; // true when a blended GPS solution being output
|
||||
uint8_t _blend_health_counter; // 0 = perfectly health, 100 = very unhealthy
|
||||
@ -573,6 +615,12 @@ private:
|
||||
|
||||
bool needs_uart(GPS_Type type) const;
|
||||
|
||||
/// Update primary instance
|
||||
void update_primary(void);
|
||||
|
||||
// helper function for mavlink gps yaw
|
||||
uint16_t gps_yaw_cdeg(uint8_t instance) const;
|
||||
|
||||
// Auto configure types
|
||||
enum GPS_AUTO_CONFIG {
|
||||
GPS_AUTO_CONFIG_DISABLE = 0,
|
||||
@ -581,6 +629,9 @@ private:
|
||||
|
||||
// used for flight testing with GPS loss
|
||||
bool _force_disable_gps;
|
||||
|
||||
// used to ensure we continue sending status messages if we ever detected the second GPS
|
||||
bool has_had_second_instance;
|
||||
};
|
||||
|
||||
namespace AP {
|
||||
|
||||
@ -295,6 +295,11 @@ bool AP_GPS_NMEA::_term_complete()
|
||||
_last_HDT_ms = now;
|
||||
state.gps_yaw = wrap_360(_new_gps_yaw*0.01f);
|
||||
state.have_gps_yaw = true;
|
||||
// remember that we are setup to provide yaw. With
|
||||
// a NMEA GPS we can only tell if the GPS is
|
||||
// configured to provide yaw when it first sends a
|
||||
// HDT sentence.
|
||||
state.gps_yaw_configured = true;
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
|
||||
@ -51,6 +51,15 @@ AP_GPS_NOVA::AP_GPS_NOVA(AP_GPS &_gps, AP_GPS::GPS_State &_state,
|
||||
port->write((const uint8_t*)init_str1, strlen(init_str1));
|
||||
}
|
||||
|
||||
const char* const AP_GPS_NOVA::_initialisation_blob[6] {
|
||||
"\r\n\r\nunlogall\r\n", // cleanup enviroment
|
||||
"log bestposb ontime 0.2 0 nohold\r\n", // get bestpos
|
||||
"log bestvelb ontime 0.2 0 nohold\r\n", // get bestvel
|
||||
"log psrdopb onchanged\r\n", // tersus
|
||||
"log psrdopb ontime 0.2\r\n", // comnav
|
||||
"log psrdopb\r\n" // poll message, as dop only changes when a sat is dropped/added to the visible list
|
||||
};
|
||||
|
||||
// Process all bytes available from the stream
|
||||
//
|
||||
bool
|
||||
|
||||
@ -56,14 +56,7 @@ private:
|
||||
|
||||
uint8_t _init_blob_index = 0;
|
||||
uint32_t _init_blob_time = 0;
|
||||
const char* _initialisation_blob[6] = {
|
||||
"\r\n\r\nunlogall\r\n", // cleanup enviroment
|
||||
"log bestposb ontime 0.2 0 nohold\r\n", // get bestpos
|
||||
"log bestvelb ontime 0.2 0 nohold\r\n", // get bestvel
|
||||
"log psrdopb onchanged\r\n", // tersus
|
||||
"log psrdopb ontime 0.2\r\n", // comnav
|
||||
"log psrdopb\r\n" // poll message, as dop only changes when a sat is dropped/added to the visible list
|
||||
};
|
||||
static const char* const _initialisation_blob[6];
|
||||
|
||||
uint32_t crc_error_counter = 0;
|
||||
uint32_t last_injected_data_ms = 0;
|
||||
|
||||
@ -106,6 +106,17 @@ AP_GPS_SBF::read(void)
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool AP_GPS_SBF::logging_healthy(void) const
|
||||
{
|
||||
switch (gps._raw_data) {
|
||||
case 1:
|
||||
default:
|
||||
return (RxState & SBF_DISK_MOUNTED) && (RxState & SBF_DISK_ACTIVITY);
|
||||
case 2:
|
||||
return ((RxState & SBF_DISK_MOUNTED) && (RxState & SBF_DISK_ACTIVITY)) || (!hal.util->get_soft_armed() && _has_been_armed);
|
||||
}
|
||||
}
|
||||
|
||||
bool
|
||||
AP_GPS_SBF::parse(uint8_t temp)
|
||||
{
|
||||
@ -340,7 +351,7 @@ AP_GPS_SBF::process_message(void)
|
||||
check_new_itow(temp.TOW, sbf_msg.length);
|
||||
RxState = temp.RxState;
|
||||
if ((RxError & RX_ERROR_MASK) != (temp.RxError & RX_ERROR_MASK)) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "GPS %u: SBF error changed (0x%08x/0x%08x)", (unsigned int)(state.instance + 1),
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %u: SBF error changed (0x%08x/0x%08x)", (unsigned int)(state.instance + 1),
|
||||
(unsigned int)(RxError & RX_ERROR_MASK), (unsigned int)(temp.RxError & RX_ERROR_MASK));
|
||||
}
|
||||
RxError = temp.RxError;
|
||||
@ -371,7 +382,7 @@ void AP_GPS_SBF::broadcast_configuration_failure_reason(void) const
|
||||
{
|
||||
if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE &&
|
||||
_init_blob_index < ARRAY_SIZE(_initialisation_blob)) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "GPS %u: SBF is not fully configured (%u/%u)", state.instance + 1,
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %u: SBF is not fully configured (%u/%u)", state.instance + 1,
|
||||
_init_blob_index, (unsigned)ARRAY_SIZE(_initialisation_blob));
|
||||
}
|
||||
}
|
||||
@ -393,7 +404,7 @@ void AP_GPS_SBF::mount_disk (void) const {
|
||||
|
||||
void AP_GPS_SBF::unmount_disk (void) const {
|
||||
const char* command = "emd, DSK1, Unmount\n";
|
||||
Debug("Unmounting disk");
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "SBF unmounting disk");
|
||||
port->write((const uint8_t*)command, strlen(command));
|
||||
}
|
||||
|
||||
@ -402,22 +413,22 @@ bool AP_GPS_SBF::prepare_for_arming(void) {
|
||||
if (gps._raw_data) {
|
||||
if (!(RxState & SBF_DISK_MOUNTED)){
|
||||
is_logging = false;
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: SBF disk is not mounted", state.instance + 1);
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %d: SBF disk is not mounted", state.instance + 1);
|
||||
|
||||
// simply attempt to mount the disk, no need to check if the command was
|
||||
// ACK/NACK'd as we don't continuously attempt to remount the disk
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: Attempting to mount disk", state.instance + 1);
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %d: Attempting to mount disk", state.instance + 1);
|
||||
mount_disk();
|
||||
// reset the flag to indicate if we should be logging
|
||||
_has_been_armed = false;
|
||||
}
|
||||
else if (RxState & SBF_DISK_FULL) {
|
||||
is_logging = false;
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: SBF disk is full", state.instance + 1);
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %d: SBF disk is full", state.instance + 1);
|
||||
}
|
||||
else if (!(RxState & SBF_DISK_ACTIVITY)) {
|
||||
is_logging = false;
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: SBF is not currently logging", state.instance + 1);
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %d: SBF is not currently logging", state.instance + 1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -47,6 +47,8 @@ public:
|
||||
|
||||
bool is_healthy(void) const override;
|
||||
|
||||
bool logging_healthy(void) const override;
|
||||
|
||||
bool prepare_for_arming(void) override;
|
||||
|
||||
|
||||
|
||||
@ -51,7 +51,7 @@ do { \
|
||||
#if SBP_INFOREPORTING
|
||||
# define Info(fmt, args ...) \
|
||||
do { \
|
||||
gcs().send_text(MAV_SEVERITY_INFO, fmt "\n", ## args); \
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, fmt "\n", ## args); \
|
||||
} while(0)
|
||||
#else
|
||||
# define Info(fmt, args ...)
|
||||
|
||||
@ -23,6 +23,8 @@
|
||||
#include <AP_HAL/Util.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include "RTCM3_Parser.h"
|
||||
#include <stdio.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || \
|
||||
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
|
||||
@ -36,13 +38,14 @@
|
||||
#define UBLOX_FAKE_3DLOCK 0
|
||||
#define CONFIGURE_PPS_PIN 0
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
// this is number of epochs per output. A higher value will reduce
|
||||
// the uart bandwidth needed and allow for higher latency
|
||||
#define RTK_MB_RTCM_RATE 1
|
||||
|
||||
#ifdef HAL_NO_GCS
|
||||
#define GCS_SEND_TEXT(severity, format, args...)
|
||||
#else
|
||||
#define GCS_SEND_TEXT(severity, format, args...) gcs().send_text(severity, format, ##args)
|
||||
#endif
|
||||
// use this to enable debugging of moving baseline configs
|
||||
#define UBLOX_MB_DEBUGGING 0
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
#if UBLOX_DEBUGGING
|
||||
# define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
|
||||
@ -50,14 +53,21 @@ extern const AP_HAL::HAL& hal;
|
||||
# define Debug(fmt, args ...)
|
||||
#endif
|
||||
|
||||
AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
|
||||
#if UBLOX_MB_DEBUGGING
|
||||
# define MB_Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
|
||||
#else
|
||||
# define MB_Debug(fmt, args ...)
|
||||
#endif
|
||||
|
||||
AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port, AP_GPS::GPS_Role _role) :
|
||||
AP_GPS_Backend(_gps, _state, _port),
|
||||
_next_message(STEP_PVT),
|
||||
_ublox_port(255),
|
||||
_unconfigured_messages(CONFIG_ALL),
|
||||
_hardware_generation(UBLOX_UNKNOWN_HARDWARE_GENERATION),
|
||||
next_fix(AP_GPS::NO_FIX),
|
||||
noReceivedHdop(true)
|
||||
noReceivedHdop(true),
|
||||
role(_role)
|
||||
{
|
||||
// stop any config strings that are pending
|
||||
gps.send_blob_start(state.instance, nullptr, 0);
|
||||
@ -68,8 +78,136 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART
|
||||
#if CONFIGURE_PPS_PIN
|
||||
_unconfigured_messages |= CONFIG_TP5;
|
||||
#endif
|
||||
|
||||
#if GPS_UBLOX_MOVING_BASELINE
|
||||
if (role == AP_GPS::GPS_ROLE_MB_BASE && !mb_use_uart2()) {
|
||||
rtcm3_parser = new RTCM3_Parser;
|
||||
if (rtcm3_parser == nullptr) {
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "u-blox %d: failed RTCMv3 parser allocation", state.instance + 1);
|
||||
}
|
||||
_unconfigured_messages |= CONFIG_RTK_MOVBASE;
|
||||
}
|
||||
if (role == AP_GPS::GPS_ROLE_MB_ROVER) {
|
||||
_unconfigured_messages |= CONFIG_RTK_MOVBASE;
|
||||
state.gps_yaw_configured = true;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
AP_GPS_UBLOX::~AP_GPS_UBLOX()
|
||||
{
|
||||
#if GPS_UBLOX_MOVING_BASELINE
|
||||
if (rtcm3_parser) {
|
||||
delete rtcm3_parser;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
#if GPS_UBLOX_MOVING_BASELINE
|
||||
/*
|
||||
config for F9 GPS in moving baseline base role
|
||||
See ZED-F9P integration manual section 3.1.5.6.1
|
||||
*/
|
||||
const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_MB_Base_uart1[] {
|
||||
{ ConfigKey::CFG_UART1OUTPROT_RTCM3X, 1},
|
||||
{ ConfigKey::CFG_UART2OUTPROT_RTCM3X, 0},
|
||||
{ ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART1, 0},
|
||||
{ ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART2, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART1, RTK_MB_RTCM_RATE},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART1, RTK_MB_RTCM_RATE},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART1, RTK_MB_RTCM_RATE},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART1, RTK_MB_RTCM_RATE},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART1, RTK_MB_RTCM_RATE},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART1, RTK_MB_RTCM_RATE},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART1, RTK_MB_RTCM_RATE},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART2, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART2, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART2, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART2, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART2, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART2, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART2, 0},
|
||||
};
|
||||
|
||||
const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_MB_Base_uart2[] {
|
||||
{ ConfigKey::CFG_UART2_ENABLED, 1},
|
||||
{ ConfigKey::CFG_UART2_BAUDRATE, 460800},
|
||||
{ ConfigKey::CFG_UART2OUTPROT_RTCM3X, 1},
|
||||
{ ConfigKey::CFG_UART1OUTPROT_RTCM3X, 0},
|
||||
{ ConfigKey::CFG_UART1INPROT_RTCM3X, 1},
|
||||
{ ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART2, 0},
|
||||
{ ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART1, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART2, RTK_MB_RTCM_RATE},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART2, RTK_MB_RTCM_RATE},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART2, RTK_MB_RTCM_RATE},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART2, RTK_MB_RTCM_RATE},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART2, RTK_MB_RTCM_RATE},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART2, RTK_MB_RTCM_RATE},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART2, RTK_MB_RTCM_RATE},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART1, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART1, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART1, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART1, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART1, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART1, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART1, 0},
|
||||
};
|
||||
|
||||
|
||||
/*
|
||||
config for F9 GPS in moving baseline rover role
|
||||
See ZED-F9P integration manual section 3.1.5.6.1.
|
||||
Note that we list the RTCM msg types as 0 to prevent getting RTCM
|
||||
data from a GPS previously configured as a base
|
||||
*/
|
||||
const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_MB_Rover_uart1[] {
|
||||
{ ConfigKey::CFG_UART2OUTPROT_RTCM3X, 0},
|
||||
{ ConfigKey::CFG_UART1INPROT_RTCM3X, 1},
|
||||
{ ConfigKey::CFG_UART2INPROT_RTCM3X, 0},
|
||||
{ ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART1, 1},
|
||||
{ ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART2, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART1, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART1, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART1, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART1, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART1, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART1, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART1, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART2, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART2, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART2, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART2, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART2, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART2, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART2, 0},
|
||||
};
|
||||
|
||||
const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_MB_Rover_uart2[] {
|
||||
{ ConfigKey::CFG_UART2_ENABLED, 1},
|
||||
{ ConfigKey::CFG_UART2_BAUDRATE, 460800},
|
||||
{ ConfigKey::CFG_UART2OUTPROT_RTCM3X, 0},
|
||||
{ ConfigKey::CFG_UART2INPROT_RTCM3X, 1},
|
||||
{ ConfigKey::CFG_UART1INPROT_RTCM3X, 0},
|
||||
{ ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART1, 1},
|
||||
{ ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART2, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART2, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART2, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART2, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART2, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART2, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART2, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART2, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART1, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART1, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART1, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART1, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART1, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART1, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART1, 0},
|
||||
};
|
||||
#endif // GPS_UBLOX_MOVING_BASELINE
|
||||
|
||||
|
||||
void
|
||||
AP_GPS_UBLOX::_request_next_config(void)
|
||||
{
|
||||
@ -212,12 +350,36 @@ AP_GPS_UBLOX::_request_next_config(void)
|
||||
}
|
||||
break;
|
||||
case STEP_TMODE:
|
||||
if (_hardware_generation >= UBLOX_F9) {
|
||||
if (supports_F9_config()) {
|
||||
if (!_configure_valget(ConfigKey::TMODE_MODE)) {
|
||||
_next_message--;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case STEP_RTK_MOVBASE:
|
||||
#if GPS_UBLOX_MOVING_BASELINE
|
||||
if (supports_F9_config()) {
|
||||
static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Base_uart1), "done_mask too small, base1");
|
||||
static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Base_uart2), "done_mask too small, base2");
|
||||
static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Rover_uart1), "done_mask too small, rover1");
|
||||
static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Rover_uart2), "done_mask too small, rover2");
|
||||
if (role == AP_GPS::GPS_ROLE_MB_BASE) {
|
||||
const config_list *list = mb_use_uart2()?config_MB_Base_uart2:config_MB_Base_uart1;
|
||||
uint8_t list_length = mb_use_uart2()?ARRAY_SIZE(config_MB_Base_uart2):ARRAY_SIZE(config_MB_Base_uart1);
|
||||
if (!_configure_config_set(list, list_length, CONFIG_RTK_MOVBASE)) {
|
||||
_next_message--;
|
||||
}
|
||||
}
|
||||
if (role == AP_GPS::GPS_ROLE_MB_ROVER) {
|
||||
const config_list *list = mb_use_uart2()?config_MB_Rover_uart2:config_MB_Rover_uart1;
|
||||
uint8_t list_length = mb_use_uart2()?ARRAY_SIZE(config_MB_Rover_uart2):ARRAY_SIZE(config_MB_Rover_uart1);
|
||||
if (!_configure_config_set(list, list_length, CONFIG_RTK_MOVBASE)) {
|
||||
_next_message--;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
default:
|
||||
// this case should never be reached, do a full reset if it is hit
|
||||
_next_message = STEP_PVT;
|
||||
@ -410,6 +572,20 @@ AP_GPS_UBLOX::read(void)
|
||||
// read the next byte
|
||||
data = port->read();
|
||||
|
||||
#if GPS_UBLOX_MOVING_BASELINE
|
||||
if (rtcm3_parser) {
|
||||
if (rtcm3_parser->read(data)) {
|
||||
// we've found a RTCMv3 packet. We stop parsing at
|
||||
// this point and reset u-blox parse state. We need to
|
||||
// stop parsing to give the higher level driver a
|
||||
// chance to send the RTCMv3 packet to another (rover)
|
||||
// GPS
|
||||
_step = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
reset:
|
||||
switch(_step) {
|
||||
|
||||
@ -506,6 +682,12 @@ AP_GPS_UBLOX::read(void)
|
||||
break; // bad checksum
|
||||
}
|
||||
|
||||
#if GPS_UBLOX_MOVING_BASELINE
|
||||
if (rtcm3_parser) {
|
||||
// this is a uBlox packet, discard any partial RTCMv3 state
|
||||
rtcm3_parser->reset();
|
||||
}
|
||||
#endif
|
||||
if (_parse_gps()) {
|
||||
parsed = true;
|
||||
}
|
||||
@ -647,6 +829,46 @@ void AP_GPS_UBLOX::unexpected_message(void)
|
||||
}
|
||||
}
|
||||
|
||||
// return size of a config key, or 0 if unknown
|
||||
uint8_t AP_GPS_UBLOX::config_key_size(ConfigKey key) const
|
||||
{
|
||||
// step over the value
|
||||
const uint8_t key_size = (uint32_t(key) >> 28) & 0x07; // mask off the storage size
|
||||
switch (key_size) {
|
||||
case 0x1: // bit
|
||||
case 0x2: // byte
|
||||
return 1;
|
||||
case 0x3: // 2 bytes
|
||||
return 2;
|
||||
case 0x4: // 4 bytes
|
||||
return 4;
|
||||
case 0x5: // 8 bytes
|
||||
return 8;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
// invalid
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
find index of a config key in the active_config list, or -1
|
||||
*/
|
||||
int8_t AP_GPS_UBLOX::find_active_config_index(ConfigKey key) const
|
||||
{
|
||||
#if GPS_UBLOX_MOVING_BASELINE
|
||||
if (active_config.list == nullptr) {
|
||||
return -1;
|
||||
}
|
||||
for (uint8_t i=0; i<active_config.count; i++) {
|
||||
if (key == active_config.list[i].key) {
|
||||
return (int8_t)i;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
return -1;
|
||||
}
|
||||
|
||||
bool
|
||||
AP_GPS_UBLOX::_parse_gps(void)
|
||||
{
|
||||
@ -889,7 +1111,8 @@ AP_GPS_UBLOX::_parse_gps(void)
|
||||
if (mode != 0) {
|
||||
// ask for mode 0, to disable TIME mode
|
||||
mode = 0;
|
||||
_configure_valset(ConfigKey::TMODE_MODE, 1, &mode);
|
||||
_configure_valset(ConfigKey::TMODE_MODE, &mode);
|
||||
_cfg_needs_save = true;
|
||||
_unconfigured_messages |= CONFIG_TMODE_MODE;
|
||||
} else {
|
||||
_unconfigured_messages &= ~CONFIG_TMODE_MODE;
|
||||
@ -898,37 +1121,35 @@ AP_GPS_UBLOX::_parse_gps(void)
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
#if GPS_UBLOX_MOVING_BASELINE
|
||||
// see if it is in active config list
|
||||
int8_t cfg_idx = find_active_config_index(id);
|
||||
if (cfg_idx >= 0) {
|
||||
const uint8_t key_size = config_key_size(id);
|
||||
if (cfg_len < key_size ||
|
||||
memcmp(&active_config.list[cfg_idx].value, cfg_data, key_size) != 0) {
|
||||
_configure_valset(id, &active_config.list[cfg_idx].value);
|
||||
_unconfigured_messages |= active_config.unconfig_bit;
|
||||
active_config.done_mask &= ~(1U << cfg_idx);
|
||||
_cfg_needs_save = true;
|
||||
} else {
|
||||
active_config.done_mask |= (1U << cfg_idx);
|
||||
if (active_config.done_mask == (1U<<active_config.count)-1) {
|
||||
// all done!
|
||||
_unconfigured_messages &= ~active_config.unconfig_bit;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif // GPS_UBLOX_MOVING_BASELINE
|
||||
|
||||
// step over the value
|
||||
const uint8_t key_size = ((uint32_t)id >> 28) & 0x07; // mask off the storage size
|
||||
uint8_t step_size = 0;
|
||||
switch (key_size) {
|
||||
case 0x1: // bit
|
||||
step_size = 1;
|
||||
break;
|
||||
case 0x2: // byte
|
||||
step_size = 1;
|
||||
break;
|
||||
case 0x3: // 2 bytes
|
||||
step_size = 2;
|
||||
break;
|
||||
case 0x4: // 4 bytes
|
||||
step_size = 4;
|
||||
break;
|
||||
case 0x5: // 8 bytes
|
||||
step_size = 8;
|
||||
break;
|
||||
default:
|
||||
// unknown/bad key size
|
||||
return false;
|
||||
}
|
||||
if (cfg_len <= step_size) {
|
||||
cfg_len = 0;
|
||||
} else {
|
||||
cfg_len -= step_size;
|
||||
cfg_data += step_size;
|
||||
uint8_t step_size = config_key_size(id);
|
||||
if (step_size == 0) {
|
||||
return false;
|
||||
}
|
||||
cfg_len -= step_size;
|
||||
cfg_data += step_size;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1098,8 +1319,8 @@ AP_GPS_UBLOX::_parse_gps(void)
|
||||
break;
|
||||
case MSG_RELPOSNED:
|
||||
{
|
||||
const Vector3f &offset0 = gps._antenna_offset[0].get();
|
||||
const Vector3f &offset1 = gps._antenna_offset[1].get();
|
||||
const Vector3f &offset0 = gps._antenna_offset[state.instance^1].get();
|
||||
const Vector3f &offset1 = gps._antenna_offset[state.instance].get();
|
||||
// note that we require the yaw to come from a fixed solution, not a float solution
|
||||
// yaw from a float solution would only be acceptable with a very large separation between
|
||||
// GPS modules
|
||||
@ -1112,22 +1333,49 @@ AP_GPS_UBLOX::_parse_gps(void)
|
||||
static_cast<uint32_t>(RELPOSNED::refObsMiss) |
|
||||
static_cast<uint32_t>(RELPOSNED::carrSolnFloat);
|
||||
|
||||
const float offset_dist = (offset0 - offset1).length();
|
||||
const float rel_dist = _buffer.relposned.relPosLength * 1.0e-2;
|
||||
const Vector3f antenna_offset = offset0 - offset1;
|
||||
const float offset_dist = antenna_offset.length();
|
||||
const float rel_dist = _buffer.relposned.relPosLength * 0.01;
|
||||
const float dist_error = offset_dist - rel_dist;
|
||||
const float strict_length_error_allowed = 0.2; // allow for up to 20% error
|
||||
const float min_separation = 0.05;
|
||||
bool tilt_ok = true;
|
||||
const float min_dist = MIN(offset_dist, rel_dist);
|
||||
#ifndef HAL_BUILD_AP_PERIPH
|
||||
// when ahrs is available use it to constrain vertical component
|
||||
const Vector3f antenna_tilt = AP::ahrs().get_rotation_body_to_ned() * antenna_offset;
|
||||
const float alt_error = _buffer.relposned.relPosD*0.01 + antenna_tilt.z;
|
||||
tilt_ok = fabsf(alt_error) < strict_length_error_allowed * min_dist;
|
||||
#endif
|
||||
|
||||
_check_new_itow(_buffer.relposned.iTOW);
|
||||
if (_buffer.relposned.iTOW != _last_relposned_itow+200) {
|
||||
// useful for looking at packet loss on links
|
||||
MB_Debug("RELPOSNED ITOW %u %u\n", unsigned(_buffer.relposned.iTOW), unsigned(_last_relposned_itow));
|
||||
}
|
||||
_last_relposned_itow = _buffer.relposned.iTOW;
|
||||
_last_relposned_ms = AP_HAL::millis();
|
||||
|
||||
/*
|
||||
RELPOSNED messages gives the NED distance from base to
|
||||
rover. It comes from the rover
|
||||
*/
|
||||
MB_Debug("RELPOSNED[%u]: od:%.2f rd:%.2f ae:%.2f flags:0x%04x t=%u",
|
||||
state.instance+1,
|
||||
offset_dist, rel_dist, alt_error,
|
||||
unsigned(_buffer.relposned.flags),
|
||||
unsigned(_buffer.relposned.iTOW));
|
||||
|
||||
if (((_buffer.relposned.flags & valid_mask) == valid_mask) &&
|
||||
((_buffer.relposned.flags & invalid_mask) == 0) &&
|
||||
rel_dist > min_separation &&
|
||||
offset_dist > min_separation &&
|
||||
fabsf(offset_dist - rel_dist) / MIN(offset_dist, rel_dist) < strict_length_error_allowed) {
|
||||
fabsf(dist_error) < strict_length_error_allowed * min_dist &&
|
||||
tilt_ok) {
|
||||
float rotation_offset_rad;
|
||||
const Vector3f diff = offset0 - offset1;
|
||||
const Vector3f diff = offset1 - offset0;
|
||||
rotation_offset_rad = Vector2f(diff.x, diff.y).angle();
|
||||
if (state.instance != 0) {
|
||||
rotation_offset_rad += M_PI;
|
||||
}
|
||||
state.gps_yaw = wrap_360(_buffer.relposned.relPosHeading * 1e-5 + degrees(rotation_offset_rad));
|
||||
state.gps_yaw = wrap_360(_buffer.relposned.relPosHeading * 1e-5 - degrees(rotation_offset_rad));
|
||||
state.have_gps_yaw = true;
|
||||
state.gps_yaw_accuracy = _buffer.relposned.accHeading * 1e-5;
|
||||
state.have_gps_yaw_accuracy = true;
|
||||
@ -1143,6 +1391,7 @@ AP_GPS_UBLOX::_parse_gps(void)
|
||||
havePvtMsg = true;
|
||||
// position
|
||||
_check_new_itow(_buffer.pvt.itow);
|
||||
_last_pvt_itow = _buffer.pvt.itow;
|
||||
_last_pos_time = _buffer.pvt.itow;
|
||||
state.location.lng = _buffer.pvt.lon;
|
||||
state.location.lat = _buffer.pvt.lat;
|
||||
@ -1291,6 +1540,19 @@ AP_GPS_UBLOX::_parse_gps(void)
|
||||
return false;
|
||||
}
|
||||
|
||||
if (state.have_gps_yaw) {
|
||||
// when we are a rover we want to ensure we have both the new
|
||||
// PVT and the new RELPOSNED message so that we give a
|
||||
// consistent view
|
||||
if (AP_HAL::millis() - _last_relposned_ms > 400) {
|
||||
// we have stopped receiving RELPOSNED messages, disable yaw reporting
|
||||
state.have_gps_yaw = false;
|
||||
} else if (_last_relposned_itow != _last_pvt_itow) {
|
||||
// wait until ITOW matches
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// we only return true when we get new position and speed data
|
||||
// this ensures we don't use stale data
|
||||
if (_new_position && _new_speed && _last_vel_time == _last_pos_time) {
|
||||
@ -1387,11 +1649,12 @@ AP_GPS_UBLOX::_configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t
|
||||
* configure F9 based key/value pair - VALSET
|
||||
*/
|
||||
bool
|
||||
AP_GPS_UBLOX::_configure_valset(ConfigKey key, const uint8_t len, const uint8_t *value)
|
||||
AP_GPS_UBLOX::_configure_valset(ConfigKey key, const void *value)
|
||||
{
|
||||
if (_hardware_generation < UBLOX_F9) {
|
||||
if (!supports_F9_config()) {
|
||||
return false;
|
||||
}
|
||||
const uint8_t len = config_key_size(key);
|
||||
struct ubx_cfg_valset msg {};
|
||||
uint8_t buf[sizeof(msg)+len];
|
||||
if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(buf)+2)) {
|
||||
@ -1413,7 +1676,7 @@ AP_GPS_UBLOX::_configure_valset(ConfigKey key, const uint8_t len, const uint8_t
|
||||
bool
|
||||
AP_GPS_UBLOX::_configure_valget(ConfigKey key)
|
||||
{
|
||||
if (_hardware_generation < UBLOX_F9) {
|
||||
if (!supports_F9_config()) {
|
||||
return false;
|
||||
}
|
||||
struct {
|
||||
@ -1429,6 +1692,35 @@ AP_GPS_UBLOX::_configure_valget(ConfigKey key)
|
||||
return _send_message(CLASS_CFG, MSG_CFG_VALGET, &msg, sizeof(msg));
|
||||
}
|
||||
|
||||
/*
|
||||
* configure F9 based key/value pair for a complete config list
|
||||
*/
|
||||
bool
|
||||
AP_GPS_UBLOX::_configure_config_set(const config_list *list, uint8_t count, uint32_t unconfig_bit)
|
||||
{
|
||||
#if GPS_UBLOX_MOVING_BASELINE
|
||||
active_config.list = list;
|
||||
active_config.count = count;
|
||||
active_config.done_mask = 0;
|
||||
active_config.unconfig_bit = unconfig_bit;
|
||||
|
||||
uint8_t buf[sizeof(ubx_cfg_valget)+count*sizeof(ConfigKey)];
|
||||
struct ubx_cfg_valget msg {};
|
||||
if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(buf)+2)) {
|
||||
return false;
|
||||
}
|
||||
msg.version = 0;
|
||||
msg.layers = 0; // ram
|
||||
memcpy(buf, &msg, sizeof(msg));
|
||||
for (uint8_t i=0; i<count; i++) {
|
||||
memcpy(&buf[sizeof(msg)+i*sizeof(ConfigKey)], &list[i].key, sizeof(ConfigKey));
|
||||
}
|
||||
return _send_message(CLASS_CFG, MSG_CFG_VALGET, buf, sizeof(buf));
|
||||
#else
|
||||
return false;
|
||||
#endif
|
||||
}
|
||||
|
||||
/*
|
||||
* save gps configurations to non-volatile memory sent until the call of
|
||||
* this message
|
||||
@ -1543,7 +1835,8 @@ static const char *reasons[] = {"navigation rate",
|
||||
"PVT rate",
|
||||
"time pulse settings",
|
||||
"TIMEGPS rate",
|
||||
"Time mode settings"};
|
||||
"Time mode settings",
|
||||
"RTK MB"};
|
||||
|
||||
static_assert((1 << ARRAY_SIZE(reasons)) == CONFIG_LAST, "UBLOX: Missing configuration description");
|
||||
|
||||
@ -1584,6 +1877,15 @@ bool AP_GPS_UBLOX::get_lag(float &lag_sec) const
|
||||
// F9 lag not verified yet from flight log, but likely to be at least
|
||||
// as good as M8
|
||||
lag_sec = 0.12f;
|
||||
#if GPS_UBLOX_MOVING_BASELINE
|
||||
if (role == AP_GPS::GPS_ROLE_MB_BASE ||
|
||||
role == AP_GPS::GPS_ROLE_MB_ROVER) {
|
||||
// the moving baseline rover will lag about 40ms from the
|
||||
// base. We need to provide the more conservative value to
|
||||
// ensure that the EKF allocates a larger enough buffer
|
||||
lag_sec += 0.04;
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
};
|
||||
return true;
|
||||
@ -1608,3 +1910,55 @@ void AP_GPS_UBLOX::_check_new_itow(uint32_t itow)
|
||||
{
|
||||
check_new_itow(itow, _payload_length + sizeof(ubx_header) + 2);
|
||||
}
|
||||
|
||||
// support for retrieving RTCMv3 data from a moving baseline base
|
||||
bool AP_GPS_UBLOX::get_RTCMV3(const uint8_t *&bytes, uint16_t &len)
|
||||
{
|
||||
#if GPS_UBLOX_MOVING_BASELINE
|
||||
if (rtcm3_parser) {
|
||||
len = rtcm3_parser->get_len(bytes);
|
||||
return len > 0;
|
||||
}
|
||||
#endif
|
||||
return false;
|
||||
}
|
||||
|
||||
// clear previous RTCM3 packet
|
||||
void AP_GPS_UBLOX::clear_RTCMV3(void)
|
||||
{
|
||||
#if GPS_UBLOX_MOVING_BASELINE
|
||||
if (rtcm3_parser) {
|
||||
rtcm3_parser->clear_packet();
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
// ublox specific healthy checks
|
||||
bool AP_GPS_UBLOX::is_healthy(void) const
|
||||
{
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
if (gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE) {
|
||||
// allow for fake ublox moving baseline
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
#if GPS_UBLOX_MOVING_BASELINE
|
||||
if ((role == AP_GPS::GPS_ROLE_MB_BASE ||
|
||||
role == AP_GPS::GPS_ROLE_MB_ROVER) &&
|
||||
!supports_F9_config()) {
|
||||
// need F9 or above for moving baseline
|
||||
return false;
|
||||
}
|
||||
if (role == AP_GPS::GPS_ROLE_MB_BASE && rtcm3_parser == nullptr && !mb_use_uart2()) {
|
||||
// we haven't initialised RTCMv3 parser
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
|
||||
// return true if GPS is capable of F9 config
|
||||
bool AP_GPS_UBLOX::supports_F9_config(void) const
|
||||
{
|
||||
return _hardware_generation >= UBLOX_F9 && _hardware_generation != UBLOX_UNKNOWN_HARDWARE_GENERATION;
|
||||
}
|
||||
|
||||
@ -39,6 +39,12 @@
|
||||
*/
|
||||
#define UBLOX_SET_BINARY "\265\142\006\001\003\000\001\006\001\022\117$PUBX,41,1,0023,0001,115200,0*1C\r\n"
|
||||
|
||||
// a varient with 230400 baudrate
|
||||
#define UBLOX_SET_BINARY_230400 "\265\142\006\001\003\000\001\006\001\022\117$PUBX,41,1,0023,0001,230400,0*1E\r\n"
|
||||
|
||||
// a varient with 460800 baudrate
|
||||
#define UBLOX_SET_BINARY_460800 "\265\142\006\001\003\000\001\006\001\022\117$PUBX,41,1,0023,0001,460800,0*11\r\n"
|
||||
|
||||
#define UBLOX_RXM_RAW_LOGGING 1
|
||||
#define UBLOX_MAX_RXM_RAW_SATS 22
|
||||
#define UBLOX_MAX_RXM_RAWX_SATS 32
|
||||
@ -78,7 +84,8 @@
|
||||
#define CONFIG_TP5 (1<<14)
|
||||
#define CONFIG_RATE_TIMEGPS (1<<15)
|
||||
#define CONFIG_TMODE_MODE (1<<16)
|
||||
#define CONFIG_LAST (1<<17) // this must always be the last bit
|
||||
#define CONFIG_RTK_MOVBASE (1<<17)
|
||||
#define CONFIG_LAST (1<<18) // this must always be the last bit
|
||||
|
||||
#define CONFIG_REQUIRED_INITIAL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_VELNED)
|
||||
|
||||
@ -96,10 +103,13 @@
|
||||
#define SAVE_CFG_ANT (1<<10)
|
||||
#define SAVE_CFG_ALL (SAVE_CFG_IO|SAVE_CFG_MSG|SAVE_CFG_INF|SAVE_CFG_NAV|SAVE_CFG_RXM|SAVE_CFG_RINV|SAVE_CFG_ANT)
|
||||
|
||||
class RTCM3_Parser;
|
||||
|
||||
class AP_GPS_UBLOX : public AP_GPS_Backend
|
||||
{
|
||||
public:
|
||||
AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
|
||||
AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port, AP_GPS::GPS_Role role);
|
||||
~AP_GPS_UBLOX() override;
|
||||
|
||||
// Methods
|
||||
bool read() override;
|
||||
@ -128,6 +138,13 @@ public:
|
||||
|
||||
const char *name() const override { return "u-blox"; }
|
||||
|
||||
// support for retrieving RTCMv3 data from a moving baseline base
|
||||
bool get_RTCMV3(const uint8_t *&bytes, uint16_t &len) override;
|
||||
void clear_RTCMV3(void) override;
|
||||
|
||||
// ublox specific healthy checks
|
||||
bool is_healthy(void) const override;
|
||||
|
||||
private:
|
||||
// u-blox UBX protocol essentials
|
||||
struct PACKED ubx_header {
|
||||
@ -215,6 +232,43 @@ private:
|
||||
// F9 config keys
|
||||
enum class ConfigKey : uint32_t {
|
||||
TMODE_MODE = 0x20030001,
|
||||
CFG_RATE_MEAS = 0x30210001,
|
||||
|
||||
CFG_UART1_BAUDRATE = 0x40520001,
|
||||
CFG_UART1_ENABLED = 0x10520005,
|
||||
CFG_UART1INPROT_UBX = 0x10730001,
|
||||
CFG_UART1INPROT_NMEA = 0x10730002,
|
||||
CFG_UART1INPROT_RTCM3X = 0x10730004,
|
||||
CFG_UART1OUTPROT_UBX = 0x10740001,
|
||||
CFG_UART1OUTPROT_NMEA = 0x10740002,
|
||||
CFG_UART1OUTPROT_RTCM3X = 0x10740004,
|
||||
|
||||
CFG_UART2_BAUDRATE = 0x40530001,
|
||||
CFG_UART2_ENABLED = 0x10530005,
|
||||
CFG_UART2INPROT_UBX = 0x10750001,
|
||||
CFG_UART2INPROT_NMEA = 0x10750002,
|
||||
CFG_UART2INPROT_RTCM3X = 0x10750004,
|
||||
CFG_UART2OUTPROT_UBX = 0x10760001,
|
||||
CFG_UART2OUTPROT_NMEA = 0x10760002,
|
||||
CFG_UART2OUTPROT_RTCM3X = 0x10760004,
|
||||
|
||||
MSGOUT_RTCM_3X_TYPE4072_0_UART1 = 0x209102ff,
|
||||
MSGOUT_RTCM_3X_TYPE4072_1_UART1 = 0x20910382,
|
||||
MSGOUT_RTCM_3X_TYPE1077_UART1 = 0x209102cd,
|
||||
MSGOUT_RTCM_3X_TYPE1087_UART1 = 0x209102d2,
|
||||
MSGOUT_RTCM_3X_TYPE1097_UART1 = 0x20910319,
|
||||
MSGOUT_RTCM_3X_TYPE1127_UART1 = 0x209102d7,
|
||||
MSGOUT_RTCM_3X_TYPE1230_UART1 = 0x20910304,
|
||||
MSGOUT_UBX_NAV_RELPOSNED_UART1 = 0x2091008e,
|
||||
|
||||
MSGOUT_RTCM_3X_TYPE4072_0_UART2 = 0x20910300,
|
||||
MSGOUT_RTCM_3X_TYPE4072_1_UART2 = 0x20910383,
|
||||
MSGOUT_RTCM_3X_TYPE1077_UART2 = 0x209102ce,
|
||||
MSGOUT_RTCM_3X_TYPE1087_UART2 = 0x209102d3,
|
||||
MSGOUT_RTCM_3X_TYPE1097_UART2 = 0x2091031a,
|
||||
MSGOUT_RTCM_3X_TYPE1127_UART2 = 0x209102d8,
|
||||
MSGOUT_RTCM_3X_TYPE1230_UART2 = 0x20910305,
|
||||
MSGOUT_UBX_NAV_RELPOSNED_UART2 = 0x2091008f,
|
||||
};
|
||||
struct PACKED ubx_cfg_valset {
|
||||
uint8_t version;
|
||||
@ -500,10 +554,12 @@ private:
|
||||
diffSoln = 1U << 1,
|
||||
relPosValid = 1U << 2,
|
||||
carrSolnFloat = 1U << 3,
|
||||
|
||||
carrSolnFixed = 1U << 4,
|
||||
isMoving = 1U << 5,
|
||||
refPosMiss = 1U << 6,
|
||||
refObsMiss = 1U << 7,
|
||||
|
||||
relPosHeadingValid = 1U << 8,
|
||||
relPosNormalized = 1U << 9
|
||||
};
|
||||
@ -596,9 +652,15 @@ private:
|
||||
STEP_RAW,
|
||||
STEP_RAWX,
|
||||
STEP_VERSION,
|
||||
STEP_RTK_MOVBASE, // setup moving baseline
|
||||
STEP_LAST
|
||||
};
|
||||
|
||||
// GPS_DRV_OPTIONS bits
|
||||
enum class DRV_OPTIONS {
|
||||
MB_USE_UART2 = 1U<<0,
|
||||
};
|
||||
|
||||
// Packet checksum accumulators
|
||||
uint8_t _ck_a;
|
||||
uint8_t _ck_b;
|
||||
@ -624,7 +686,12 @@ private:
|
||||
struct ubx_mon_ver _version;
|
||||
uint32_t _unconfigured_messages;
|
||||
uint8_t _hardware_generation;
|
||||
uint32_t _last_pvt_itow;
|
||||
uint32_t _last_relposned_itow;
|
||||
uint32_t _last_relposned_ms;
|
||||
|
||||
// the role set from GPS_TYPE
|
||||
AP_GPS::GPS_Role role;
|
||||
|
||||
// do we have new position information?
|
||||
bool _new_position:1;
|
||||
@ -646,7 +713,7 @@ private:
|
||||
bool havePvtMsg;
|
||||
|
||||
bool _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
|
||||
bool _configure_valset(ConfigKey key, const uint8_t len, const uint8_t *value);
|
||||
bool _configure_valset(ConfigKey key, const void *value);
|
||||
bool _configure_valget(ConfigKey key);
|
||||
void _configure_rate(void);
|
||||
void _configure_sbas(bool enable);
|
||||
@ -671,4 +738,53 @@ private:
|
||||
uint8_t _ubx_msg_log_index(uint8_t ubx_msg) {
|
||||
return (uint8_t)(ubx_msg + (state.instance * UBX_MSG_TYPES));
|
||||
}
|
||||
|
||||
#if GPS_UBLOX_MOVING_BASELINE
|
||||
// see if we should use uart2 for moving baseline config
|
||||
bool mb_use_uart2(void) const {
|
||||
return (driver_options() & unsigned(DRV_OPTIONS::MB_USE_UART2))?true:false;
|
||||
}
|
||||
#endif
|
||||
|
||||
// structure for list of config key/value pairs for
|
||||
// specific configurations
|
||||
struct PACKED config_list {
|
||||
ConfigKey key;
|
||||
// support up to 4 byte values, assumes little-endian
|
||||
uint32_t value;
|
||||
};
|
||||
|
||||
// return size of a config key payload
|
||||
uint8_t config_key_size(ConfigKey key) const;
|
||||
|
||||
// configure a set of config key/value pairs. The unconfig_bit corresponds to
|
||||
// a bit in _unconfigured_messages
|
||||
bool _configure_config_set(const config_list *list, uint8_t count, uint32_t unconfig_bit);
|
||||
|
||||
// find index in active_config list
|
||||
int8_t find_active_config_index(ConfigKey key) const;
|
||||
|
||||
// return true if GPS is capable of F9 config
|
||||
bool supports_F9_config(void) const;
|
||||
|
||||
#if GPS_UBLOX_MOVING_BASELINE
|
||||
// config for moving baseline base
|
||||
static const config_list config_MB_Base_uart1[];
|
||||
static const config_list config_MB_Base_uart2[];
|
||||
|
||||
// config for moving baseline rover
|
||||
static const config_list config_MB_Rover_uart1[];
|
||||
static const config_list config_MB_Rover_uart2[];
|
||||
|
||||
// status of active configuration for a role
|
||||
struct {
|
||||
const config_list *list;
|
||||
uint8_t count;
|
||||
uint32_t done_mask;
|
||||
uint32_t unconfig_bit;
|
||||
} active_config;
|
||||
|
||||
// RTCM3 parser for when in moving baseline base mode
|
||||
RTCM3_Parser *rtcm3_parser;
|
||||
#endif // GPS_UBLOX_MOVING_BASELINE
|
||||
};
|
||||
|
||||
@ -57,6 +57,8 @@ public:
|
||||
|
||||
// driver specific health, returns true if the driver is healthy
|
||||
virtual bool is_healthy(void) const { return true; }
|
||||
// returns true if the GPS is doing any logging it is expected to
|
||||
virtual bool logging_healthy(void) const { return true; }
|
||||
|
||||
virtual const char *name() const = 0;
|
||||
|
||||
@ -65,6 +67,10 @@ public:
|
||||
|
||||
virtual bool prepare_for_arming(void) { return true; }
|
||||
|
||||
// optional support for retrieving RTCMv3 data from a moving baseline base
|
||||
virtual bool get_RTCMV3(const uint8_t *&bytes, uint16_t &len) { return false; }
|
||||
virtual void clear_RTCMV3(void) {};
|
||||
|
||||
protected:
|
||||
AP_HAL::UARTDriver *port; ///< UART we are attached to
|
||||
AP_GPS &gps; ///< access to frontend (for parameters)
|
||||
@ -96,7 +102,14 @@ protected:
|
||||
void set_uart_timestamp(uint16_t nbytes);
|
||||
|
||||
void check_new_itow(uint32_t itow, uint32_t msg_length);
|
||||
|
||||
|
||||
/*
|
||||
access to driver option bits
|
||||
*/
|
||||
uint16_t driver_options(void) const {
|
||||
return uint16_t(gps._driver_options.get());
|
||||
}
|
||||
|
||||
private:
|
||||
// itow from previous message
|
||||
uint32_t _last_itow;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user