diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 5d2df84ad48f8..755d699a62a34 100644 --- a/ArduCopter/Copter.cpp +++ b/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 }; diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 99e3615076a43..a5e0a2613ddb6 100644 --- a/ArduCopter/Copter.h +++ b/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 diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index b446904e1ab05..2e06ecf80ffdd 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -1,4 +1,5 @@ #include "Copter.h" +#include #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 diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 893ea45542190..df594406a9e30 100644 --- a/ArduCopter/Parameters.cpp +++ b/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), diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index acba81d953960..6603c1846e922 100644 --- a/ArduCopter/defines.h +++ b/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) diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 341c2fbe36b4a..0e856d73fb346 100644 --- a/ArduCopter/motors.cpp +++ b/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) { diff --git a/ArduCopter/rate_thread.cpp b/ArduCopter/rate_thread.cpp index ef8b624c54dd1..ac533165e3b1d 100644 --- a/ArduCopter/rate_thread.cpp +++ b/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;