zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: autotune check for vel overshoot in angle P 

this change reduces the chance of over tuned angle P
mission-4.1.18
Leonard Hall 7 years ago committed by Randy Mackay
parent
64be134f7b
commit
83fac326a4
2 changed files with 174 additions and 116 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 25
      ArduCopter/mode.h
  2. 265
      ArduCopter/mode_autotune.cpp

25
ArduCopter/mode.h
Unescape View File

@ -373,13 +373,14 @@ private: @@ -373,13 +373,14 @@ private:
bool roll_enabled();
bool pitch_enabled();
bool yaw_enabled();
void twitching_test(float measurement, float target, float &measurement_min, float &measurement_max);
void updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max);
void updating_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max);
void updating_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float target, float measurement_max);
void updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float target, float measurement_max);
void updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max);
void twitching_test_rate(float rate, float rate_target, float &meas_rate_min, float &meas_rate_max);
void twitching_test_angle(float angle, float rate, float angle_target, float &meas_angle_min, float &meas_angle_max, float &meas_rate_min, float &meas_rate_max);
void twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max);
void updating_rate_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max);
void updating_rate_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max);
void updating_rate_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max);
void updating_angle_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max);
void updating_angle_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max);
void get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd);
#if LOGGING_ENABLED == ENABLED
@ -450,17 +451,19 @@ private: @@ -450,17 +451,19 @@ private:
// variables
uint32_t override_time; // the last time the pilot overrode the controls
float test_min; // the minimum angular rate achieved during TESTING_RATE step
float test_max; // the maximum angular rate achieved during TESTING_RATE step
float test_rate_min; // the minimum angular rate achieved during TESTING_RATE step
float test_rate_max; // the maximum angular rate achieved during TESTING_RATE step
float test_angle_min; // the minimum angle achieved during TESTING_ANGLE step
float test_angle_max; // the maximum angle achieved during TESTING_ANGLE step
uint32_t step_start_time; // start time of current tuning step (used for timeout checks)
uint32_t step_stop_time; // start time of current tuning step (used for timeout checks)
int8_t counter; // counter for tuning gains
float target_rate, start_rate; // target and start rate
float target_angle, start_angle; // target and start angles
float target_rate, start_rate; // target and start rate
float target_angle, start_angle; // target and start angles
float desired_yaw; // yaw heading during tune
float rate_max, test_accel_max; // maximum acceleration variables
LowPassFilterFloat rotation_rate_filt; // filtered rotation rate in radians/second
LowPassFilterFloat rotation_rate_filt; // filtered rotation rate in radians/second
// backup of currently being tuned parameter values
float orig_roll_rp = 0, orig_roll_ri, orig_roll_rd, orig_roll_sp, orig_roll_accel;

265
ArduCopter/mode_autotune.cpp
Unescape View File

