Copter: move RTDT logging to fast path

log after motor output in fast rate thread

ArduCopter/Copter.cpp

@@ -261,6 +261,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
     SCHED_TASK_CLASS(AP_Button,            &copter.button,              update,           5, 100, 168),
 #endif
 #if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
+    // don't delete this, there is an equivalent (virtual) in AP_Vehicle for the non-rate loop case
     SCHED_TASK(update_dynamic_notch_at_specified_rate_main,                       LOOP_RATE, 200, 215),
 #endif
 };

ArduCopter/Copter.h

@@ -919,6 +919,7 @@ class Copter : public AP_Vehicle {
     void Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out);
     void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z);
     void Log_Write_Vehicle_Startup_Messages();
+    void Log_Write_Rate_Thread_Dt(float dt, float dtAvg, float dtMax, float dtMin);
 #endif  // HAL_LOGGING_ENABLED
 
     // mode.cpp

ArduCopter/Log.cpp

@@ -1,4 +1,5 @@
 #include "Copter.h"
+#include <AP_InertialSensor/AP_InertialSensor_rate_config.h>
 
 #if HAL_LOGGING_ENABLED
 
@@ -343,6 +344,16 @@ struct PACKED log_Guided_Attitude_Target {
     float climb_rate;
 };
 
+// rate thread dt stats
+struct PACKED log_Rate_Thread_Dt {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    float dt;
+    float dtAvg;
+    float dtMax;
+    float dtMin;
+};
+
 // Write a Guided mode position target
 // pos_target is lat, lon, alt OR offset from ekf origin in cm
 // terrain should be 0 if pos_target.z is alt-above-ekf-origin, 1 if alt-above-terrain
@@ -390,6 +401,21 @@ void Copter::Log_Write_Guided_Attitude_Target(ModeGuided::SubMode target_type, f
     logger.WriteBlock(&pkt, sizeof(pkt));
 }
 
+void Copter::Log_Write_Rate_Thread_Dt(float dt, float dtAvg, float dtMax, float dtMin)
+{
+#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
+    const log_Rate_Thread_Dt pkt {
+        LOG_PACKET_HEADER_INIT(LOG_RATE_THREAD_DT_MSG),
+        time_us         : AP_HAL::micros64(),
+        dt              : dt,
+        dtAvg           : dtAvg,
+        dtMax           : dtMax,
+        dtMin           : dtMin
+    };
+    logger.WriteBlock(&pkt, sizeof(pkt));
+#endif
+}
+
 // type and unit information can be found in
 // libraries/AP_Logger/Logstructure.h; search for "log_Units" for
 // units and "Format characters" for field type information
@@ -531,6 +557,18 @@ const struct LogStructure Copter::log_structure[] = {
 
     { LOG_GUIDED_ATTITUDE_TARGET_MSG, sizeof(log_Guided_Attitude_Target),
       "GUIA",  "QBffffffff",    "TimeUS,Type,Roll,Pitch,Yaw,RollRt,PitchRt,YawRt,Thrust,ClimbRt", "s-dddkkk-n", "F-000000-0" , true },
+
+// @LoggerMessage: RTDT
+// @Description: Attitude controller time deltas
+// @Field: TimeUS: Time since system startup
+// @Field: dt: current time delta
+// @Field: dtAvg: current time delta average
+// @Field: dtMax: Max time delta since last log output
+// @Field: dtMin: Min time delta since last log output
+
+    { LOG_RATE_THREAD_DT_MSG, sizeof(log_Rate_Thread_Dt),
+      "RTDT", "Qffff", "TimeUS,dt,dtAvg,dtMax,dtMin", "sssss", "F----" , true },
+
 };
 
 uint8_t Copter::get_num_log_structures() const

ArduCopter/Parameters.cpp

