AP_AHRS: use last sample time for Write_Attitude()

log attitude dt
ArduPilot · Sep 5, 2024 · fd466ef · fd466ef
1 parent c6b57ae
commit fd466ef
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,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,
@@ -63,6 +63,7 @@ void AP_AHRS::Write_Attitude(const Vector3f &targets) const
         error_rp        : (uint16_t)(get_error_rp() * 100),
         error_yaw       : (uint16_t)(get_error_yaw() * 100),
         active          : uint8_t(active_EKF_type()),
+        sensor_dt       : dt
     };
     AP::logger().WriteBlock(&pkt, sizeof(pkt));
 }
@@ -125,11 +126,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,
@@ -139,6 +140,7 @@ void AP_AHRS_View::Write_AttitudeView(const Vector3f &targets) const
         error_rp        : (uint16_t)(get_error_rp() * 100),
         error_yaw       : (uint16_t)(get_error_yaw() * 100),
         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,13 +53,14 @@ 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: ErrRP: lowest estimated gyro drift error
 // @Field: ErrYaw: difference between measured yaw and DCM yaw estimate
 // @Field: AEKF: active EKF type
+// @Field: Dt: attitude delta time
 struct PACKED log_Attitude {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -72,6 +73,7 @@ struct PACKED log_Attitude {
     uint16_t error_rp;
     uint16_t error_yaw;
     uint8_t  active;
+    float sensor_dt;
 };
 
 // @LoggerMessage: ORGN
@@ -199,7 +201,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", "QccccCCCCB", "TimeUS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,ErrRP,ErrYaw,AEKF", "sddddhhdh-", "FBBBBBBBB-" , true }, \
+        "ATT", "QccccCCCCBf", "TimeUS,DesRoll,Roll,DesPit,Pit,DesYaw,Yaw,ErrRP,ErrYaw,AEKF,Dt", "sddddhhdh-s", "FBBBBBBBB-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), \