@ -73,7 +73,7 @@ @@ -73,7 +73,7 @@
// roll and pitch axes
#define AUTOTUNE_TARGET_ANGLE_RLLPIT_CD 2000 // target angle during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_RATE_RLLPIT_CDS 9000 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step
#define AUTOTUNE_TARGET_RATE_RLLPIT_CDS 18000 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step
#define AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD 1000 // minimum target angle during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS 4500 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step
@ -499,8 +499,10 @@ void Copter::ModeAutoTune::autotune_attitude_control() @@ -499,8 +499,10 @@ void Copter::ModeAutoTune::autotune_attitude_control()
step_start_time = millis();
step_stop_time = step_start_time + AUTOTUNE_TESTING_STEP_TIMEOUT_MS;
twitch_first_iter = true;
test_max = 0.0f;
test_min = 0.0f;
test_rate_max = 0.0f;
test_rate_min = 0.0f;
test_angle_max = 0.0f;
test_angle_min = 0.0f;
rotation_rate_filt.reset(0.0f);
rate_max = 0.0f;
// set gains to their to-be-tested values
@ -628,14 +630,14 @@ void Copter::ModeAutoTune::autotune_attitude_control() @@ -628,14 +630,14 @@ void Copter::ModeAutoTune::autotune_attitude_control()
switch (tune_type) {
case RD_UP:
case RD_DOWN:
twitching_test(rotation_rate, target_rate, test_min, test_max);
twitching_test_rate(rotation_rate, target_rate, test_rate_min, test_rate_max);
twitching_measure_acceleration(test_accel_max, rotation_rate, rate_max);
if (lean_angle >= target_angle) {
step = UPDATE_GAINS;
}
break;
case RP_UP:
twitching_test(rotation_rate, target_rate*(1+0.5f*g.autotune_aggressiveness), test_min, test_max);
twitching_test_rate(rotation_rate, target_rate*(1+0.5f*g.autotune_aggressiveness), test_rate_min, test_rate_max);
twitching_measure_acceleration(test_accel_max, rotation_rate, rate_max);
if (lean_angle >= target_angle) {
step = UPDATE_GAINS;
@ -643,7 +645,7 @@ void Copter::ModeAutoTune::autotune_attitude_control() @@ -643,7 +645,7 @@ void Copter::ModeAutoTune::autotune_attitude_control()
break;
case SP_DOWN:
case SP_UP:
twitching_test(lean_angle, target_angle*(1+0.5f*g.autotune_aggressiveness), test_min, test_max);
twitching_test_angle(lean_angle, rotation_rate, target_angle*(1+0.5f*g.autotune_aggressiveness), test_angle_min, test_angle_max, test_rate_min, test_rate_max);
twitching_measure_acceleration(test_accel_max, rotation_rate - direction_sign * start_rate, rate_max);
break;
}
@ -665,25 +667,25 @@ void Copter::ModeAutoTune::autotune_attitude_control() @@ -665,25 +667,25 @@ void Copter::ModeAutoTune::autotune_attitude_control()
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
switch (axis) {
case ROLL:
Log_Write_AutoTune(axis, tune_type, target_angle, test_min, test_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max);
Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max);
break;
case PITCH:
Log_Write_AutoTune(axis, tune_type, target_angle, test_min, test_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max);
Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max);
break;
case YAW:
Log_Write_AutoTune(axis, tune_type, target_angle, test_min, test_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max);
Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max);
break;
}
} else {
switch (axis) {
case ROLL:
Log_Write_AutoTune(axis, tune_type, target_rate, test_min, test_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max);
Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max);
break;
case PITCH:
Log_Write_AutoTune(axis, tune_type, target_rate, test_min, test_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max);
Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max);
break;
case YAW:
Log_Write_AutoTune(axis, tune_type, target_rate, test_min, test_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max);
Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max);
break;
}
}
@ -694,13 +696,13 @@ void Copter::ModeAutoTune::autotune_attitude_control() @@ -694,13 +696,13 @@ void Copter::ModeAutoTune::autotune_attitude_control()
case RD_UP:
switch (axis) {
case ROLL:
updating_d_up(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max);
updating_rate_d_up(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
break;
case PITCH:
updating_d_up(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max);
updating_rate_d_up(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
break;
case YAW:
updating_d_up(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RLPF_MAX, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max);
updating_rate_d_up(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RLPF_MAX, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
break;
}
break;
@ -708,13 +710,13 @@ void Copter::ModeAutoTune::autotune_attitude_control() @@ -708,13 +710,13 @@ void Copter::ModeAutoTune::autotune_attitude_control()
case RD_DOWN:
switch (axis) {
case ROLL:
updating_d_down(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max);
updating_rate_d_down(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
break;
case PITCH:
updating_d_down(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max);
updating_rate_d_down(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
break;
case YAW:
updating_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max);
updating_rate_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
break;
}
break;
@ -722,13 +724,13 @@ void Copter::ModeAutoTune::autotune_attitude_control() @@ -722,13 +724,13 @@ void Copter::ModeAutoTune::autotune_attitude_control()
case RP_UP:
switch (axis) {
case ROLL:
updating_p_up_d_down(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max);
updating_rate_p_up_d_down(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
break;
case PITCH:
updating_p_up_d_down(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max);
updating_rate_p_up_d_down(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
break;
case YAW:
updating_p_up_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max);
updating_rate_p_up_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
break;
}
break;
@ -736,13 +738,13 @@ void Copter::ModeAutoTune::autotune_attitude_control() @@ -736,13 +738,13 @@ void Copter::ModeAutoTune::autotune_attitude_control()
case SP_DOWN:
switch (axis) {
case ROLL:
updating_p_down(tune_roll_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_max);
updating_angle_p_down(tune_roll_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max);
break;
case PITCH:
updating_p_down(tune_pitch_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_max);
updating_angle_p_down(tune_pitch_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max);
break;
case YAW:
updating_p_down(tune_yaw_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_max);
updating_angle_p_down(tune_yaw_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max);
break;
}
break;
@ -750,13 +752,13 @@ void Copter::ModeAutoTune::autotune_attitude_control() @@ -750,13 +752,13 @@ void Copter::ModeAutoTune::autotune_attitude_control()
case SP_UP:
switch (axis) {
case ROLL:
updating_p_up(tune_roll_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_max);
updating_angle_p_up(tune_roll_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max);
break;
case PITCH:
updating_p_up(tune_pitch_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_max);
updating_angle_p_up(tune_pitch_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max);
break;
case YAW:
updating_p_up(tune_yaw_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_max);
updating_angle_p_up(tune_yaw_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max);
break;
}
break;
@ -1182,37 +1184,37 @@ inline bool Copter::ModeAutoTune::yaw_enabled() { @@ -1182,37 +1184,37 @@ inline bool Copter::ModeAutoTune::yaw_enabled() {
return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_YAW;
}
// twitching_test - twitching tests
// twitching_test_rate - twitching tests
// update min and max and test for end conditions
void Copter::ModeAutoTune::twitching_test(float measurement, float target, float &measurement_min, float &measurement_max)
void Copter::ModeAutoTune::twitching_test_rate(float rate, float rate_target_max, float &meas_rate_min, float &meas_rate_max)
{
// capture maximum measurement
if (measurement > measurement_max) {
// capture maximum rate
if (rate > meas_rate_max) {
// the measurement is continuing to increase without stopping
measurement_max = measurement;
measurement_min = measurement;
meas_rate_max = rate;
meas_rate_min = rate;
}
// capture minimum measurement after the measurement has peaked (aka "bounce back")
if ((measurement < measurement_min) && (measurement_max > target * 0.5f)) {
if ((rate < meas_rate_min) && (meas_rate_max > rate_target_max * 0.5f)) {
// the measurement is bouncing back
measurement_min = measurement;
meas_rate_min = rate;
}
// calculate early stopping time based on the time it takes to get to 90%
if (measurement_max < target * 0.75f) {
if (meas_rate_max < rate_target_max * 0.75f) {
// the measurement not reached the 90% threshold yet
step_stop_time = step_start_time + (millis() - step_start_time) * 3.0f;
step_stop_time = MIN(step_stop_time, step_start_time + AUTOTUNE_TESTING_STEP_TIMEOUT_MS);
}
if (measurement_max > target) {
// the measurement has passed the target
if (meas_rate_max > rate_target_max) {
// the measured rate has passed the maximum target rate
step = UPDATE_GAINS;
}
if (measurement_max-measurement_min > measurement_max*g.autotune_aggressiveness) {
// the measurement has passed 50% of the target and bounce back is larger than the threshold
if (meas_rate_max-meas_rate_min > meas_rate_max*g.autotune_aggressiveness) {
// the measurement has passed 50% of the maximum rate and bounce back is larger than the threshold
step = UPDATE_GAINS;
}
@ -1222,11 +1224,73 @@ void Copter::ModeAutoTune::twitching_test(float measurement, float target, float @@ -1222,11 +1224,73 @@ void Copter::ModeAutoTune::twitching_test(float measurement, float target, float
}
}
// updating_d_up - increase D and adjust P to optimize the D term for a little bounce back
// twitching_test_angle - twitching tests
// update min and max and test for end conditions
void Copter::ModeAutoTune::twitching_test_angle(float angle, float rate, float angle_target_max, float &meas_angle_min, float &meas_angle_max, float &meas_rate_min, float &meas_rate_max)
{
// capture maximum angle
if (angle > meas_angle_max) {
// the angle still increasing
meas_angle_max = angle;
meas_angle_min = angle;
}
// capture minimum angle after we have reached a reasonable maximum angle
if ((angle < meas_angle_min) && (meas_angle_max > angle_target_max * 0.5f)) {
// the measurement is bouncing back
meas_angle_min = angle;
}
// capture maximum rate
if (rate > meas_rate_max) {
// the measurement is still increasing
meas_rate_max = rate;
meas_rate_min = rate;
}
// capture minimum rate after we have reached maximum rate
if (rate < meas_rate_min) {
// the measurement is still decreasing
meas_rate_min = rate;
}
// calculate early stopping time based on the time it takes to get to 90%
if (meas_angle_max < angle_target_max * 0.75f) {
// the measurement not reached the 90% threshold yet
step_stop_time = step_start_time + (millis() - step_start_time) * 3.0f;
step_stop_time = MIN(step_stop_time, step_start_time + AUTOTUNE_TESTING_STEP_TIMEOUT_MS);
}
if (meas_angle_max > angle_target_max) {
// the measurement has passed the maximum angle
step = UPDATE_GAINS;
}
if (meas_angle_max-meas_angle_min > meas_angle_max*g.autotune_aggressiveness) {
// the measurement has passed 50% of the maximum angle and bounce back is larger than the threshold
step = UPDATE_GAINS;
}
if (millis() >= step_stop_time) {
// we have passed the maximum stop time
step = UPDATE_GAINS;
}
}
// twitching_measure_acceleration - measure rate of change of measurement
void Copter::ModeAutoTune::twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max)
{
if (rate_measurement_max < rate_measurement) {
rate_measurement_max = rate_measurement;
rate_of_change = (1000.0f*rate_measurement_max)/(millis() - step_start_time);
}
}
// updating_rate_d_up - increase D and adjust P to optimize the D term for a little bounce back
// optimize D term while keeping the maximum just below the target by adjusting P
void Copter::ModeAutoTune::updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max)
void Copter::ModeAutoTune::updating_rate_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max)
{
if (measurement_max > target) {
if (meas_rate_max > rate_target) {
// if maximum measurement was higher than target
// reduce P gain (which should reduce maximum)
tune_p -= tune_p*tune_p_step_ratio;
@ -1241,7 +1305,7 @@ void Copter::ModeAutoTune::updating_d_up(float &tune_d, float tune_d_min, float @@ -1241,7 +1305,7 @@ void Copter::ModeAutoTune::updating_d_up(float &tune_d, float tune_d_min, float
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
}
}
}else if ((measurement_max < target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (tune_p <= tune_p_max)) {
}else if ((meas_rate_max < rate_target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (tune_p <= tune_p_max)) {
// we have not achieved a high enough maximum to get a good measurement of bounce back.
// increase P gain (which should increase maximum)
tune_p += tune_p*tune_p_step_ratio;
@ -1251,7 +1315,7 @@ void Copter::ModeAutoTune::updating_d_up(float &tune_d, float tune_d_min, float @@ -1251,7 +1315,7 @@ void Copter::ModeAutoTune::updating_d_up(float &tune_d, float tune_d_min, float
}
}else{
// we have a good measurement of bounce back
if (measurement_max-measurement_min > measurement_max*g.autotune_aggressiveness) {
if (meas_rate_max-meas_rate_min > meas_rate_max*g.autotune_aggressiveness) {
// ignore the next result unless it is the same as this one
ignore_next = true;
// bounce back is bigger than our threshold so increment the success counter
@ -1277,11 +1341,11 @@ void Copter::ModeAutoTune::updating_d_up(float &tune_d, float tune_d_min, float @@ -1277,11 +1341,11 @@ void Copter::ModeAutoTune::updating_d_up(float &tune_d, float tune_d_min, float
}
}
// updating_d_down - decrease D and adjust P to optimize the D term for no bounce back
// updating_rate_d_down - decrease D and adjust P to optimize the D term for no bounce back
// optimize D term while keeping the maximum just below the target by adjusting P
void Copter::ModeAutoTune::updating_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max)
void Copter::ModeAutoTune::updating_rate_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max)
{
if (measurement_max > target) {
if (meas_rate_max > rate_target) {
// if maximum measurement was higher than target
// reduce P gain (which should reduce maximum)
tune_p -= tune_p*tune_p_step_ratio;
@ -1296,7 +1360,7 @@ void Copter::ModeAutoTune::updating_d_down(float &tune_d, float tune_d_min, floa @@ -1296,7 +1360,7 @@ void Copter::ModeAutoTune::updating_d_down(float &tune_d, float tune_d_min, floa
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
}
}
}else if ((measurement_max < target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (tune_p <= tune_p_max)) {
}else if ((meas_rate_max < rate_target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (tune_p <= tune_p_max)) {
// we have not achieved a high enough maximum to get a good measurement of bounce back.
// increase P gain (which should increase maximum)
tune_p += tune_p*tune_p_step_ratio;
@ -1306,7 +1370,7 @@ void Copter::ModeAutoTune::updating_d_down(float &tune_d, float tune_d_min, floa @@ -1306,7 +1370,7 @@ void Copter::ModeAutoTune::updating_d_down(float &tune_d, float tune_d_min, floa
}
}else{
// we have a good measurement of bounce back
if (measurement_max-measurement_min < measurement_max*g.autotune_aggressiveness) {
if (meas_rate_max-meas_rate_min < meas_rate_max*g.autotune_aggressiveness) {
if (ignore_next == false) {
// bounce back is less than our threshold so increment the success counter
counter++;
@ -1332,44 +1396,36 @@ void Copter::ModeAutoTune::updating_d_down(float &tune_d, float tune_d_min, floa @@ -1332,44 +1396,36 @@ void Copter::ModeAutoTune::updating_d_down(float &tune_d, float tune_d_min, floa
}
}
// updating_p_down - decrease P until we don't reach the target before time out
// P is decreased to ensure we are not overshooting the target
void Copter::ModeAutoTune::updating_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float target, float measurement_max)
// updating_rate_p_up_d_down - increase P to ensure the target is reached while checking bounce back isn't increasing
// P is increased until we achieve our target within a reasonable time while reducing D if bounce back increases above the threshold
void Copter::ModeAutoTune::updating_rate_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max)
{
if (measurement_max < target*(1+0.5f*g.autotune_aggressiveness)) {
if (ignore_next == false) {
// if maximum measurement was lower than target so increment the success counter
counter++;
} else {
ignore_next = false;
}
}else{
if (meas_rate_max > rate_target*(1+0.5f*g.autotune_aggressiveness)) {
// ignore the next result unless it is the same as this one
ignore_next = true;
// if maximum measurement was higher than target so decrement the success counter
// if maximum measurement was greater than target so increment the success counter
counter++;
} else if ((meas_rate_max < rate_target) && (meas_rate_max > rate_target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (meas_rate_max-meas_rate_min > meas_rate_max*g.autotune_aggressiveness) && (tune_d > tune_d_min)) {
// if bounce back was larger than the threshold so decrement the success counter
if (counter > 0 ) {
counter--;
}
// decrease P gain (which should decrease the maximum)
// decrease D gain (which should decrease bounce back)
tune_d -= tune_d*tune_d_step_ratio;
// do not decrease the D term past the minimum
if (tune_d <= tune_d_min) {
tune_d = tune_d_min;
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
}
// decrease P gain to match D gain reduction
tune_p -= tune_p*tune_p_step_ratio;
// stop tuning if we hit maximum P
// do not decrease the P term past the minimum
if (tune_p <= tune_p_min) {
tune_p = tune_p_min;
counter = AUTOTUNE_SUCCESS_COUNT;
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
}
}
}
// updating_p_up - increase P to ensure the target is reached
// P is increased until we achieve our target within a reasonable time
void Copter::ModeAutoTune::updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float target, float measurement_max)
{
if (measurement_max > target*(1+0.5f*g.autotune_aggressiveness)) {
// ignore the next result unless it is the same as this one
ignore_next = 1;
// if maximum measurement was greater than target so increment the success counter
counter++;
// cancel change in direction
positive_direction = !positive_direction;
}else{
if (ignore_next == false) {
// if maximum measurement was lower than target so decrement the success counter
@ -1390,36 +1446,44 @@ void Copter::ModeAutoTune::updating_p_up(float &tune_p, float tune_p_max, float @@ -1390,36 +1446,44 @@ void Copter::ModeAutoTune::updating_p_up(float &tune_p, float tune_p_max, float
}
}
// updating_p_up - increase P to ensure the target is reached while checking bounce back isn't increasing
// P is increased until we achieve our target within a reasonable time while reducing D if bounce back increases above the threshold
void Copter::ModeAutoTune::updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max)
// updating_angle_p_down - decrease P until we don't reach the target before time out
// P is decreased to ensure we are not overshooting the target
void Copter::ModeAutoTune::updating_angle_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max)
{
if (measurement_max > target*(1+0.5f*g.autotune_aggressiveness)) {
if ((meas_angle_max < angle_target*(1+0.5f*g.autotune_aggressiveness)) && (meas_rate_min > -meas_rate_max*g.autotune_aggressiveness)) {
if (ignore_next == false) {
// if maximum measurement was lower than target so increment the success counter
counter++;
} else {
ignore_next = false;
}
}else{
// ignore the next result unless it is the same as this one
ignore_next = true;
// if maximum measurement was greater than target so increment the success counter
counter++;
} else if ((measurement_max < target) && (measurement_max > target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (measurement_max-measurement_min > measurement_max*g.autotune_aggressiveness) && (tune_d > tune_d_min)) {
// if bounce back was larger than the threshold so decrement the success counter
// if maximum measurement was higher than target so decrement the success counter
if (counter > 0 ) {
counter--;
}
// decrease D gain (which should decrease bounce back)
tune_d -= tune_d*tune_d_step_ratio;
// stop tuning if we hit minimum D
if (tune_d <= tune_d_min) {
tune_d = tune_d_min;
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
}
// decrease P gain to match D gain reduction
// decrease P gain (which should decrease the maximum)
tune_p -= tune_p*tune_p_step_ratio;
// stop tuning if we hit minimum P
// stop tuning if we hit maximum P
if (tune_p <= tune_p_min) {
tune_p = tune_p_min;
counter = AUTOTUNE_SUCCESS_COUNT;
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
}
// cancel change in direction
positive_direction = !positive_direction;
}
}
// updating_angle_p_up - increase P to ensure the target is reached
// P is increased until we achieve our target within a reasonable time
void Copter::ModeAutoTune::updating_angle_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max)
{
if ((meas_angle_max > angle_target*(1+0.5f*g.autotune_aggressiveness)) || (meas_rate_min < -meas_rate_max*g.autotune_aggressiveness)) {
// ignore the next result unless it is the same as this one
ignore_next = 1;
// if maximum measurement was greater than target so increment the success counter
counter++;
}else{
if (ignore_next == false) {
// if maximum measurement was lower than target so decrement the success counter
@ -1440,15 +1504,6 @@ void Copter::ModeAutoTune::updating_p_up_d_down(float &tune_d, float tune_d_min, @@ -1440,15 +1504,6 @@ void Copter::ModeAutoTune::updating_p_up_d_down(float &tune_d, float tune_d_min,
}
}
// twitching_measure_acceleration - measure rate of change of measurement
void Copter::ModeAutoTune::twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max)
{
if (rate_measurement_max < rate_measurement) {
rate_measurement_max = rate_measurement;
rate_of_change = (1000.0f*rate_measurement_max)/(millis() - step_start_time);
}
}
// get attitude for slow position hold in autotune mode
void Copter::ModeAutoTune::get_poshold_attitude(float &roll_cd_out, float &pitch_cd_out, float &yaw_cd_out)
{

Write Preview
Loading…
Cancel
Save