From b92d7aaad5a343bbf1c3e0730e75310006bb2405 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 18 Feb 2012 22:04:09 +0900 Subject: [PATCH] ArduCopter - fix for yaw control on Octa Quad Plus frame. --- ArduCopter/motors_octa_quad.pde | 21 ++++++++++----------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/ArduCopter/motors_octa_quad.pde b/ArduCopter/motors_octa_quad.pde index 1728d0754f..b8566f7f8e 100644 --- a/ArduCopter/motors_octa_quad.pde +++ b/ArduCopter/motors_octa_quad.pde @@ -51,7 +51,6 @@ static void output_motors_armed() motor_out[MOT_6] = g.rc_3.radio_out - roll_out + pitch_out; // APM2 OUT6 APM1 OUT8 FRONT RIGHT CW BOTTOM motor_out[MOT_7] = g.rc_3.radio_out - roll_out - pitch_out; // APM2 OUT7 APM1 OUT10 BACK RIGHT CCW BOTTOM motor_out[MOT_8] = g.rc_3.radio_out + roll_out - pitch_out; // APM2 OUT8 APM1 OUT11 BACK LEFT CW BOTTOM - }else{ roll_out = g.rc_1.pwm_out; pitch_out = g.rc_2.pwm_out; @@ -64,18 +63,18 @@ static void output_motors_armed() motor_out[MOT_6] = g.rc_3.radio_out + pitch_out; // APM2 OUT6 APM1 OUT8 FRONT CW BOTTOM motor_out[MOT_7] = g.rc_3.radio_out - roll_out; // APM2 OUT7 APM1 OUT10 RIGHT CCW BOTTOM motor_out[MOT_8] = g.rc_3.radio_out - pitch_out; // APM2 OUT8 APM1 OUT11 BACK CW BOTTOM + } - // Yaw - motor_out[MOT_1] += g.rc_4.pwm_out; // CCW - motor_out[MOT_3] += g.rc_4.pwm_out; // CCW - motor_out[MOT_5] += g.rc_4.pwm_out; // CCW - motor_out[MOT_7] += g.rc_4.pwm_out; // CCW + // Yaw + motor_out[MOT_1] += g.rc_4.pwm_out; // CCW + motor_out[MOT_3] += g.rc_4.pwm_out; // CCW + motor_out[MOT_5] += g.rc_4.pwm_out; // CCW + motor_out[MOT_7] += g.rc_4.pwm_out; // CCW - motor_out[MOT_2] -= g.rc_4.pwm_out; // CW - motor_out[MOT_4] -= g.rc_4.pwm_out; // CW - motor_out[MOT_6] -= g.rc_4.pwm_out; // CW - motor_out[MOT_8] -= g.rc_4.pwm_out; // CW - } + motor_out[MOT_2] -= g.rc_4.pwm_out; // CW + motor_out[MOT_4] -= g.rc_4.pwm_out; // CW + motor_out[MOT_6] -= g.rc_4.pwm_out; // CW + motor_out[MOT_8] -= g.rc_4.pwm_out; // CW // TODO add stability patch motor_out[MOT_1] = min(motor_out[MOT_1], out_max);