diff --git a/libraries/AP_AHRS/AP_AHRS_Logging.cpp b/libraries/AP_AHRS/AP_AHRS_Logging.cpp index 46fd26f49fe951..6347dfb3515fe6 100644 --- a/libraries/AP_AHRS/AP_AHRS_Logging.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Logging.cpp @@ -4,6 +4,7 @@ #include "AP_AHRS.h" #include +#include #include #include @@ -53,7 +54,7 @@ void AP_AHRS::Write_Attitude(const Vector3f &targets) const { const struct log_Attitude pkt{ LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG), - time_us : AP_HAL::micros64(), + time_us : AP::scheduler().get_loop_start_time_us(), control_roll : (int16_t)targets.x, roll : (int16_t)roll_sensor, control_pitch : (int16_t)targets.y, @@ -61,6 +62,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 : AP::scheduler().get_last_loop_time_s() }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } @@ -127,7 +129,7 @@ void AP_AHRS_View::Write_AttitudeView(const Vector3f &targets) const { const struct log_Attitude pkt{ LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG), - time_us : AP_HAL::micros64(), + time_us : AP::scheduler().get_loop_start_time_us(), control_roll : (int16_t)targets.x, roll : (int16_t)roll_sensor, control_pitch : (int16_t)targets.y, @@ -135,6 +137,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 : AP::scheduler().get_last_loop_time_s() }; 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..18d921391169f4 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) 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), \