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

Copter: Add circular limits pilot's roll/pitch inputs

This commit is contained in:
Jolyon Saunders 2014-11-10 16:06:53 -08:00 committed by Randy Mackay
parent ef62a2c697
commit d46c659d0f
1 changed files with 8 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
ArduCopter/Attitude.pde
View File

@ -14,9 +14,14 @@ static void get_pilot_desired_lean_angles(int16_t roll_in, int16_t pitch_in, int
static float _scaler = 1.0; static float _scaler = 1.0;
static int16_t _angle_max = 0; static int16_t _angle_max = 0;
// range check the input // apply circular limit to pitch and roll inputs
roll_in = constrain_int16(roll_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX); float total_out = pythagorous2((float)pitch_in, (float)roll_in);
pitch_in = constrain_int16(pitch_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX);
if (total_out > ROLL_PITCH_INPUT_MAX) {
float ratio = (float)ROLL_PITCH_INPUT_MAX / total_out;
roll_in *= ratio;
pitch_in *= ratio;
}
// return filtered roll if no scaling required // return filtered roll if no scaling required
if (aparm.angle_max == ROLL_PITCH_INPUT_MAX) { if (aparm.angle_max == ROLL_PITCH_INPUT_MAX) {