mirror of https://github.com/ArduPilot/ardupilot
AP_GPS: enable ublox moving baseline compilation option
disable for HAL_MINIMIZE_FEATURES and if max receivers 1. This fixes the f103-GPS AP_Periph build
This commit is contained in:
Andrew Tridgell
parent
ae2132d861
commit
3397ed766d
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -85,6 +85,7 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART
|
|||
_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) {
|
||||
|
|
@ -96,15 +97,19 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART
|
|||
_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
|
||||
|
|
@ -216,6 +221,7 @@ const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_MB_Rover_uart2[] {
|
|||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART1, 0},
|
||||
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART1, 0},
|
||||
};
|
||||
#endif // GPS_UBLOX_MOVING_BASELINE
|
||||
|
||||
|
||||
void
|
||||
|
|
@ -367,6 +373,7 @@ AP_GPS_UBLOX::_request_next_config(void)
|
|||
}
|
||||
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");
|
||||
|
|
@ -387,6 +394,7 @@ AP_GPS_UBLOX::_request_next_config(void)
|
|||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
default:
|
||||
// this case should never be reached, do a full reset if it is hit
|
||||
|
|
@ -580,6 +588,7 @@ 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
|
||||
|
|
@ -591,6 +600,7 @@ AP_GPS_UBLOX::read(void)
|
|||
break;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
reset:
|
||||
switch(_step) {
|
||||
|
|
@ -688,10 +698,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;
|
||||
}
|
||||
|
|
@ -860,6 +872,7 @@ uint8_t AP_GPS_UBLOX::config_key_size(ConfigKey key) const
|
|||
*/
|
||||
int8_t AP_GPS_UBLOX::find_active_config_index(ConfigKey key) const
|
||||
{
|
||||
#if GPS_UBLOX_MOVING_BASELINE
|
||||
if (active_config.list == nullptr) {
|
||||
return -1;
|
||||
}
|
||||
|
|
@ -868,6 +881,7 @@ int8_t AP_GPS_UBLOX::find_active_config_index(ConfigKey key) const
|
|||
return (int8_t)i;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
|
@ -1124,6 +1138,7 @@ 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) {
|
||||
|
|
@ -1142,6 +1157,7 @@ AP_GPS_UBLOX::_parse_gps(void)
|
|||
}
|
||||
}
|
||||
}
|
||||
#endif // GPS_UBLOX_MOVING_BASELINE
|
||||
|
||||
// step over the value
|
||||
uint8_t step_size = config_key_size(id);
|
||||
|
|
@ -1334,13 +1350,20 @@ AP_GPS_UBLOX::_parse_gps(void)
|
|||
static_cast<uint32_t>(RELPOSNED::carrSolnFloat);
|
||||
|
||||
const Vector3f antenna_offset = offset0 - offset1;
|
||||
const Vector3f antenna_tilt = AP::ahrs().get_rotation_body_to_ned() * antenna_offset;
|
||||
const float offset_dist = antenna_offset.length();
|
||||
const float rel_dist = _buffer.relposned.relPosLength * 0.01;
|
||||
const float alt_error = _buffer.relposned.relPosD*0.01 + antenna_tilt.z;
|
||||
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
|
||||
|
|
@ -1359,13 +1382,12 @@ AP_GPS_UBLOX::_parse_gps(void)
|
|||
unsigned(_buffer.relposned.flags),
|
||||
unsigned(_buffer.relposned.iTOW));
|
||||
|
||||
const float min_dist = MIN(offset_dist, rel_dist);
|
||||
if (((_buffer.relposned.flags & valid_mask) == valid_mask) &&
|
||||
((_buffer.relposned.flags & invalid_mask) == 0) &&
|
||||
rel_dist > min_separation &&
|
||||
offset_dist > min_separation &&
|
||||
fabsf(dist_error) < strict_length_error_allowed * min_dist &&
|
||||
fabsf(alt_error) < strict_length_error_allowed * min_dist) {
|
||||
tilt_ok) {
|
||||
float rotation_offset_rad;
|
||||
const Vector3f diff = offset1 - offset0;
|
||||
rotation_offset_rad = Vector2f(diff.x, diff.y).angle();
|
||||
|
|
@ -1692,6 +1714,7 @@ AP_GPS_UBLOX::_configure_valget(ConfigKey key)
|
|||
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;
|
||||
|
|
@ -1709,6 +1732,9 @@ AP_GPS_UBLOX::_configure_config_set(const config_list *list, uint8_t count, uint
|
|||
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
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -1867,6 +1893,7 @@ 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
|
||||
|
|
@ -1874,6 +1901,7 @@ bool AP_GPS_UBLOX::get_lag(float &lag_sec) const
|
|||
// ensure that the EKF allocates a larger enough buffer
|
||||
lag_sec += 0.04;
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
};
|
||||
return true;
|
||||
|
|
@ -1902,19 +1930,23 @@ void AP_GPS_UBLOX::_check_new_itow(uint32_t itow)
|
|||
// 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
|
||||
|
|
@ -1926,6 +1958,7 @@ bool AP_GPS_UBLOX::is_healthy(void) const
|
|||
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()) {
|
||||
|
|
@ -1936,6 +1969,7 @@ bool AP_GPS_UBLOX::is_healthy(void) const
|
|||
// we haven't initialised RTCMv3 parser
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -739,10 +739,12 @@ private:
|
|||
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
|
||||
|
|
@ -765,6 +767,7 @@ private:
|
|||
// 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[];
|
||||
|
|
@ -783,4 +786,5 @@ private:
|
|||
|
||||
// RTCM3 parser for when in moving baseline base mode
|
||||
RTCM3_Parser *rtcm3_parser;
|
||||
#endif // GPS_UBLOX_MOVING_BASELINE
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue