From a096292edb8a38a77388852831fb3620f2528ec1 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Tue, 28 Feb 2012 20:14:49 -0800 Subject: [PATCH] added stab_d gain scheduling --- ArduCopter/ArduCopter.pde | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 6cd32e4a4d..d7b95c700e 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -400,6 +400,8 @@ static boolean motor_auto_armed; static Vector3f omega; // This is used to hold radio tuning values for in-flight CH6 tuning float tuning_value; +// This will keep track of the percent of roll or pitch the user is applying +float roll_scale_d, pitch_scale_d; //////////////////////////////////////////////////////////////////////////////// // LED output @@ -1487,8 +1489,20 @@ void update_roll_pitch_mode(void) //reset_stability_I(); } - // clear new radio frame info - new_radio_frame = false; + if(new_radio_frame){ + // clear new radio frame info + new_radio_frame = false; + + // These values can be used to scale the PID gains + // This allows for a simple gain scheduling implementation + roll_scale_d = g.stabilize_d_schedule * (float)abs(g.rc_1.control_in); + roll_scale_d = (1 - (roll_scale_d / 4500.0)); + roll_scale_d = constrain(roll_scale_d, 0, 1) * g.stabilize_d; + + pitch_scale_d = g.stabilize_d_schedule * (float)abs(g.rc_2.control_in); + pitch_scale_d = (1 - (pitch_scale_d / 4500.0)); + pitch_scale_d = constrain(pitch_scale_d, 0, 1) * g.stabilize_d; + } } // new radio frame is used to make sure we only call this at 50hz