From e89600afe32d04dfa632a945805bdf4ff2800a6a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 19 Feb 2014 20:52:00 +0900 Subject: [PATCH] Copter: integrate smoothing gain into flight modes --- ArduCopter/control_althold.pde | 5 +---- ArduCopter/control_autotune.pde | 5 +---- ArduCopter/control_drift.pde | 4 +--- ArduCopter/control_land.pde | 5 +---- ArduCopter/control_ofloiter.pde | 4 +--- ArduCopter/control_stabilize.pde | 5 +---- ArduCopter/heli_control_stabilize.pde | 5 +---- 7 files changed, 7 insertions(+), 26 deletions(-) diff --git a/ArduCopter/control_althold.pde b/ArduCopter/control_althold.pde index 74b2174d16..7aa90112b2 100644 --- a/ArduCopter/control_althold.pde +++ b/ArduCopter/control_althold.pde @@ -7,9 +7,6 @@ // althold_init - initialise althold controller static bool althold_init(bool ignore_checks) { - // initialise filters on roll/pitch input - reset_roll_pitch_in_filters(g.rc_1.control_in, g.rc_2.control_in); - // initialise altitude target to stopping point pos_control.set_target_to_stopping_point_z(); return true; @@ -59,7 +56,7 @@ static void althold_run() attitude_control.set_throttle_out(0, false); }else{ // call attitude controller - attitude_control.angle_ef_roll_pitch_rate_ef_yaw(target_roll, target_pitch, target_yaw_rate); + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // body-frame rate controller is run directly from 100hz loop // call throttle controller diff --git a/ArduCopter/control_autotune.pde b/ArduCopter/control_autotune.pde index 9ada6affae..cf2cbf8183 100644 --- a/ArduCopter/control_autotune.pde +++ b/ArduCopter/control_autotune.pde @@ -192,9 +192,6 @@ static bool autotune_init(bool ignore_checks) return false; } - // initialise filters on roll/pitch input - reset_roll_pitch_in_filters(g.rc_1.control_in, g.rc_2.control_in); - // initialise altitude target to stopping point pos_control.set_target_to_stopping_point_z(); return true; @@ -264,7 +261,7 @@ static void autotune_run() // if pilot override call attitude controller if (autotune_state.pilot_override || autotune_state.mode != AUTOTUNE_MODE_TUNING) { - attitude_control.angle_ef_roll_pitch_rate_ef_yaw(target_roll, target_pitch, target_yaw_rate); + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); }else{ // somehow get attitude requests from autotuning autotune_attitude_control(); diff --git a/ArduCopter/control_drift.pde b/ArduCopter/control_drift.pde index 89962240be..3ef762e876 100644 --- a/ArduCopter/control_drift.pde +++ b/ArduCopter/control_drift.pde @@ -12,8 +12,6 @@ static bool drift_init(bool ignore_checks) { if (GPS_ok() || ignore_checks) { - // initialise filters on roll/pitch input - reset_roll_pitch_in_filters(g.rc_1.control_in, g.rc_2.control_in); return true; }else{ return false; @@ -71,7 +69,7 @@ static void drift_run() } // call attitude controller - attitude_control.angle_ef_roll_pitch_rate_ef_yaw(target_roll, target_pitch, target_yaw_rate); + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // output pilot's throttle with angle boost attitude_control.set_throttle_out(pilot_throttle_scaled, true); diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index 123d82d3b1..39a04da1ab 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -16,9 +16,6 @@ static bool land_init(bool ignore_checks) wp_nav.set_loiter_target(stopping_point); } - // initialise filters on roll/pitch input - reset_roll_pitch_in_filters(g.rc_1.control_in, g.rc_2.control_in); - // initialise altitude target to stopping point pos_control.set_target_to_stopping_point_z(); return true; @@ -129,7 +126,7 @@ static void land_nogps_run() } // call attitude controller - attitude_control.angle_ef_roll_pitch_rate_ef_yaw(target_roll, target_pitch, target_yaw_rate); + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // call position controller pos_control.set_alt_target_from_climb_rate(get_throttle_land(), G_Dt); diff --git a/ArduCopter/control_ofloiter.pde b/ArduCopter/control_ofloiter.pde index e7d3fe70cc..2cda161b39 100644 --- a/ArduCopter/control_ofloiter.pde +++ b/ArduCopter/control_ofloiter.pde @@ -9,8 +9,6 @@ static bool ofloiter_init(bool ignore_checks) { #if OPTFLOW == ENABLED if (g.optflow_enabled || ignore_checks) { - // initialise filters on roll/pitch input - reset_roll_pitch_in_filters(g.rc_1.control_in, g.rc_2.control_in); return true; }else{ return false; @@ -70,7 +68,7 @@ static void ofloiter_run() target_pitch = get_of_pitch(target_pitch); // call attitude controller - attitude_control.angle_ef_roll_pitch_rate_ef_yaw(target_roll, target_pitch, target_yaw_rate); + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // run altitude controller if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) { diff --git a/ArduCopter/control_stabilize.pde b/ArduCopter/control_stabilize.pde index 7b928435a3..46163b3045 100644 --- a/ArduCopter/control_stabilize.pde +++ b/ArduCopter/control_stabilize.pde @@ -7,9 +7,6 @@ // stabilize_init - initialise stabilize controller static bool stabilize_init(bool ignore_checks) { - // initialise filters on roll/pitch input - reset_roll_pitch_in_filters(g.rc_1.control_in, g.rc_2.control_in); - // set target altitude to zero for reporting // To-Do: make pos controller aware when it's active/inactive so it can always report the altitude error? pos_control.set_alt_target(0); @@ -51,7 +48,7 @@ static void stabilize_run() attitude_control.init_targets(); }else{ // call attitude controller - attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, g.rc_feel_rp); + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // body-frame rate controller is run directly from 100hz loop } diff --git a/ArduCopter/heli_control_stabilize.pde b/ArduCopter/heli_control_stabilize.pde index e5d5f178f4..10465f6369 100644 --- a/ArduCopter/heli_control_stabilize.pde +++ b/ArduCopter/heli_control_stabilize.pde @@ -7,9 +7,6 @@ // stabilize_init - initialise stabilize controller static bool heli_stabilize_init(bool ignore_checks) { - // initialise filters on roll/pitch input - reset_roll_pitch_in_filters(g.rc_1.control_in, g.rc_2.control_in); - // set target altitude to zero for reporting // To-Do: make pos controller aware when it's active/inactive so it can always report the altitude error? pos_control.set_alt_target(0); @@ -40,7 +37,7 @@ static void heli_stabilize_run() pilot_throttle_scaled = get_pilot_desired_throttle(g.rc_3.control_in); // call attitude controller - attitude_control.angle_ef_roll_pitch_rate_ef_yaw(target_roll, target_pitch, target_yaw_rate); + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // output pilot's throttle - note that TradHeli does not used angle-boost attitude_control.set_throttle_out(pilot_throttle_scaled, false);