aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: move RTDT logging to fast path

Browse Source 
log after motor output in fast rate thread
This commit is contained in:
Andy Piper 2024-10-25 13:04:08 +09:00 committed by Andrew Tridgell
parent 8fc4a6a6e9
commit 3d43f2053f
7 changed files with 62 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/Copter.cpp
View File

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

1
ArduCopter/Copter.h
View File

@ -922,6 +922,7 @@ private:
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

38
ArduCopter/Log.cpp
View File

@ -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

4
ArduCopter/Parameters.cpp
View File

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

3
ArduCopter/defines.h
View File

@ -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)

5
ArduCopter/motors.cpp
View File

@ -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 AP_COPTER_ADVANCED_FAILSAFE_ENABLED
@ -184,13 +185,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.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) {

36
ArduCopter/rate_thread.cpp
View File

@ -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;