From 4b2c766857e6620643f3ff527d21ae74582647cb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 20 Jul 2021 17:36:45 +1000 Subject: [PATCH] AP_GPS: change handling of moving baseline yaw this changes yaw handling in a few ways: - GPS yaw now has a timestamp associated with the yaw separate from the timestamp associated with the GPS fix - we no longer force the primary to change to the UBLOX MB rover when it has a GPS yaw. This means we don't change GPS primary due to GPS loss, which keeps the GPS more stable. It also increases accuracy as the rover is always less accurate in position and velocity than the base - now we force the primary to be the MB base if the other GPS is a rover and the base has GPS lock --- libraries/AP_GPS/AP_GPS.cpp | 82 ++++++++++++++++++------------ libraries/AP_GPS/AP_GPS.h | 19 ++----- libraries/AP_GPS/AP_GPS_MAV.cpp | 10 +++- libraries/AP_GPS/AP_GPS_MSP.cpp | 5 +- libraries/AP_GPS/AP_GPS_NMEA.cpp | 1 + libraries/AP_GPS/AP_GPS_UAVCAN.cpp | 3 ++ libraries/AP_GPS/GPS_Backend.cpp | 1 + 7 files changed, 70 insertions(+), 51 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index beb6a4eba1..c35597f099 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -984,45 +984,26 @@ void AP_GPS::update_primary(void) uint32_t now = AP_HAL::millis(); - // 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 + // special handling of RTK moving baseline pair. Always use the + // base as the rover position is derived from the base, which + // means the rover always has worse position and velocity than the + // base. This overrides the normal logic which would select the + // rover as it typically is in fix type 6 (RTK) whereas base is + // usually fix type 3 for (uint8_t i=0; i= GPS_OK_FIX_3D && - _type[i2] == GPS_TYPE_UBLOX_RTK_ROVER && - (state[i2].status != GPS_OK_FIX_3D_RTK_FIXED || - !state[i2].have_gps_yaw)) { + _type[i^1] == GPS_TYPE_UBLOX_RTK_ROVER && + ((state[i].status >= GPS_OK_FIX_3D) || (state[i].status >= state[i^1].status))) { if (primary_instance != i) { _last_instance_swap_ms = now; primary_instance = i; } + // don't do any more switching logic. We don't want the + // RTK status of the rover to cause a switch return; } } - + // handling switching away from blended GPS if (primary_instance == GPS_BLENDED_INSTANCE) { primary_instance = 0; @@ -1195,7 +1176,8 @@ uint16_t AP_GPS::gps_yaw_cdeg(uint8_t instance) const return 0; } float yaw_deg, accuracy_deg; - if (!gps_yaw_deg(instance, yaw_deg, accuracy_deg)) { + uint32_t time_ms; + if (!gps_yaw_deg(instance, yaw_deg, accuracy_deg, time_ms)) { return 65535; } int yaw_cd = wrap_360_cd(yaw_deg * 100); @@ -1944,7 +1926,8 @@ void AP_GPS::Write_GPS(uint8_t i) const struct Location &loc = location(i); float yaw_deg=0, yaw_accuracy_deg=0; - gps_yaw_deg(i, yaw_deg, yaw_accuracy_deg); + uint32_t yaw_time_ms; + gps_yaw_deg(i, yaw_deg, yaw_accuracy_deg, yaw_time_ms); const struct log_GPS pkt { LOG_PACKET_HEADER_INIT(LOG_GPS_MSG), @@ -1987,6 +1970,41 @@ void AP_GPS::Write_GPS(uint8_t i) AP::logger().WriteBlock(&pkt2, sizeof(pkt2)); } +/* + get GPS based yaw + */ +bool AP_GPS::gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg, uint32_t &time_ms) const +{ +#if GPS_MAX_RECEIVERS > 1 + if (instance < GPS_MAX_RECEIVERS && + _type[instance] == GPS_TYPE_UBLOX_RTK_BASE && + _type[instance^1] == GPS_TYPE_UBLOX_RTK_ROVER) { + // return the yaw from the rover + instance ^= 1; + } +#endif + if (!have_gps_yaw(instance)) { + return false; + } + yaw_deg = state[instance].gps_yaw; + + // get lagged timestamp + time_ms = state[instance].gps_yaw_time_ms; + float lag_s; + if (get_lag(instance, lag_s)) { + uint32_t lag_ms = lag_s * 1000; + time_ms -= lag_ms; + } + + if (state[instance].have_gps_yaw_accuracy) { + accuracy_deg = state[instance].gps_yaw_accuracy; + } else { + // fall back to 10 degrees as a generic default + accuracy_deg = 10; + } + return true; +} + namespace AP { AP_GPS &gps() diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 726f99f0e7..495c475123 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -168,6 +168,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) + uint32_t gps_yaw_time_ms; ///< timestamp of last GPS yaw reading 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 @@ -328,21 +329,9 @@ public: } // yaw in degrees if available - bool gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg) const { - if (!have_gps_yaw(instance)) { - return false; - } - yaw_deg = state[instance].gps_yaw; - if (state[instance].have_gps_yaw_accuracy) { - accuracy_deg = state[instance].gps_yaw_accuracy; - } else { - // fall back to 10 degrees as a generic default - accuracy_deg = 10; - } - return true; - } - bool gps_yaw_deg(float &yaw_deg, float &accuracy_deg) const { - return gps_yaw_deg(primary_instance, yaw_deg, accuracy_deg); + bool gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg, uint32_t &time_ms) const; + bool gps_yaw_deg(float &yaw_deg, float &accuracy_deg, uint32_t &time_ms) const { + return gps_yaw_deg(primary_instance, yaw_deg, accuracy_deg, time_ms); } // number of locked satellites diff --git a/libraries/AP_GPS/AP_GPS_MAV.cpp b/libraries/AP_GPS/AP_GPS_MAV.cpp index 5c8b1a1d57..2e49e81be6 100644 --- a/libraries/AP_GPS/AP_GPS_MAV.cpp +++ b/libraries/AP_GPS/AP_GPS_MAV.cpp @@ -104,8 +104,11 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t &msg) state.have_vertical_accuracy = true; } + const uint32_t now_ms = AP_HAL::millis(); + if (have_yaw) { state.gps_yaw = wrap_360(packet.yaw*0.01); + state.gps_yaw_time_ms = now_ms; state.have_gps_yaw = true; state.gps_yaw_configured = true; } @@ -120,12 +123,15 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t &msg) first_week = packet.time_week; } uint32_t timestamp_ms = (packet.time_week - first_week) * AP_MSEC_PER_WEEK + packet.time_week_ms; - uint32_t corrected_ms = jitter.correct_offboard_timestamp_msec(timestamp_ms, AP_HAL::millis()); + uint32_t corrected_ms = jitter.correct_offboard_timestamp_msec(timestamp_ms, now_ms); state.uart_timestamp_ms = corrected_ms; + if (have_yaw) { + state.gps_yaw_time_ms = corrected_ms; + } } state.num_sats = packet.satellites_visible; - state.last_gps_time_ms = AP_HAL::millis(); + state.last_gps_time_ms = now_ms; _new_data = true; break; } diff --git a/libraries/AP_GPS/AP_GPS_MSP.cpp b/libraries/AP_GPS/AP_GPS_MSP.cpp index 7500778dfa..16ce23a5de 100644 --- a/libraries/AP_GPS/AP_GPS_MSP.cpp +++ b/libraries/AP_GPS/AP_GPS_MSP.cpp @@ -77,13 +77,14 @@ void AP_GPS_MSP::handle_msp(const MSP::msp_gps_data_message_t &pkt) state.vertical_accuracy = pkt.vertical_pos_accuracy * 0.01; state.speed_accuracy = pkt.horizontal_vel_accuracy * 0.01; + state.last_gps_time_ms = AP_HAL::millis(); + if (pkt.true_yaw != 65535) { state.gps_yaw = wrap_360(pkt.true_yaw*0.01); state.have_gps_yaw = true; + state.gps_yaw_time_ms = state.last_gps_time_ms; } - state.last_gps_time_ms = AP_HAL::millis(); - new_data = pkt.fix_type>0; } diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 2fa579989a..cf28f8b092 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -339,6 +339,7 @@ bool AP_GPS_NMEA::_term_complete() _last_HDT_THS_ms = now; state.gps_yaw = wrap_360(_new_gps_yaw*0.01f); state.have_gps_yaw = true; + state.gps_yaw_time_ms = AP_HAL::millis(); // 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 diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp index 4465b25a3c..0e5203793e 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp @@ -525,6 +525,9 @@ void AP_GPS_UAVCAN::handle_heading_msg(const HeadingCb &cb) interim_state.have_gps_yaw = cb.msg->heading_valid; interim_state.gps_yaw = degrees(cb.msg->heading_rad); + if (interim_state.have_gps_yaw) { + interim_state.gps_yaw_time_ms = AP_HAL::millis(); + } interim_state.have_gps_yaw_accuracy = cb.msg->heading_accuracy_valid; interim_state.gps_yaw_accuracy = degrees(cb.msg->heading_accuracy_rad); diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 65d00a9c46..0a98ce8fb6 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -384,6 +384,7 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(const float reported_heading_deg, const float rotation_offset_rad = Vector2f(-offset.x, -offset.y).angle(); state.gps_yaw = wrap_360(reported_heading_deg - degrees(rotation_offset_rad)); state.have_gps_yaw = true; + state.gps_yaw_time_ms = AP_HAL::millis(); } }