From 3397ed766d40e5092b41424834b21383743e6aff Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 3 Apr 2020 17:52:42 +1100 Subject: [PATCH] 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 --- libraries/AP_GPS/AP_GPS.h | 4 +++ libraries/AP_GPS/AP_GPS_UBLOX.cpp | 42 ++++++++++++++++++++++++++++--- libraries/AP_GPS/AP_GPS_UBLOX.h | 4 +++ 3 files changed, 46 insertions(+), 4 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 589f1d436e..74590719fa 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -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 diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index baa9379d35..7ed9452347 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -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(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; } diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index bc866936cf..5c8c3f5e4f 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -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 };