diff --git a/libraries/AP_AHRS/AP_AHRS_Logging.cpp b/libraries/AP_AHRS/AP_AHRS_Logging.cpp index 46fd26f49fe95..6347dfb3515fe 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 ff0f2293c1651..18d921391169f 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 9769eee06bab9..8f48b85346dd4 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), \ diff --git a/libraries/AP_Scheduler/AP_Scheduler.cpp b/libraries/AP_Scheduler/AP_Scheduler.cpp index 7d21cf015a9b8..57f938a616b84 100644 --- a/libraries/AP_Scheduler/AP_Scheduler.cpp +++ b/libraries/AP_Scheduler/AP_Scheduler.cpp @@ -347,7 +347,8 @@ void AP_Scheduler::loop() _rsem.take_blocking(); hal.util->persistent_data.scheduler_task = -1; - const uint32_t sample_time_us = AP_HAL::micros(); + _loop_sample_time_us = AP_HAL::micros64(); + const uint32_t sample_time_us = uint32_t(_loop_sample_time_us); if (_loop_timer_start_us == 0) { _loop_timer_start_us = sample_time_us; diff --git a/libraries/AP_Scheduler/AP_Scheduler.h b/libraries/AP_Scheduler/AP_Scheduler.h index bf15592d495bc..baf5534d62d2c 100644 --- a/libraries/AP_Scheduler/AP_Scheduler.h +++ b/libraries/AP_Scheduler/AP_Scheduler.h @@ -176,6 +176,11 @@ class AP_Scheduler return _last_loop_time_s; } + // get the time in microseconds that the current loop started + uint64_t get_loop_start_time_us(void) const { + return _loop_sample_time_us; + } + // get the amount of extra time being added on each loop uint32_t get_extra_loop_us(void) const { return extra_loop_us; @@ -240,12 +245,15 @@ class AP_Scheduler // number of ticks that _spare_micros is counted over uint8_t _spare_ticks; - // start of loop timing + // start of previous loop uint32_t _loop_timer_start_us; // time of last loop in seconds float _last_loop_time_s; - + + // start of current loop + uint64_t _loop_sample_time_us; + // bitmask bit which indicates if we should log PERF message uint32_t _log_performance_bit;