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

quad: fixed the clipping of PWM outputs

Browse Source 
when we clip a motor output we need to decrease the opposite motor by
the same amount, otherwise we lose the ability to control the quad
when the servo outputs are extreme

Found using the quad HIL test. Thanks to Jani for testing this on a
real quad!

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2929 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
tridge60@gmail.com 2011-07-20 00:33:04 +00:00
parent 09256e12da
commit e80ed6d687
1 changed files with 14 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
ArduCopterMega/motors_quad.pde
View File

@ -52,17 +52,26 @@ static void output_motors_armed()
motor_out[CH_3] -= g.rc_4.pwm_out; // CW
motor_out[CH_4] -= g.rc_4.pwm_out; // CW
/* We need to clip motor output at out_max. When cipping a motors
* output we also need to compensate for the instability by
* lowering the opposite motor by the same proportion. This
* ensures that we retain control when one or more of the motors
* is at its maximum output
*/
for (int i=CH_1; i<=CH_4; i++) {
if (motor_out[i] > out_max && motor_out[i] > out_min) {
/* note that i^1 is the 'opposite' motor for a quad */
motor_out[i^1] *= ((float)out_max - out_min) / (motor_out[i] - out_min);
motor_out[i] = out_max;
}
}
// limit output so motors don't stop
motor_out[CH_1] = max(motor_out[CH_1], out_min);
motor_out[CH_2] = max(motor_out[CH_2], out_min);
motor_out[CH_3] = max(motor_out[CH_3], out_min);
motor_out[CH_4] = max(motor_out[CH_4], out_min);
motor_out[CH_1] = min(motor_out[CH_1], out_max);
motor_out[CH_2] = min(motor_out[CH_2], out_max);
motor_out[CH_3] = min(motor_out[CH_3], out_max);
motor_out[CH_4] = min(motor_out[CH_4], out_max);
#if CUT_MOTORS == ENABLED
// Send commands to motors
if(g.rc_3.servo_out > 0){