aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: mode_autotune: factor out common filter reset code

Browse Source
This commit is contained in:
Peter Barker 2018-07-30 17:09:10 +10:00 committed by Randy Mackay
parent fa1ad09a3b
commit 214adc121f
1 changed files with 5 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduCopter/mode_autotune.cpp
View File

@ -515,11 +515,6 @@ void Copter::ModeAutoTune::autotune_attitude_control()
start_rate = ToDeg(ahrs.get_gyro().x) * 100.0f;
start_angle = ahrs.roll_sensor;
rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_roll_pid().filt_hz()*2.0f);
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
rotation_rate_filt.reset(start_rate);
} else {
rotation_rate_filt.reset(0);
}
break;
case PITCH:
target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, AUTOTUNE_TARGET_RATE_RLLPIT_CDS);
@ -527,11 +522,6 @@ void Copter::ModeAutoTune::autotune_attitude_control()
start_rate = ToDeg(ahrs.get_gyro().y) * 100.0f;
start_angle = ahrs.pitch_sensor;
rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_pitch_pid().filt_hz()*2.0f);
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
rotation_rate_filt.reset(start_rate);
} else {
rotation_rate_filt.reset(0);
}
break;
case YAW:
target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, AUTOTUNE_TARGET_RATE_YAW_CDS);
@ -539,13 +529,13 @@ void Copter::ModeAutoTune::autotune_attitude_control()
start_rate = ToDeg(ahrs.get_gyro().z) * 100.0f;
start_angle = ahrs.yaw_sensor;
rotation_rate_filt.set_cutoff_frequency(AUTOTUNE_Y_FILT_FREQ);
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
rotation_rate_filt.reset(start_rate);
} else {
rotation_rate_filt.reset(0);
}
break;
}
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
rotation_rate_filt.reset(start_rate);
} else {
rotation_rate_filt.reset(0);
}
break;
case TWITCHING: {