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

Copter: simplify autotune's filter value determination

Browse Source
This commit is contained in:
Peter Barker 2018-04-18 09:20:25 +10:00 committed by Randy Mackay
parent 84ff9c6928
commit 9c72a8ecc7
1 changed files with 20 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
ArduCopter/mode_autotune.cpp
View File

@ -548,7 +548,7 @@ void Copter::ModeAutoTune::autotune_attitude_control()
}
break;
case TWITCHING:
case TWITCHING: {
// Run the twitching step
// Note: we should be using intra-test gains (which are very close to the original gains but have lower I)
@ -598,34 +598,36 @@ void Copter::ModeAutoTune::autotune_attitude_control()
}
// capture this iterations rotation rate and lean angle
// Add filter to measurements
float gyro_reading = 0;
switch (axis) {
case ROLL:
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f), copter.scheduler.get_loop_period_s());
} else {
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f - start_rate), copter.scheduler.get_loop_period_s());
}
gyro_reading = ahrs.get_gyro().x;
lean_angle = direction_sign * (ahrs.roll_sensor - (int32_t)start_angle);
break;
case PITCH:
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f), copter.scheduler.get_loop_period_s());
} else {
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f - start_rate), copter.scheduler.get_loop_period_s());
}
gyro_reading = ahrs.get_gyro().y;
lean_angle = direction_sign * (ahrs.pitch_sensor - (int32_t)start_angle);
break;
case YAW:
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f), copter.scheduler.get_loop_period_s());
} else {
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f - start_rate), copter.scheduler.get_loop_period_s());
}
gyro_reading = ahrs.get_gyro().z;
lean_angle = direction_sign * wrap_180_cd(ahrs.yaw_sensor-(int32_t)start_angle);
break;
}
// Add filter to measurements
float filter_value;
switch (tune_type) {
case SP_DOWN:
case SP_UP:
filter_value = direction_sign * (ToDeg(gyro_reading) * 100.0f);
break;
default:
filter_value = direction_sign * (ToDeg(gyro_reading) * 100.0f - start_rate);
break;
}
rotation_rate = rotation_rate_filt.apply(filter_value,
copter.scheduler.get_loop_period_s());
switch (tune_type) {
case RD_UP:
case RD_DOWN:
@ -655,7 +657,7 @@ void Copter::ModeAutoTune::autotune_attitude_control()
copter.DataFlash.Log_Write_Rate(ahrs, *motors, *attitude_control, *pos_control);
#endif
break;
}
case UPDATE_GAINS:
// re-enable rate limits