From 394b89a7d59f6c41e36645ee5dc95c273683d5f7 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 4 Sep 2024 09:53:42 +0100 Subject: [PATCH] AP_AHRS: use last sample time for Write_Attitude() log attitude dt --- libraries/AP_AHRS/AP_AHRS.h | 2 +- libraries/AP_AHRS/AP_AHRS_Logging.cpp | 10 ++++++---- libraries/AP_AHRS/AP_AHRS_View.h | 2 +- libraries/AP_AHRS/LogStructure.h | 8 +++++--- 4 files changed, 13 insertions(+), 9 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 98cd09fefceba6..29310cb6c0ad71 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -503,7 +503,7 @@ class AP_AHRS { // Logging functions void Log_Write_Home_And_Origin(); - void Write_Attitude(const Vector3f &targets) const; + void Write_Attitude(const Vector3f &targets, uint64_t sample_time_us, float dt) const; enum class LogOriginType { ekf_origin = 0, diff --git a/libraries/AP_AHRS/AP_AHRS_Logging.cpp b/libraries/AP_AHRS/AP_AHRS_Logging.cpp index 46fd26f49fe951..024acd4d355b02 100644 --- a/libraries/AP_AHRS/AP_AHRS_Logging.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Logging.cpp @@ -49,11 +49,11 @@ void AP_AHRS::Write_AOA_SSA(void) const } // Write an attitude packet -void AP_AHRS::Write_Attitude(const Vector3f &targets) const +void AP_AHRS::Write_Attitude(const Vector3f &targets, uint64_t sample_time_us, float dt) const { const struct log_Attitude pkt{ LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG), - time_us : AP_HAL::micros64(), + time_us : sample_time_us, control_roll : (int16_t)targets.x, roll : (int16_t)roll_sensor, control_pitch : (int16_t)targets.y, @@ -61,6 +61,7 @@ void AP_AHRS::Write_Attitude(const Vector3f &targets) const control_yaw : (uint16_t)wrap_360_cd(targets.z), yaw : (uint16_t)wrap_360_cd(yaw_sensor), active : uint8_t(active_EKF_type()), + sensor_dt : dt }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } @@ -123,11 +124,11 @@ void AP_AHRS::write_video_stabilisation() const } // Write an attitude view packet -void AP_AHRS_View::Write_AttitudeView(const Vector3f &targets) const +void AP_AHRS_View::Write_AttitudeView(const Vector3f &targets, uint64_t sample_time_us, float dt) const { const struct log_Attitude pkt{ LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG), - time_us : AP_HAL::micros64(), + time_us : sample_time_us, control_roll : (int16_t)targets.x, roll : (int16_t)roll_sensor, control_pitch : (int16_t)targets.y, @@ -135,6 +136,7 @@ void AP_AHRS_View::Write_AttitudeView(const Vector3f &targets) const control_yaw : (uint16_t)wrap_360_cd(targets.z), yaw : (uint16_t)wrap_360_cd(yaw_sensor), active : uint8_t(AP::ahrs().active_EKF_type()), + sensor_dt : dt }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } diff --git a/libraries/AP_AHRS/AP_AHRS_View.h b/libraries/AP_AHRS/AP_AHRS_View.h index ff0f2293c16510..4a5fc851a65e30 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.h +++ b/libraries/AP_AHRS/AP_AHRS_View.h @@ -173,7 +173,7 @@ class AP_AHRS_View } // Logging Functions - void Write_AttitudeView(const Vector3f &targets) const; + void Write_AttitudeView(const Vector3f &targets, uint64_t sample_time_us, float dt) const; void Write_Rate(const class AP_Motors &motors, const class AC_AttitudeControl &attitude_control, const AC_PosControl &pos_control) const; diff --git a/libraries/AP_AHRS/LogStructure.h b/libraries/AP_AHRS/LogStructure.h index 9769eee06bab94..904245c2a3f575 100644 --- a/libraries/AP_AHRS/LogStructure.h +++ b/libraries/AP_AHRS/LogStructure.h @@ -53,11 +53,12 @@ struct PACKED log_AOA_SSA { // @Field: TimeUS: Time since system startup // @Field: DesRoll: vehicle desired roll // @Field: Roll: achieved vehicle roll -// @Field: DesPitch: vehicle desired pitch -// @Field: Pitch: achieved vehicle pitch +// @Field: DesPit: vehicle desired pitch +// @Field: Pit: achieved vehicle pitch // @Field: DesYaw: vehicle desired yaw // @Field: Yaw: achieved vehicle yaw // @Field: AEKF: active EKF type +// @Field: Dt: attitude delta time struct PACKED log_Attitude { LOG_PACKET_HEADER; uint64_t time_us; @@ -68,6 +69,7 @@ struct PACKED log_Attitude { uint16_t control_yaw; uint16_t yaw; uint8_t active; + float sensor_dt; }; // @LoggerMessage: ORGN @@ -195,7 +197,7 @@ struct PACKED log_ATSC { { LOG_AOA_SSA_MSG, sizeof(log_AOA_SSA), \ "AOA", "Qff", "TimeUS,AOA,SSA", "sdd", "F00" , true }, \ { LOG_ATTITUDE_MSG, sizeof(log_Attitude),\ - "ATT", "QccccCCB", "TimeUS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,AEKF", "sddddhh-", "FBBBBBB-" , true }, \ + "ATT", "QccccCCBf", "TimeUS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,AEKF,Dt", "sddddhh-s", "FBBBBBB-0" , true }, \ { LOG_ORGN_MSG, sizeof(log_ORGN), \ "ORGN","QBLLe","TimeUS,Type,Lat,Lng,Alt", "s#DUm", "F-GGB" }, \ { LOG_POS_MSG, sizeof(log_POS), \