Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AutoTune: allow compilation with HAL_LOGGING_ENABLED false

This commit is contained in:
Peter Barker 2023-07-14 10:58:04 +10:00 committed by Andrew Tridgell
parent 65baf8abc7
commit 2eede45f3a
6 changed files with 46 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

@ -75,7 +75,7 @@ bool AC_AutoTune::init_internals(bool _use_poshold,
level_start_time_ms = step_start_time_ms;
// reset gains to tuning-start gains (i.e. low I term)
load_gains(GAIN_INTRA_TEST);
AP::logger().Write_Event(LogEvent::AUTOTUNE_RESTART);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_RESTART);
update_gcs(AUTOTUNE_MESSAGE_STARTED);
break;
@ -83,7 +83,7 @@ bool AC_AutoTune::init_internals(bool _use_poshold,
// we have completed a tune and the pilot wishes to test the new gains
load_gains(GAIN_TUNED);
update_gcs(AUTOTUNE_MESSAGE_TESTING);
AP::logger().Write_Event(LogEvent::AUTOTUNE_PILOT_TESTING);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_PILOT_TESTING);
break;
}
@ -102,7 +102,8 @@ void AC_AutoTune::stop()
attitude_control->use_sqrt_controller(true);
update_gcs(AUTOTUNE_MESSAGE_STOPPED);
AP::logger().Write_Event(LogEvent::AUTOTUNE_OFF);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_OFF);
// Note: we leave the mode as it was so that we know how the autotune ended
// we expect the caller will change the flight mode back to the flight mode indicated by the flight mode switch
@ -400,10 +401,12 @@ void AC_AutoTune::control_attitude()
step = WAITING_FOR_LEVEL;
}
#if HAL_LOGGING_ENABLED
// log this iterations lean angle and rotation rate
Log_AutoTuneDetails();
ahrs_view->Write_Rate(*motors, *attitude_control, *pos_control);
log_pids();
#endif
break;
}
@ -412,8 +415,10 @@ void AC_AutoTune::control_attitude()
// re-enable rate limits
attitude_control->use_sqrt_controller(true);
#if HAL_LOGGING_ENABLED
// log the latest gains
Log_AutoTune();
#endif
// Announce tune type test results
// must be done before updating method because this method changes parameters for next test
@ -519,7 +524,7 @@ void AC_AutoTune::control_attitude()
if (complete) {
mode = SUCCESS;
update_gcs(AUTOTUNE_MESSAGE_SUCCESS);
AP::logger().Write_Event(LogEvent::AUTOTUNE_SUCCESS);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_SUCCESS);
AP_Notify::events.autotune_complete = true;
} else {
AP_Notify::events.autotune_next_axis = true;

5
libraries/AC_AutoTune/AC_AutoTune.h
View File

@ -92,8 +92,10 @@ protected:
// init pos controller Z velocity and accel limits
virtual void init_z_limits() = 0;
#if HAL_LOGGING_ENABLED
// log PIDs at full rate for during twitch
virtual void log_pids() = 0;
#endif
//
// methods to load and save gains
@ -151,9 +153,12 @@ protected:
// reverse direction for twitch test
virtual bool twitch_reverse_direction() = 0;
#if HAL_LOGGING_ENABLED
virtual void Log_AutoTune() = 0;
virtual void Log_AutoTuneDetails() = 0;
virtual void Log_AutoTuneSweep() = 0;
#endif
// internal init function, should be called from init()
bool init_internals(bool use_poshold,

20
libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View File

@ -250,12 +250,12 @@ void AC_AutoTune_Heli::test_run(AxisType test_axis, const float dir_sign)
if ((tune_type == RP_UP || tune_type == RD_UP) && (max_rate_p.max_allowed <= 0.0f || max_rate_d.max_allowed <= 0.0f)) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Max Gain Determination Failed");
mode = FAILED;
AP::logger().Write_Event(LogEvent::AUTOTUNE_FAILED);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED);
update_gcs(AUTOTUNE_MESSAGE_FAILED);
} else if ((tune_type == MAX_GAINS || tune_type == RP_UP || tune_type == RD_UP || tune_type == SP_UP) && exceeded_freq_range(start_freq)){
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Exceeded frequency range");
mode = FAILED;
AP::logger().Write_Event(LogEvent::AUTOTUNE_FAILED);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED);
update_gcs(AUTOTUNE_MESSAGE_FAILED);
} else if (tune_type == TUNE_COMPLETE) {
counter = AUTOTUNE_SUCCESS_COUNT;
@ -442,7 +442,7 @@ void AC_AutoTune_Heli::backup_gains_and_initialise()
tune_yaw_sp = attitude_control->get_angle_yaw_p().kP();
tune_yaw_accel = attitude_control->get_accel_yaw_max_cdss();
AP::logger().Write_Event(LogEvent::AUTOTUNE_INITIALISED);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_INITIALISED);
}
// load_orig_gains - set gains to their original values
@ -660,7 +660,7 @@ void AC_AutoTune_Heli::save_tuning_gains()
// update GCS and log save gains event
update_gcs(AUTOTUNE_MESSAGE_SAVED_GAINS);
AP::logger().Write_Event(LogEvent::AUTOTUNE_SAVEDGAINS);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_SAVEDGAINS);
reset();
}
@ -1166,8 +1166,10 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq,
if (dwell_type == DRB) {test_accel_max = freqresp.get_accel_max();}
// reset cycle_complete to allow indication of next cycle
freqresp.reset_cycle_complete();
#if HAL_LOGGING_ENABLED
// log sweep data
Log_AutoTuneSweep();
#endif
} else {
dwell_gain = freqresp.get_gain();
dwell_phase = freqresp.get_phase();
@ -1383,14 +1385,14 @@ void AC_AutoTune_Heli::updating_rate_ff_up(float &tune_ff, float rate_target, fl
if (tune_ff <= AUTOTUNE_RFF_MIN) {
tune_ff = AUTOTUNE_RFF_MIN;
counter = AUTOTUNE_SUCCESS_COUNT;
AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
}
} else if (is_positive(rate_target * meas_rate) && fabsf(meas_rate) < 0.95f * fabsf(rate_target)) {
tune_ff = 1.02f * tune_ff;
if (tune_ff >= AUTOTUNE_RFF_MAX) {
tune_ff = AUTOTUNE_RFF_MAX;
counter = AUTOTUNE_SUCCESS_COUNT;
AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
}
} else {
if (!is_zero(meas_rate)) {
@ -1547,7 +1549,7 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, float *freq, float *ga
// exceeded max response gain at the minimum allowable tuning gain. terminate testing.
tune_p = AUTOTUNE_SP_MIN;
counter = AUTOTUNE_SUCCESS_COUNT;
AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
} else if (gain[freq_cnt] > gain[freq_cnt_max]) {
freq_cnt_max = freq_cnt;
phase_max = phase[freq_cnt];
@ -1579,7 +1581,7 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, float *freq, float *ga
if (tune_p >= AUTOTUNE_SP_MAX) {
tune_p = AUTOTUNE_SP_MAX;
counter = AUTOTUNE_SUCCESS_COUNT;
AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
}
}
curr_test.freq = freq[freq_cnt];
@ -1722,6 +1724,7 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase
}
#if HAL_LOGGING_ENABLED
// log autotune summary data
void AC_AutoTune_Heli::Log_AutoTune()
{
@ -1840,6 +1843,7 @@ void AC_AutoTune_Heli::Log_Write_AutoTuneSweep(float freq, float gain, float pha
gain,
phase);
}
#endif // HAL_LOGGING_ENABLED
// reset the test variables for each vehicle
void AC_AutoTune_Heli::reset_vehicle_test_variables()

2
libraries/AC_AutoTune/AC_AutoTune_Heli.h
View File

@ -103,6 +103,7 @@ protected:
// reverse direction for twitch test
bool twitch_reverse_direction() override { return positive_direction; }
#if HAL_LOGGING_ENABLED
// methods to log autotune summary data
void Log_AutoTune() override;
void Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, float dwell_freq, float meas_gain, float meas_phase, float new_gain_rff, float new_gain_rp, float new_gain_rd, float new_gain_sp, float max_accel);
@ -114,6 +115,7 @@ protected:
// methods to log autotune frequency response results
void Log_AutoTuneSweep() override;
void Log_Write_AutoTuneSweep(float freq, float gain, float phase);
#endif
// send intermittent updates to user on status of tune
void do_gcs_announcements() override;

