Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-08 08:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: AutoTune formatting fixes

Browse Source 
no functional change
This commit is contained in:
Randy Mackay 2015-03-11 14:32:44 +09:00
parent cc0d5b9ced
commit b475a2fe10
1 changed files with 149 additions and 161 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
ArduCopter/control_autotune.pde
View File

@ -392,7 +392,7 @@ static void autotune_attitude_control()
// disable rate limits
attitude_control.limit_angle_to_rate_request(false);
if(autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN || autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP){
if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) {
// Testing increasing stabilize P gain so will set lean angle target
switch (autotune_state.axis) {
case AUTOTUNE_AXIS_ROLL:
@ -433,7 +433,7 @@ static void autotune_attitude_control()
// Add filter to measurements
switch (autotune_state.axis) {
case AUTOTUNE_AXIS_ROLL:
if(autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN || autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP){
if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) {
rotation_rate = direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f);
} else {
rotation_rate = direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f - autotune_start_rate);
@ -441,7 +441,7 @@ static void autotune_attitude_control()
lean_angle = direction_sign * (ahrs.roll_sensor - (int32_t)autotune_start_angle);
break;
case AUTOTUNE_AXIS_PITCH:
if(autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN || autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP){
if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) {
rotation_rate = direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f);
} else {
rotation_rate = direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f - autotune_start_rate);
@ -449,7 +449,7 @@ static void autotune_attitude_control()
lean_angle = direction_sign * (ahrs.pitch_sensor - (int32_t)autotune_start_angle);
break;
case AUTOTUNE_AXIS_YAW:
if(autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN || autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP){
if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) {
rotation_rate = direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f);
} else {
rotation_rate = direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f - autotune_start_rate);
@ -492,7 +492,7 @@ static void autotune_attitude_control()
attitude_control.limit_angle_to_rate_request(true);
// log the latest gains
if(autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN || autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP){
if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) {
switch (autotune_state.axis) {
case AUTOTUNE_AXIS_ROLL:
Log_Write_AutoTune(autotune_state.axis, autotune_state.tune_type, autotune_target_angle, autotune_test_min, autotune_test_max, tune_roll_rp, tune_roll_rd, tune_roll_sp);
@ -759,7 +759,7 @@ static void autotune_load_orig_gains()
// sanity check the gains
bool failed = false;
if (autotune_roll_enabled()) {
if (orig_roll_rp != 0 || orig_roll_sp != 0 ) {
if ((orig_roll_rp != 0) || (orig_roll_sp != 0)) {
g.pid_rate_roll.kP(orig_roll_rp);
g.pid_rate_roll.kI(orig_roll_ri);
g.pid_rate_roll.kD(orig_roll_rd);
@ -769,7 +769,7 @@ static void autotune_load_orig_gains()
}
}
if (autotune_pitch_enabled()) {
if (orig_pitch_rp != 0 || orig_pitch_sp != 0 ) {
if ((orig_pitch_rp != 0) || (orig_pitch_sp != 0)) {
g.pid_rate_pitch.kP(orig_pitch_rp);
g.pid_rate_pitch.kI(orig_pitch_ri);
g.pid_rate_pitch.kD(orig_pitch_rd);
@ -779,7 +779,7 @@ static void autotune_load_orig_gains()
}
}
if (autotune_yaw_enabled()) {
if (orig_yaw_rp != 0 || orig_yaw_sp != 0 || orig_yaw_rLPF != 0 ) {
if ((orig_yaw_rp != 0) || (orig_yaw_sp != 0) || (orig_yaw_rLPF != 0)) {
g.pid_rate_yaw.kP(orig_yaw_rp);
g.pid_rate_yaw.kI(orig_yaw_ri);
g.pid_rate_yaw.kD(orig_yaw_rd);
@ -841,26 +841,21 @@ static void autotune_load_intra_test_gains()
// we are restarting tuning so reset gains to tuning-start gains (i.e. low I term)
// sanity check the gains
bool failed = true;
if (autotune_roll_enabled()) {
if (orig_roll_rp != 0) {
if (autotune_roll_enabled() && (orig_roll_rp != 0)) {
g.pid_rate_roll.kP(orig_roll_rp);
g.pid_rate_roll.kI(orig_roll_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
g.pid_rate_roll.kD(orig_roll_rd);
g.p_stabilize_roll.kP(orig_roll_sp);
failed = false;
}
}
if (autotune_pitch_enabled()) {
if (orig_pitch_rp != 0) {
if (autotune_pitch_enabled() && (orig_pitch_rp != 0)) {
g.pid_rate_pitch.kP(orig_pitch_rp);
g.pid_rate_pitch.kI(orig_pitch_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
g.pid_rate_pitch.kD(orig_pitch_rd);
g.p_stabilize_pitch.kP(orig_pitch_sp);
failed = false;
}
}
if (autotune_yaw_enabled()) {
if (orig_yaw_rp != 0) {
if (autotune_yaw_enabled() && (orig_yaw_rp != 0)) {
g.pid_rate_yaw.kP(orig_yaw_rp);
g.pid_rate_yaw.kI(orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
g.pid_rate_yaw.kD(orig_yaw_rd);
@ -868,7 +863,6 @@ static void autotune_load_intra_test_gains()
g.p_stabilize_yaw.kP(orig_yaw_sp);
failed = false;
}
}
if (failed) {
// log an error message and fail the autotune
Log_Write_Error(ERROR_SUBSYSTEM_AUTOTUNE,ERROR_CODE_AUTOTUNE_BAD_GAINS);
@ -923,8 +917,7 @@ static void autotune_save_tuning_gains()
// if we successfully completed tuning
if (autotune_state.mode == AUTOTUNE_MODE_SUCCESS) {
// sanity check the rate P values
if (autotune_roll_enabled()) {
if (tune_roll_rp != 0) {
if (autotune_roll_enabled() && (tune_roll_rp != 0)) {
// rate roll gains
g.pid_rate_roll.kP(tune_roll_rp);
g.pid_rate_roll.kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL);
@ -944,10 +937,8 @@ static void autotune_save_tuning_gains()
orig_roll_rd = g.pid_rate_roll.kD();
orig_roll_sp = g.p_stabilize_roll.kP();
}
}
if (autotune_pitch_enabled()) {
if (tune_pitch_rp != 0) {
if (autotune_pitch_enabled() && (tune_pitch_rp != 0)) {
// rate pitch gains
g.pid_rate_pitch.kP(tune_pitch_rp);
g.pid_rate_pitch.kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL);
@ -967,10 +958,8 @@ static void autotune_save_tuning_gains()
orig_pitch_rd = g.pid_rate_pitch.kD();
orig_pitch_sp = g.p_stabilize_pitch.kP();
}
}
if (autotune_yaw_enabled()) {
if (tune_yaw_rp != 0) {
if (autotune_yaw_enabled() && (tune_yaw_rp != 0)) {
// rate yaw gains
g.pid_rate_yaw.kP(tune_yaw_rp);
g.pid_rate_yaw.kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL);
@ -992,7 +981,6 @@ static void autotune_save_tuning_gains()
orig_yaw_rLPF = g.pid_rate_yaw.filt_hz();
orig_yaw_sp = g.p_stabilize_yaw.kP();
}
}
// update GCS and log save gains event
autotune_update_gcs(AUTOTUNE_MESSAGE_SAVED_GAINS);
Log_Write_Event(DATA_AUTOTUNE_SAVEDGAINS);
@ -1134,7 +1122,7 @@ void autotune_updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, f
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 ((measurement_max < 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;
@ -1181,7 +1169,7 @@ void autotune_updating_d_down(float &tune_d, float tune_d_min, float tune_d_step
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 ((measurement_max < 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;