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(); } }