From f7e14fff37ebe528192c75e8fbbd12818ae4d115 Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Sun, 1 Jan 2012 15:49:58 -0500 Subject: [PATCH] ArduCopter motors_hexa: rewrite janky loops to use ch_of_mot * Previously the loop was written over channels, now it is over motors * the correct channel for that motor is determined by ch_of_mot. * ch_of_mot is defined correctly based on the config_channels macros. --- ArduCopter/motors_hexa.pde | 24 +++++++++++------------- 1 file changed, 11 insertions(+), 13 deletions(-) diff --git a/ArduCopter/motors_hexa.pde b/ArduCopter/motors_hexa.pde index fc80a31c2c..159017ee3d 100644 --- a/ArduCopter/motors_hexa.pde +++ b/ArduCopter/motors_hexa.pde @@ -67,13 +67,12 @@ static void output_motors_armed() // Tridge's stability patch - for (int i = CH_1; i<=CH_8; i++) { - if(i == CH_5 || i == CH_6) - continue; - if (motor_out[i] > out_max) { - // note that i^1 is the opposite motor - motor_out[i^1] -= motor_out[i] - out_max; - motor_out[i] = out_max; + for (int m = 0; 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. + if (motor_out[c] > out_max) { + motor_out[c_opp] -= motor_out[c] - out_max; + motor_out[c] = out_max; } } @@ -99,14 +98,13 @@ 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 i = CH_1; i <= CH_8; i++ ) { - if(i == CH_5 || i == CH_6) - continue; - if(motor_filtered[i] < motor_out[i]){ - motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2; + for(int8_t m = 0; 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; }else{ // don't filter - motor_filtered[i] = motor_out[i]; + motor_filtered[c] = motor_out[c]; } }