aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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

76
ArduCopter/control_autotune.pde
View File

@ -255,7 +255,7 @@ static void autotune_run()
// if not auto armed set throttle to zero and exit immediately
// this should not actually be possible because of the autotune_init() checks
if(!ap.auto_armed) {
if (!ap.auto_armed) {
attitude_control.relax_bf_rate_controller();
attitude_control.set_yaw_target_to_current_heading();
attitude_control.set_throttle_out(0, false);
@ -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,25 +433,25 @@ 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{
} else {
rotation_rate = direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f - autotune_start_rate);
}
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{
} else {
rotation_rate = direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f - autotune_start_rate);
}
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{
} else {
rotation_rate = direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f - autotune_start_rate);
}
lean_angle = direction_sign * wrap_180_cd(ahrs.yaw_sensor-(int32_t)autotune_start_angle);
@ -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);
@ -504,7 +504,7 @@ static void autotune_attitude_control()
Log_Write_AutoTune(autotune_state.axis, autotune_state.tune_type, autotune_target_angle, autotune_test_min, autotune_test_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp);
break;
}
}else{
} else {
switch (autotune_state.axis) {
case AUTOTUNE_AXIS_ROLL:
Log_Write_AutoTune(autotune_state.axis, autotune_state.tune_type, autotune_target_rate, autotune_test_min, autotune_test_max, tune_roll_rp, tune_roll_rd, tune_roll_sp);
@ -687,7 +687,7 @@ static void autotune_attitude_control()
// reverse direction
autotune_state.positive_direction = !autotune_state.positive_direction;
if(autotune_state.axis == AUTOTUNE_AXIS_YAW){
if (autotune_state.axis == AUTOTUNE_AXIS_YAW) {
attitude_control.angle_ef_roll_pitch_yaw( 0.0f, 0.0f, ahrs.yaw_sensor, false);
}
@ -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);
@ -934,7 +927,7 @@ static void autotune_save_tuning_gains()
g.p_stabilize_roll.kP(tune_roll_sp);
g.p_stabilize_roll.save_gains();
if(attitude_control.get_bf_feedforward()){
if (attitude_control.get_bf_feedforward()) {
attitude_control.save_accel_roll_max(tune_roll_accel);
}
@ -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);
@ -957,7 +948,7 @@ static void autotune_save_tuning_gains()
g.p_stabilize_pitch.kP(tune_pitch_sp);
g.p_stabilize_pitch.save_gains();
if(attitude_control.get_bf_feedforward()){
if (attitude_control.get_bf_feedforward()) {
attitude_control.save_accel_pitch_max(tune_pitch_accel);
}
@ -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);
@ -981,7 +970,7 @@ static void autotune_save_tuning_gains()
g.p_stabilize_yaw.kP(tune_yaw_sp);
g.p_stabilize_yaw.save_gains();
if(attitude_control.get_bf_feedforward()){
if (attitude_control.get_bf_feedforward()) {
attitude_control.save_accel_yaw_max(tune_yaw_accel);
}
@ -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);
@ -1040,7 +1028,7 @@ void autotune_twitching_test_p(float measurement, float target, float &measureme
{
// capture maximum measurement
if (measurement > measurement_max) {
if((measurement_min < measurement_max) && (measurement_max > target * 0.5f)){
if ((measurement_min < measurement_max) && (measurement_max > target * 0.5f)) {
// the measurement has stopped, bounced back and is starting to increase again.
measurement_max = measurement;
autotune_state.step = AUTOTUNE_STEP_UPDATE_GAINS;
@ -1069,7 +1057,7 @@ void autotune_twitching_test_p(float measurement, float target, float &measureme
autotune_state.step = AUTOTUNE_STEP_UPDATE_GAINS;
}
if(millis() >= autotune_step_stop_time) {
if (millis() >= autotune_step_stop_time) {
// we have passed the maximum stop time
autotune_state.step = AUTOTUNE_STEP_UPDATE_GAINS;
}
@ -1109,7 +1097,7 @@ void autotune_twitching_test_d(float measurement, float target, float &measureme
autotune_state.step = AUTOTUNE_STEP_UPDATE_GAINS;
}
if(millis() >= autotune_step_stop_time) {
if (millis() >= autotune_step_stop_time) {
// we have passed the maximum stop time
autotune_state.step = AUTOTUNE_STEP_UPDATE_GAINS;
}
@ -1123,7 +1111,7 @@ void autotune_updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, f
// if maximum measurement was higher than target
// reduce P gain (which should reduce maximum)
tune_p -= tune_p*tune_p_step_ratio;
if(tune_p < tune_p_min) {
if (tune_p < tune_p_min) {
// P gain is at minimum so start reducing D
tune_p = tune_p_min;
tune_d -= tune_d*tune_d_step_ratio;
@ -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;
@ -1170,7 +1158,7 @@ void autotune_updating_d_down(float &tune_d, float tune_d_min, float tune_d_step
// if maximum measurement was higher than target
// reduce P gain (which should reduce maximum)
tune_p -= tune_p*tune_p_step_ratio;
if(tune_p < tune_p_min) {
if (tune_p < tune_p_min) {
// P gain is at minimum so start reducing D gain
tune_p = tune_p_min;
tune_d -= tune_d*tune_d_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;
@ -1268,7 +1256,7 @@ void autotune_updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d
autotune_counter++;
// cancel change in direction
autotune_state.positive_direction = !autotune_state.positive_direction;
}else if((measurement_max-measurement_min > measurement_max*g.autotune_aggressiveness) && (tune_d > tune_d_min)) {
}else if ((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 (autotune_counter > 0 ) {
autotune_counter--;
@ -1308,7 +1296,7 @@ void autotune_updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d
// autotune_twitching_measure_acceleration - measure rate of change of measurement
void autotune_twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max)
{
if(rate_measurement_max < rate_measurement){
if (rate_measurement_max < rate_measurement) {
rate_measurement_max = rate_measurement;
rate_of_change = (1000.0f*rate_measurement_max)/(millis() - autotune_step_start_time);
}