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

AC_AutoTune: adding comments to code

This commit is contained in:
Bill Geyer 2021-12-05 23:27:42 -05:00 committed by Bill Geyer
parent 1f6424c781
commit 2d3228c699
2 changed files with 53 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View File

@ -553,6 +553,7 @@ void AC_AutoTune_Heli::updating_rate_ff_up(float &tune_ff, float rate_target, fl
}
}
// updating_rate_p_up - uses maximum allowable gain determined from max_gain test to determine rate p gain that does not exceed exceed max response gain
void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_p)
{
float test_freq_incr = 0.25f * 3.14159f * 2.0f;
@ -631,6 +632,7 @@ void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, float *freq, float *gai
}
}
// updating_rate_d_up - uses maximum allowable gain determined from max_gain test to determine rate d gain where the response gain is at a minimum
void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_d)
{
float test_freq_incr = 0.25f * 3.14159f * 2.0f;
@ -727,6 +729,7 @@ void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, float *freq, float *gai
}
}
// updating_angle_p_up - determines maximum angle p gain for pitch and roll
void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt)
{
float test_freq_incr = 0.5f * 3.14159f * 2.0f;
@ -829,6 +832,7 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, float *freq, float *ga
}
}
// updating_angle_p_up_yaw - determines maximum angle p gain for yaw
void AC_AutoTune_Heli::updating_angle_p_up_yaw(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt)
{
float test_freq_incr = 0.5f * 3.14159f * 2.0f;
@ -1002,6 +1006,7 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase
}
}
// log autotune summary data
void AC_AutoTune_Heli::Log_AutoTune()
{
switch (axis) {
@ -1018,6 +1023,7 @@ void AC_AutoTune_Heli::Log_AutoTune()
// }
}
// log autotune time history results for command, angular rate, and attitude
void AC_AutoTune_Heli::Log_AutoTuneDetails()
{
if (tune_type == SP_UP) {
@ -1027,6 +1033,7 @@ void AC_AutoTune_Heli::Log_AutoTuneDetails()
}
}
// log autotune frequency response data
void AC_AutoTune_Heli::Log_AutoTuneSweep()
{
Log_Write_AutoTuneSweep(curr_test_freq, curr_test_gain, curr_test_phase);
@ -1047,7 +1054,7 @@ void AC_AutoTune_Heli::Log_AutoTuneSweep()
// @Field: SP: new angle P term
// @Field: ACC: new max accel term
// Write an Autotune data packet
// Write an Autotune summary data packet
void AC_AutoTune_Heli::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)
{
AP::logger().Write(
@ -1069,7 +1076,7 @@ void AC_AutoTune_Heli::Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, floa
max_accel);
}
// Write an Autotune data packet
// Write an Autotune detailed data packet
void AC_AutoTune_Heli::Log_Write_AutoTuneDetails(float motor_cmd, float tgt_rate_rads, float rate_rads, float tgt_ang_rad, float ang_rad)
{
// @LoggerMessage: ATDH
@ -1095,7 +1102,7 @@ void AC_AutoTune_Heli::Log_Write_AutoTuneDetails(float motor_cmd, float tgt_rate
ang_rad*57.3f);
}
// Write an Autotune data packet
// Write an Autotune frequency response data packet
void AC_AutoTune_Heli::Log_Write_AutoTuneSweep(float freq, float gain, float phase)
{
// @LoggerMessage: ATSH
@ -1171,6 +1178,7 @@ float AC_AutoTune_Heli::get_yaw_rate_filt_min() const
return AUTOTUNE_RLPF_MIN;
}
// set the tuning test sequence
void AC_AutoTune_Heli::set_tune_sequence()
{
uint8_t seq_cnt = 0;

44
libraries/AC_AutoTune/AC_AutoTune_Heli.h
View File

@ -45,7 +45,10 @@ protected:
// get tuned yaw rate d gain
float get_tuned_yaw_rd() override { return tune_yaw_rd; }
// initializes test
void test_init() override;
// runs test
void test_run(AxisType test_axis, const float dir_sign) override;
// update gains for the rate p up tune type
@ -87,12 +90,18 @@ protected:
// reverse direction for twitch test
bool twitch_reverse_direction() override { return positive_direction; }
// methods to log autotune summary data
void Log_AutoTune() override;
void Log_AutoTuneDetails() override;
void Log_AutoTuneSweep() 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);
// methods to log autotune time history results for command, angular rate, and attitude.
void Log_AutoTuneDetails() override;
void Log_Write_AutoTuneDetails(float motor_cmd, float tgt_rate_rads, float rate_rads, float tgt_ang_rad, float ang_rad);
// methods to log autotune frequency response results
void Log_AutoTuneSweep() override;
void Log_Write_AutoTuneSweep(float freq, float gain, float phase);
// returns true if rate P gain of zero is acceptable for this vehicle
bool allow_zero_rate_p() override { return true; }
@ -102,48 +111,79 @@ protected:
// send intermittant updates to user on status of tune
void do_gcs_announcements() override;
// set the tuning test sequence
void set_tune_sequence() override;
// tuning sequence bitmask
AP_Int8 seq_bitmask;
// minimum sweep frequency
AP_Float min_sweep_freq;
// maximum sweep frequency
AP_Float max_sweep_freq;
// maximum response gain
AP_Float max_resp_gain;
private:
// updating_rate_ff_up - adjust FF to ensure the target is reached
// FF is adjusted until rate requested is acheived
void updating_rate_ff_up(float &tune_ff, float rate_target, float meas_rate, float meas_command);
// updating_rate_p_up - uses maximum allowable gain determined from max_gain test to determine rate p gain that does not exceed exceed max response gain
void updating_rate_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_p);
// updating_rate_d_up - uses maximum allowable gain determined from max_gain test to determine rate d gain where the response gain is at a minimum
void updating_rate_d_up(float &tune_d, float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_d);
// updating_angle_p_up - determines maximum angle p gain for pitch and roll
void updating_angle_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt);
// updating_angle_p_up_yaw - determines maximum angle p gain for yaw
void updating_angle_p_up_yaw(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt);
// updating_max_gains: use dwells at increasing frequency to determine gain at which instability will occur
void updating_max_gains(float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_p, max_gain_data &max_gain_d, float &tune_p, float &tune_d);
uint8_t method; //0: determine freq, 1: use max gain method, 2: use phase 180 method
// updating rate FF variables
// flag for completion of the initial direction for the feedforward test
bool first_dir_complete;
// feedforward gain resulting from testing in the initial direction
float first_dir_rff;
// updating max gain variables
// flag for finding maximum p gain
bool found_max_p;
// flag for finding maximum d gain
bool found_max_d;
// flag for interpolating to find max response gain
bool find_middle;
// updating angle P up variables
// track the maximum phase
float phase_max;
// previous gain
float sp_prev_gain;
// flag for finding the peak of the gain response
bool find_peak;
// updating angle P up yaw
// counter value of previous good frequency
uint8_t sp_prev_good_frq_cnt;
// updating rate P up
// counter value of previous good frequency
uint8_t rp_prev_good_frq_cnt;
// previous gain
float rp_prev_gain;
// updating rate D up
// counter value of previous good frequency
uint8_t rd_prev_good_frq_cnt;
// previous gain
float rd_prev_gain;
};