AP_AHRS: use last sample time for Write_Attitude()

log attitude dt
ArduPilot · Sep 9, 2024 · de6b842 · de6b842
1 parent a64df8f
commit de6b842
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 4 deletions.
diff --git 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 <AP_Logger/AP_Logger.h>
+#include <AP_Scheduler/AP_Scheduler.h>
 
 #include <AC_AttitudeControl/AC_AttitudeControl.h>
 #include <AC_AttitudeControl/AC_PosControl.h>
@@ -53,14 +54,15 @@ 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,
         pitch           : (int16_t)pitch_sensor,
         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,14 +129,15 @@ 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,
         pitch           : (int16_t)pitch_sensor,
         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
@@ -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
@@ -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), \