From 8b256cf92452ab8e858b59d085040d13ad3b0785 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Tue, 6 Mar 2012 23:20:30 +0900 Subject: [PATCH] ArduCopter - fix for stability patch and motor filter meant to deal with engines tendency to speed up faster than they slow down --- ArduCopter/motors_hexa.pde | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduCopter/motors_hexa.pde b/ArduCopter/motors_hexa.pde index ad7435148c..10eae0b73b 100644 --- a/ArduCopter/motors_hexa.pde +++ b/ArduCopter/motors_hexa.pde @@ -77,9 +77,9 @@ static void output_motors_armed() // Tridge's stability patch - for (int m = 0; m <= 6; m++){ + for (int m = 1; m <= 6; m++){ int c = ch_of_mot(m); - int c_opp = ch_of_mot(m ^ 1); // m ^ 1 is the opposite motor. c_opp is channel of opposite motor. + int c_opp = ch_of_mot(((m-1)^1)+1); // ((m-1)^1)+1 is the opposite motor. c_opp is channel of opposite motor. if(motor_out[c] > out_max){ motor_out[c_opp] -= motor_out[c] - out_max; motor_out[c] = out_max; @@ -108,7 +108,7 @@ static void output_motors_armed() // this filter slows the acceleration of motors vs the deceleration // Idea by Denny Rowland to help with his Yaw issue - for(int8_t m = 0; m <= 6; m++){ + for(int8_t m = 1; m <= 6; m++){ int c = ch_of_mot(m); if(motor_filtered[c] < motor_out[c]){ motor_filtered[c] = (motor_out[c] + motor_filtered[c]) / 2;