AP_AHRS: use last sample time for Write_Attitude()

log attitude dt
ArduPilot · Sep 9, 2024 · 394b89a · 394b89a
1 parent 637d3e3
commit 394b89a
Show file tree

Hide file tree

Showing 4 changed files with 13 additions and 9 deletions.
diff --git 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
@@ -49,18 +49,19 @@ 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,
         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       : dt
     };
     AP::logger().WriteBlock(&pkt, sizeof(pkt));
 }
@@ -123,18 +124,19 @@ 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,
         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       : 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
@@ -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
@@ -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), \