From 9d3fbf1b265bc3d9b364079145e93795179d68db Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Fri, 8 Dec 2023 16:01:35 +1100 Subject: [PATCH] AP_GPS: add support for calculating gps message lag over DroneCAN --- libraries/AP_GPS/AP_GPS_DroneCAN.cpp | 41 ++++++++++++++++++++++++++++ libraries/AP_GPS/AP_GPS_DroneCAN.h | 17 ++++++++++++ libraries/AP_GPS/GPS_Backend.cpp | 23 ++++++++++------ libraries/AP_GPS/GPS_Backend.h | 3 ++ 4 files changed, 76 insertions(+), 8 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp index 1f4d7e2a5f8958..2e342525a1ef97 100644 --- a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp @@ -340,6 +340,45 @@ void AP_GPS_DroneCAN::handle_velocity(const float vx, const float vy, const floa } } +void AP_GPS_DroneCAN::update_fix_lag(uint64_t gnss_timestamp_usec) +{ + if (timesync_correction) { + // calculate precise lag + uint64_t current_time = timesync_correction_us + AP_HAL::micros64() + NATIVE_TIME_OFFSET; + _fix_lag = (current_time - gnss_timestamp_usec) / 1000000.0f; + last_gnss_timestamp_usec = gnss_timestamp_usec; + fix_lag_valid = true; + } +} + +void AP_GPS_DroneCAN::update_relposned_lag() +{ + if (timesync_correction && (last_gnss_timestamp_usec != 0)) { + // calculate precise lag + uint64_t current_time = timesync_correction_us + AP_HAL::micros64() + NATIVE_TIME_OFFSET; + _relposned_lag = (current_time - last_gnss_timestamp_usec) / 1000000.0f; + relposned_lag_valid = true; + } +} + +bool AP_GPS_DroneCAN::get_fix_lag(float &lag) const +{ + if (fix_lag_valid) { + lag = _fix_lag; + return true; + } + return AP_GPS_Backend::get_lag(lag); +} + +bool AP_GPS_DroneCAN::get_relposned_lag(float &lag) const +{ + if (relposned_lag_valid) { + lag = _relposned_lag; + return true; + } + return get_fix_lag(lag); +} + void AP_GPS_DroneCAN::handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uint64_t timestamp_usec) { bool process = false; @@ -419,6 +458,7 @@ void AP_GPS_DroneCAN::handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uin } interim_state.num_sats = msg.sats_used; + update_fix_lag(msg.gnss_timestamp.usec); } else { interim_state.have_vertical_velocity = false; interim_state.have_vertical_accuracy = false; @@ -572,6 +612,7 @@ void AP_GPS_DroneCAN::handle_relposheading_msg(const ardupilot_gnss_RelPosHeadin interim_state.gps_yaw_configured = true; seen_relposheading = true; // push raw heading data to calculate moving baseline heading states + update_relposned_lag(); if (calculate_moving_base_yaw(interim_state, msg.reported_heading_deg, msg.relative_distance_m, diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.h b/libraries/AP_GPS/AP_GPS_DroneCAN.h index 9c09b474efe3d5..817f77bd452e61 100644 --- a/libraries/AP_GPS/AP_GPS_DroneCAN.h +++ b/libraries/AP_GPS/AP_GPS_DroneCAN.h @@ -90,6 +90,16 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend { // returns true once configuration has finished bool do_config(void); + void update_fix_lag(uint64_t gnss_timestamp_usec); + void update_relposned_lag(); + + bool get_lag(float &lag) const override { + return get_fix_lag(lag); + } + + bool get_fix_lag(float &lag) const; + bool get_relposned_lag(float &lag) const override; + void handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uint64_t timestamp_usec); void handle_aux_msg(const uavcan_equipment_gnss_Auxiliary& msg); void handle_heading_msg(const ardupilot_gnss_Heading& msg); @@ -105,6 +115,13 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend { static void give_registry(); static AP_GPS_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id); + float _fix_lag; + bool fix_lag_valid; + uint64_t last_gnss_timestamp_usec; + + float _relposned_lag; + bool relposned_lag_valid; + bool _new_data; AP_GPS::GPS_State interim_state; diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 095bb27366d7a4..6ba81933e168b3 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -328,6 +328,8 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state, bool selectedOffset = false; Vector3f offset; + Vector3f antenna_tilt {}; + float lag = 0.1; switch (MovingBase::Type(gps.mb_params[interim_state.instance].type.get())) { case MovingBase::Type::RelativeToAlternateInstance: offset = gps._antenna_offset[interim_state.instance^1].get() - gps._antenna_offset[interim_state.instance].get(); @@ -373,8 +375,7 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state, #if AP_AHRS_ENABLED { // get lag - float lag = 0.1; - get_lag(lag); + get_relposned_lag(lag); // get vehicle rotation, projected back in time using the gyro // this is not 100% accurate, but it is good enough for @@ -388,7 +389,7 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state, rot_body_to_ned.rotate(gyro * (-lag)); // apply rotation to the offset to get the Z offset in NED - const Vector3f antenna_tilt = rot_body_to_ned * offset; + antenna_tilt = rot_body_to_ned * offset; const float alt_error = reported_D + antenna_tilt.z; if (fabsf(alt_error) > permitted_error_length_pct * min_dist) { @@ -398,6 +399,8 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state, goto bad_yaw; } } +#else + (void)lag; #endif // AP_AHRS_ENABLED { @@ -424,17 +427,21 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state, // @Field: RHD: reported heading,deg // @Field: RDist: antenna separation,m // @Field: RDown: vertical antenna separation,m + // @Field: CRDown: corrected vertical antenna separation,m // @Field: OK: 1 if have yaw - AP::logger().WriteStreaming("GPYW", "TimeUS,Id,RHD,RDist,RDown,OK", - "s#dmm-", - "F-----", - "QBfffB", + // @Field: lag: lag in actual information,sec + AP::logger().WriteStreaming("GPYW", "TimeUS,Id,RHD,RDist,RDown,CRDown,OK,lag,fixlag", + "s#dmmm-ss", + "F--------", + "QBffffBff", AP_HAL::micros64(), state.instance, reported_heading_deg, reported_distance, reported_D, - interim_state.have_gps_yaw); + antenna_tilt.z, + interim_state.have_gps_yaw, + lag); #endif return interim_state.have_gps_yaw; diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index f69125f145147a..f4d0b51f6b4f42 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -79,6 +79,9 @@ class AP_GPS_Backend // driver specific lag, returns true if the driver is confident in the provided lag virtual bool get_lag(float &lag) const { lag = 0.2f; return true; } + // driver specific lag for relposned, returns true if the driver is confident in the provided lag + virtual bool get_relposned_lag(float &lag) const { return get_lag(lag); } + // 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