Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

added stab_d gain scheduling

This commit is contained in:
Jason Short 2012-02-28 20:14:49 -08:00
parent 6ca8aeecf3
commit a096292edb
1 changed files with 16 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
ArduCopter/ArduCopter.pde
View File

@ -400,6 +400,8 @@ static boolean motor_auto_armed;
static Vector3f omega; static Vector3f omega;
// This is used to hold radio tuning values for in-flight CH6 tuning // This is used to hold radio tuning values for in-flight CH6 tuning
float tuning_value; 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 // LED output
@ -1487,8 +1489,20 @@ void update_roll_pitch_mode(void)
//reset_stability_I(); //reset_stability_I();
} }
// clear new radio frame info if(new_radio_frame){
new_radio_frame = false; // 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 // new radio frame is used to make sure we only call this at 50hz