30
libraries/AC_AutoTune/AC_AutoTune_Multi.cpp
View File

@ -192,7 +192,7 @@ void AC_AutoTune_Multi::backup_gains_and_initialise()
tune_yaw_sp = attitude_control->get_angle_yaw_p().kP();
tune_yaw_accel = attitude_control->get_accel_yaw_max_cdss();
AP::logger().Write_Event(LogEvent::AUTOTUNE_INITIALISED);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_INITIALISED);
}
// load_orig_gains - set gains to their original values
@ -464,7 +464,7 @@ void AC_AutoTune_Multi::save_tuning_gains()
// update GCS and log save gains event
update_gcs(AUTOTUNE_MESSAGE_SAVED_GAINS);
AP::logger().Write_Event(LogEvent::AUTOTUNE_SAVEDGAINS);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_SAVEDGAINS);
reset();
}
@ -796,7 +796,7 @@ void AC_AutoTune_Multi::updating_rate_d_up(float &tune_d, float tune_d_min, floa
// We have reached minimum D gain so stop tuning
tune_d = tune_d_min;
counter = AUTOTUNE_SUCCESS_COUNT;
AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
}
}
} else if ((meas_rate_max < rate_target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (tune_p <= tune_p_max)) {
@ -805,7 +805,7 @@ void AC_AutoTune_Multi::updating_rate_d_up(float &tune_d, float tune_d_min, floa
tune_p += tune_p*tune_p_step_ratio;
if (tune_p >= tune_p_max) {
tune_p = tune_p_max;
AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
}
} else {
// we have a good measurement of bounce back
@ -826,7 +826,7 @@ void AC_AutoTune_Multi::updating_rate_d_up(float &tune_d, float tune_d_min, floa
if (tune_d >= tune_d_max) {
tune_d = tune_d_max;
counter = AUTOTUNE_SUCCESS_COUNT;
AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
}
} else {
ignore_next = false;
@ -851,7 +851,7 @@ void AC_AutoTune_Multi::updating_rate_d_down(float &tune_d, float tune_d_min, fl
// We have reached minimum D so stop tuning
tune_d = tune_d_min;
counter = AUTOTUNE_SUCCESS_COUNT;
AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
}
}
} else if ((meas_rate_max < rate_target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (tune_p <= tune_p_max)) {
@ -860,7 +860,7 @@ void AC_AutoTune_Multi::updating_rate_d_down(float &tune_d, float tune_d_min, fl
tune_p += tune_p*tune_p_step_ratio;
if (tune_p >= tune_p_max) {
tune_p = tune_p_max;
AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
}
} else {
// we have a good measurement of bounce back
@ -884,7 +884,7 @@ void AC_AutoTune_Multi::updating_rate_d_down(float &tune_d, float tune_d_min, fl
if (tune_d <= tune_d_min) {
tune_d = tune_d_min;
counter = AUTOTUNE_SUCCESS_COUNT;
AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
}
}
}
@ -909,14 +909,14 @@ void AC_AutoTune_Multi::updating_rate_p_up_d_down(float &tune_d, float tune_d_mi
// do not decrease the D term past the minimum
if (tune_d <= tune_d_min) {
tune_d = tune_d_min;
AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
}
// decrease P gain to match D gain reduction
tune_p -= tune_p*tune_p_step_ratio;
// do not decrease the P term past the minimum
if (tune_p <= tune_p_min) {
tune_p = tune_p_min;
AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
}
// cancel change in direction
positive_direction = !positive_direction;
@ -932,7 +932,7 @@ void AC_AutoTune_Multi::updating_rate_p_up_d_down(float &tune_d, float tune_d_mi
if (tune_p >= tune_p_max) {
tune_p = tune_p_max;
counter = AUTOTUNE_SUCCESS_COUNT;
AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
}
} else {
ignore_next = false;
@ -964,8 +964,8 @@ void AC_AutoTune_Multi::updating_angle_p_down(float &tune_p, float tune_p_min, f
if (tune_p <= tune_p_min) {
tune_p = tune_p_min;
counter = AUTOTUNE_SUCCESS_COUNT;
AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT);
}
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
}
}
}
@ -991,7 +991,7 @@ void AC_AutoTune_Multi::updating_angle_p_up(float &tune_p, float tune_p_max, flo
if (tune_p >= tune_p_max) {
tune_p = tune_p_max;
counter = AUTOTUNE_SUCCESS_COUNT;
AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT);
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
}
} else {
ignore_next = false;
@ -999,6 +999,7 @@ void AC_AutoTune_Multi::updating_angle_p_up(float &tune_p, float tune_p_max, flo
}
}
#if HAL_LOGGING_ENABLED
void AC_AutoTune_Multi::Log_AutoTune()
{
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
@ -1093,6 +1094,7 @@ void AC_AutoTune_Multi::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds
angle_cd*0.01f,
rate_cds*0.01f);
}
#endif // HAL_LOGGING_ENABLED
void AC_AutoTune_Multi::twitch_test_init()
{

2
libraries/AC_AutoTune/AC_AutoTune_Multi.h
View File

@ -108,6 +108,7 @@ protected:
// reverse direction for twitch test
bool twitch_reverse_direction() override { return !positive_direction; }
#if HAL_LOGGING_ENABLED
void Log_AutoTune() override;
void Log_AutoTuneDetails() override;
void Log_AutoTuneSweep() override {
@ -116,6 +117,7 @@ protected:
}
void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt);
void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds);
#endif
void set_tune_sequence() override {
tune_seq[0] = RD_UP;