Use sample time in Attitude logging

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));
 }

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;

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), \

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;

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;