From 4c27e07d06756ce392a84f56591111be300ae051 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 11 May 2020 13:59:44 +1000 Subject: [PATCH] AP_GPS: sync with master for new yaw handling --- libraries/AP_GPS/AP_GPS.cpp | 366 +++++++++++++++--------- libraries/AP_GPS/AP_GPS.h | 63 +++- libraries/AP_GPS/AP_GPS_NMEA.cpp | 5 + libraries/AP_GPS/AP_GPS_NOVA.cpp | 9 + libraries/AP_GPS/AP_GPS_NOVA.h | 9 +- libraries/AP_GPS/AP_GPS_SBF.cpp | 25 +- libraries/AP_GPS/AP_GPS_SBF.h | 2 + libraries/AP_GPS/AP_GPS_SBP2.cpp | 2 +- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 460 ++++++++++++++++++++++++++---- libraries/AP_GPS/AP_GPS_UBLOX.h | 122 +++++++- libraries/AP_GPS/GPS_Backend.h | 15 +- 11 files changed, 863 insertions(+), 215 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index bbb58285f0..a82b4d249b 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -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= 1) { - // handling switching away from blended GPS - if (primary_instance == GPS_BLENDED_INSTANCE) { - primary_instance = 0; - for (uint8_t i=1; i 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 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 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 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 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 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; iis_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() diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 50481ef234..6550c7cbaf 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 @@ -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 { diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 5e0631ca2c..d61f5aaa10 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -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 { diff --git a/libraries/AP_GPS/AP_GPS_NOVA.cpp b/libraries/AP_GPS/AP_GPS_NOVA.cpp index c2e9b249c2..21e6dcfa7f 100644 --- a/libraries/AP_GPS/AP_GPS_NOVA.cpp +++ b/libraries/AP_GPS/AP_GPS_NOVA.cpp @@ -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 diff --git a/libraries/AP_GPS/AP_GPS_NOVA.h b/libraries/AP_GPS/AP_GPS_NOVA.h index 4c0212508a..023fb4d849 100644 --- a/libraries/AP_GPS/AP_GPS_NOVA.h +++ b/libraries/AP_GPS/AP_GPS_NOVA.h @@ -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; diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 30d6c046a9..782a79e60f 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -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); } } diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h index e98f3ad83e..1eb306faeb 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.h +++ b/libraries/AP_GPS/AP_GPS_SBF.h @@ -47,6 +47,8 @@ public: bool is_healthy(void) const override; + bool logging_healthy(void) const override; + bool prepare_for_arming(void) override; diff --git a/libraries/AP_GPS/AP_GPS_SBP2.cpp b/libraries/AP_GPS/AP_GPS_SBP2.cpp index ba28557939..f3da3dee92 100644 --- a/libraries/AP_GPS/AP_GPS_SBP2.cpp +++ b/libraries/AP_GPS/AP_GPS_SBP2.cpp @@ -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 ...) diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 5965eebf4b..61284edc3a 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -23,6 +23,8 @@ #include #include #include +#include "RTCM3_Parser.h" +#include #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= 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<> 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(RELPOSNED::refObsMiss) | static_cast(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; iget_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; +} diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 15f7ab4eb1..5c8c3f5e4f 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -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 }; diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 973d7611aa..edf2534c5d 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -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;