aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: Add circular limits to ACRO

Browse Source
This commit is contained in:
Jolyon Saunders 2014-11-10 16:07:08 -08:00 committed by Randy Mackay
parent d46c659d0f
commit a9205e1032
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/control_acro.pde
View File

@ -47,9 +47,14 @@ static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int
float rate_limit; float rate_limit;
Vector3f rate_ef_level, rate_bf_level, rate_bf_request; Vector3f rate_ef_level, rate_bf_level, rate_bf_request;
// 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;
}
// calculate roll, pitch rate requests // calculate roll, pitch rate requests
if (g.acro_expo <= 0) { if (g.acro_expo <= 0) {