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..8f48b85346dd4e 100644 --- a/libraries/AP_AHRS/LogStructure.h +++ b/libraries/AP_AHRS/LogStructure.h @@ -58,6 +58,7 @@ struct PACKED log_AOA_SSA { // @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), \