zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: use new lowpass filter

master
Jonathan Challinger 10 years ago committed by Randy Mackay
parent
4993fd4d71
commit
f93ff2d3ec
2 changed files with 9 additions and 10 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. 18
      ArduCopter/control_autotune.pde
  2. 1
      ArduCopter/heli.pde

18
ArduCopter/control_autotune.pde
Unescape View File

@ -378,7 +378,7 @@ static void autotune_attitude_control() @@ -378,7 +378,7 @@ static void autotune_attitude_control()
autotune_target_angle = constrain_float(attitude_control.max_angle_step_bf_roll(), AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD);
autotune_start_rate = ToDeg(ahrs.get_gyro().x) * 100.0f;
autotune_start_angle = ahrs.roll_sensor;
rotation_rate_filt.set_cutoff_frequency(MAIN_LOOP_SECONDS,g.pid_rate_roll.filt_hz()*2.0f);
rotation_rate_filt.set_cutoff_frequency(g.pid_rate_roll.filt_hz()*2.0f);
if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) {
rotation_rate_filt.reset(autotune_start_rate);
} else {
@ -390,7 +390,7 @@ static void autotune_attitude_control() @@ -390,7 +390,7 @@ static void autotune_attitude_control()
autotune_target_angle = constrain_float(attitude_control.max_angle_step_bf_pitch(), AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD);
autotune_start_rate = ToDeg(ahrs.get_gyro().y) * 100.0f;
autotune_start_angle = ahrs.pitch_sensor;
rotation_rate_filt.set_cutoff_frequency(MAIN_LOOP_SECONDS,g.pid_rate_pitch.filt_hz()*2.0f);
rotation_rate_filt.set_cutoff_frequency(g.pid_rate_pitch.filt_hz()*2.0f);
if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) {
rotation_rate_filt.reset(autotune_start_rate);
} else {
@ -402,7 +402,7 @@ static void autotune_attitude_control() @@ -402,7 +402,7 @@ static void autotune_attitude_control()
autotune_target_angle = constrain_float(attitude_control.max_angle_step_bf_yaw(), AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD, AUTOTUNE_TARGET_ANGLE_YAW_CD);
autotune_start_rate = ToDeg(ahrs.get_gyro().z) * 100.0f;
autotune_start_angle = ahrs.yaw_sensor;
rotation_rate_filt.set_cutoff_frequency(MAIN_LOOP_SECONDS,AUTOTUNE_Y_FILT_FREQ);
rotation_rate_filt.set_cutoff_frequency(AUTOTUNE_Y_FILT_FREQ);
if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) {
rotation_rate_filt.reset(autotune_start_rate);
} else {
@ -461,25 +461,25 @@ static void autotune_attitude_control() @@ -461,25 +461,25 @@ static void autotune_attitude_control()
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)) {
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f));
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f), MAIN_LOOP_SECONDS);
} else {
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f - autotune_start_rate));
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f - autotune_start_rate), MAIN_LOOP_SECONDS);
}
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)) {
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f));
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f), MAIN_LOOP_SECONDS);
} else {
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f - autotune_start_rate));
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f - autotune_start_rate), MAIN_LOOP_SECONDS);
}
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)) {
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f));
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f), MAIN_LOOP_SECONDS);
} else {
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f - autotune_start_rate));
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f - autotune_start_rate), MAIN_LOOP_SECONDS);
}
lean_angle = direction_sign * wrap_180_cd(ahrs.yaw_sensor-(int32_t)autotune_start_angle);
break;

1
ArduCopter/heli.pde
Unescape View File

@ -16,7 +16,6 @@ static int8_t heli_dynamic_flight_counter; @@ -16,7 +16,6 @@ static int8_t heli_dynamic_flight_counter;
// heli_init - perform any special initialisation required for the tradheli
static void heli_init()
{
attitude_control.update_feedforward_filter_rates(MAIN_LOOP_SECONDS);
motors.set_dt(MAIN_LOOP_SECONDS);
// force recalculation of RSC ramp rates after setting _dt
motors.recalc_scalers();

Write Preview
Loading…
Cancel
Save