From c7c16a5b0f054094514defd12d411af3de4201e2 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Wed, 22 Feb 2012 10:27:34 -0800 Subject: [PATCH] Emile's Fixes --- ArduCopter/motors_hexa.pde | 180 ++++++++++++++++++------------------- 1 file changed, 87 insertions(+), 93 deletions(-) diff --git a/ArduCopter/motors_hexa.pde b/ArduCopter/motors_hexa.pde index 7ab56805ba..64d3e52d34 100644 --- a/ArduCopter/motors_hexa.pde +++ b/ArduCopter/motors_hexa.pde @@ -5,19 +5,19 @@ static void init_motors_out() { #if INSTANT_PWM == 0 - APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4) - | _BV(MOT_5) | _BV(MOT_6) ); + APM_RC.SetFastOutputChannels(_BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4) + | _BV(MOT_5) | _BV(MOT_6)); #endif } static void motors_output_enable() { - APM_RC.enable_out(MOT_1); - APM_RC.enable_out(MOT_2); - APM_RC.enable_out(MOT_3); - APM_RC.enable_out(MOT_4); - APM_RC.enable_out(MOT_5); - APM_RC.enable_out(MOT_6); + APM_RC.enable_out(MOT_1); + APM_RC.enable_out(MOT_2); + APM_RC.enable_out(MOT_3); + APM_RC.enable_out(MOT_4); + APM_RC.enable_out(MOT_5); + APM_RC.enable_out(MOT_6); } static void output_motors_armed() @@ -44,7 +44,7 @@ static void output_motors_armed() //left side motor_out[MOT_2] = g.rc_3.radio_out + g.rc_1.pwm_out; // CCW Middle motor_out[MOT_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW Front - motor_out[MOT_6] = g.rc_3.radio_out + roll_out - pitch_out; // CW Back + motor_out[MOT_6] = g.rc_3.radio_out + roll_out - pitch_out; // CW Back //right side motor_out[MOT_1] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW Middle @@ -73,26 +73,26 @@ static void output_motors_armed() motor_out[MOT_3] -= g.rc_4.pwm_out; // CW motor_out[MOT_1] -= g.rc_4.pwm_out; // CW - motor_out[MOT_6] -= g.rc_4.pwm_out; // CW + motor_out[MOT_6] -= g.rc_4.pwm_out; // CW // Tridge's stability patch - 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; - } - } + 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; + } + } // limit output so motors don't stop - motor_out[MOT_1] = max(motor_out[MOT_1], out_min); - motor_out[MOT_2] = max(motor_out[MOT_2], out_min); - motor_out[MOT_3] = max(motor_out[MOT_3], out_min); - motor_out[MOT_4] = max(motor_out[MOT_4], out_min); - motor_out[MOT_5] = max(motor_out[MOT_5], out_min); - motor_out[MOT_6] = max(motor_out[MOT_6], out_min); + motor_out[MOT_1] = max(motor_out[MOT_1], out_min); + motor_out[MOT_2] = max(motor_out[MOT_2], out_min); + motor_out[MOT_3] = max(motor_out[MOT_3], out_min); + motor_out[MOT_4] = max(motor_out[MOT_4], out_min); + motor_out[MOT_5] = max(motor_out[MOT_5], out_min); + motor_out[MOT_6] = max(motor_out[MOT_6], out_min); #if CUT_MOTORS == ENABLED // if we are not sending a throttle output, we cut the motors @@ -108,8 +108,8 @@ 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++ ) { - int c = ch_of_mot(m); + 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{ @@ -143,7 +143,7 @@ static void output_motors_disarmed() } // fill the motor_out[] array for HIL use - for (unsigned char i = 0; i < 8; i++) { + for (unsigned char i = 0; i < 8; i++){ motor_out[i] = g.rc_3.radio_min; } @@ -158,7 +158,7 @@ static void output_motors_disarmed() static void output_motor_test() { - motors_output_enable(); + motors_output_enable(); motor_out[MOT_1] = g.rc_3.radio_min; motor_out[MOT_2] = g.rc_3.radio_min; @@ -168,74 +168,68 @@ static void output_motor_test() motor_out[MOT_6] = g.rc_3.radio_min; if(g.frame_orientation == X_FRAME){ - - APM_RC.OutputCh(MOT_3, g.rc_3.radio_min); - delay(4000); - APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100); - delay(300); - - APM_RC.OutputCh(MOT_5, g.rc_3.radio_min); - delay(2000); - APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100); - delay(300); - - APM_RC.OutputCh(MOT_1, g.rc_3.radio_min); - delay(2000); - APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100); - delay(300); - - APM_RC.OutputCh(MOT_4, g.rc_3.radio_min); - delay(2000); - APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100); - delay(300); - - APM_RC.OutputCh(MOT_6, g.rc_3.radio_min); - delay(2000); - APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100); - delay(300); - - APM_RC.OutputCh(MOT_2, g.rc_3.radio_min); - delay(2000); - APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100); - delay(300); - - } + APM_RC.OutputCh(MOT_3, g.rc_3.radio_min); + delay(4000); + APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100); + delay(300); + + APM_RC.OutputCh(MOT_5, g.rc_3.radio_min); + delay(2000); + APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100); + delay(300); + + APM_RC.OutputCh(MOT_1, g.rc_3.radio_min); + delay(2000); + APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100); + delay(300); + + APM_RC.OutputCh(MOT_4, g.rc_3.radio_min); + delay(2000); + APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100); + delay(300); + + APM_RC.OutputCh(MOT_6, g.rc_3.radio_min); + delay(2000); + APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100); + delay(300); + + APM_RC.OutputCh(MOT_2, g.rc_3.radio_min); + delay(2000); + APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100); + delay(300); } else { /* PLUS_FRAME */ - - APM_RC.OutputCh(MOT_5, g.rc_3.radio_min); - delay(4000); - APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100); - delay(300); - - APM_RC.OutputCh(MOT_1, g.rc_3.radio_min); - delay(2000); - APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100); - delay(300); - - APM_RC.OutputCh(MOT_4, g.rc_3.radio_min); - delay(2000); - APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100); - delay(300); - - APM_RC.OutputCh(MOT_6, g.rc_3.radio_min); - delay(2000); - APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100); - delay(300); - - APM_RC.OutputCh(MOT_2, g.rc_3.radio_min); - delay(2000); - APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100); - delay(300); - - APM_RC.OutputCh(MOT_3, g.rc_3.radio_min); - delay(2000); - APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100); - delay(300); + APM_RC.OutputCh(MOT_5, g.rc_3.radio_min); + delay(4000); + APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100); + delay(300); + + APM_RC.OutputCh(MOT_1, g.rc_3.radio_min); + delay(2000); + APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100); + delay(300); + + APM_RC.OutputCh(MOT_4, g.rc_3.radio_min); + delay(2000); + APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100); + delay(300); + + APM_RC.OutputCh(MOT_6, g.rc_3.radio_min); + delay(2000); + APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100); + delay(300); + + APM_RC.OutputCh(MOT_2, g.rc_3.radio_min); + delay(2000); + APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100); + delay(300); + + APM_RC.OutputCh(MOT_3, g.rc_3.radio_min); + delay(2000); + APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100); + delay(300); } - } - APM_RC.OutputCh(MOT_1, motor_out[MOT_1]); APM_RC.OutputCh(MOT_2, motor_out[MOT_2]); APM_RC.OutputCh(MOT_3, motor_out[MOT_3]); @@ -244,4 +238,4 @@ static void output_motor_test() APM_RC.OutputCh(MOT_6, motor_out[MOT_6]); } -#endif +#endif \ No newline at end of file