@@ -1236,14 +1236,14 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = {
 #if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
     // @Param: FSTRATE_ENABLE
     // @DisplayName: Enable the fast Rate thread
-    // @Description: Enable the fast Rate thread
+    // @Description: Enable the fast Rate thread. In the default case the fast rate divisor, which controls the update frequency of the thread, is dynamically scaled from FSTRATE_DIV to avoid overrun in the gyro sample buffer and main loop slow-downs. Other values can be selected to fix the divisor to FSTRATE_DIV on arming or always.
     // @User: Advanced
     // @Values: 0:Disabled,1:Enabled-Dynamic,2:Enabled-FixedWhenArmed,3:Enabled-Fixed
     AP_GROUPINFO("FSTRATE_ENABLE", 9, ParametersG2, att_enable, 0),
 
     // @Param: FSTRATE_DIV
     // @DisplayName: Fast rate thread divisor
-    // @Description: Fast rate thread divisor used to control the maximum fast rate update rate. The actual rate is the gyro rate divided by this value.
+    // @Description: Fast rate thread divisor used to control the maximum fast rate update rate. The actual rate is the gyro rate in Hz divided by this value. This value is scaled depending on the configuration of FSTRATE_ENABLE.
     // @User: Advanced
     // @Range: 1 10
     AP_GROUPINFO("FSTRATE_DIV", 10, ParametersG2, att_decimation, 1),

ArduCopter/defines.h

@@ -87,7 +87,8 @@ enum LoggingParameters {
      LOG_GUIDED_POSITION_TARGET_MSG,
      LOG_SYSIDD_MSG,
      LOG_SYSIDS_MSG,
-     LOG_GUIDED_ATTITUDE_TARGET_MSG
+     LOG_GUIDED_ATTITUDE_TARGET_MSG,
+     LOG_RATE_THREAD_DT_MSG
 };
 
 #define MASK_LOG_ATTITUDE_FAST          (1<<0)

ArduCopter/motors.cpp

@@ -133,6 +133,7 @@ void Copter::auto_disarm_check()
 }
 
 // motors_output - send output to motors library which will adjust and send to ESCs and servos
+// full_push is true when slower rate updates (e.g. servo output) need to be performed at the main loop rate.
 void Copter::motors_output(bool full_push)
 {
 #if ADVANCED_FAILSAFE
@@ -182,13 +183,15 @@ void Copter::motors_output(bool full_push)
 
     // push all channels
     if (full_push) {
+        // motor output including servos and other updates that need to run at the main loop rate
         SRV_Channels::push();
     } else {
+        // motor output only at main loop rate or faster
         hal.rcout->push();
     }
 }
 
-// motors_output from main thread
+// motors_output from main thread at main loop rate
 void Copter::motors_output_main()
 {
     if (!using_rate_thread) {

ArduCopter/rate_thread.cpp

@@ -248,28 +248,6 @@ void Copter::rate_controller_thread()
         const float dt = dt_us * 1.0e-6;
         last_run_us = now_us;
 
-        max_dt = MAX(dt, max_dt);
-        min_dt = MIN(dt, min_dt);
-
-#if HAL_LOGGING_ENABLED
-// @LoggerMessage: RTDT
-// @Description: Attitude controller time deltas
-// @Field: TimeUS: Time since system startup
-// @Field: dt: current time delta
-// @Field: dtAvg: current time delta average
-// @Field: dtMax: Max time delta since last log output
-// @Field: dtMin: Min time delta since last log output
-
-        if (now_ms - last_rtdt_log_ms >= 10) {    // 100 Hz
-            AP::logger().WriteStreaming("RTDT", "TimeUS,dt,dtAvg,dtMax,dtMin", "Qffff",
-                                                AP_HAL::micros64(),
-                                                dt, sensor_dt, max_dt, min_dt);
-            max_dt = sensor_dt;
-            min_dt = sensor_dt;
-            last_rtdt_log_ms = now_ms;
-        }
-#endif
-
         motors->set_dt(sensor_dt);
         // check if we are falling behind
         if (ins.get_num_gyro_samples() > 2) {
@@ -304,6 +282,18 @@ void Copter::rate_controller_thread()
             rate_controller_filter_update();
         }
 
+        max_dt = MAX(dt, max_dt);
+        min_dt = MIN(dt, min_dt);
+
+#if HAL_LOGGING_ENABLED
+        if (now_ms - last_rtdt_log_ms >= 100) {    // 10 Hz
+            Log_Write_Rate_Thread_Dt(dt, sensor_dt, max_dt, min_dt);
+            max_dt = sensor_dt;
+            min_dt = sensor_dt;
+            last_rtdt_log_ms = now_ms;
+        }
+#endif
+
 #ifdef RATE_LOOP_TIMING_DEBUG
         motor_output_us += AP_HAL::micros() - rate_now_us;
         rate_now_us = AP_HAL::micros();
@@ -453,6 +443,7 @@ uint8_t Copter::rate_controller_set_rates(uint8_t rate_decimation, RateControlle
     return 0;
 }
 
+// enable the fast rate thread using the provided decimation rate and record the new output rates
 void Copter::enable_fast_rate_loop(uint8_t rate_decimation, RateControllerRates& rates)
 {
     ins.enable_fast_rate_buffer();
@@ -461,6 +452,7 @@ void Copter::enable_fast_rate_loop(uint8_t rate_decimation, RateControllerRates&
     using_rate_thread = true;
 }
 
+// disable the fast rate thread and record the new output rates
 void Copter::disable_fast_rate_loop(RateControllerRates& rates)
 {
     using_rate_thread = false;