From e80ed6d687a725ecc565b1cf04b68b49a29dcfdd Mon Sep 17 00:00:00 2001 From: "tridge60@gmail.com" Date: Wed, 20 Jul 2011 00:33:04 +0000 Subject: [PATCH] quad: fixed the clipping of PWM outputs 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 --- ArduCopterMega/motors_quad.pde | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/ArduCopterMega/motors_quad.pde b/ArduCopterMega/motors_quad.pde index 9b273589b6..779b0a5991 100644 --- a/ArduCopterMega/motors_quad.pde +++ b/ArduCopterMega/motors_quad.pde @@ -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){