diff --git a/ArduCopter/config_channels.h b/ArduCopter/config_channels.h index d3f34118ee..b3a1e7263d 100644 --- a/ArduCopter/config_channels.h +++ b/ArduCopter/config_channels.h @@ -11,7 +11,7 @@ // APM_Config.h and use APM_Config.h.example as a reference. // // WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING -// +/// ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// // @@ -23,8 +23,6 @@ // Output CH mapping for ArduCopter motor channels // // -#define CH_INVALID (-1) - #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 # define MOT_1 CH_1 # define MOT_2 CH_2 @@ -35,57 +33,6 @@ # define MOT_7 CH_7 # define MOT_8 CH_8 #elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM1 -#if FRAME_CONFIG == QUAD_FRAME -// X and Plus -# define MOT_1 CH_1 -# define MOT_2 CH_2 -# define MOT_3 CH_3 -# define MOT_4 CH_4 -// Placeholders: -# define MOT_5 CH_INVALID -# define MOT_6 CH_INVALID -# define MOT_7 CH_INVALID -# define MOT_8 CH_INVALID -#elif FRAME_CONFIG == TRI_FRAME -# define MOT_1 CH_1 -# define MOT_2 CH_2 -# define MOT_3 CH_4 -// Placeholders: -# define MOT_4 CH_INVALID -# define MOT_5 CH_INVALID -# define MOT_6 CH_INVALID -# define MOT_7 CH_INVALID -# define MOT_8 CH_INVALID -#elif FRAME_CONFIG == HEXA_FRAME -# define MOT_1 CH_7 -# define MOT_2 CH_3 -# define MOT_3 CH_2 -# define MOT_4 CH_8 -# define MOT_5 CH_4 -# define MOT_6 CH_1 -// Placeholders: -# define MOT_7 CH_INVALID -# define MOT_8 CH_INVALID -#elif FRAME_CONFIG == Y6_FRAME -# define MOT_1 CH_1 -# define MOT_2 CH_7 -# define MOT_3 CH_3 -# define MOT_4 CH_2 -# define MOT_5 CH_8 -# define MOT_6 CH_4 -// Placeholders: -# define MOT_7 CH_INVALID -# define MOT_8 CH_INVALID -#elif FRAME_CONFIG == OCTA_FRAME -# define MOT_1 CH_2 -# define MOT_2 CH_1 -# define MOT_3 CH_11 -# define MOT_4 CH_10 -# define MOT_5 CH_8 -# define MOT_6 CH_7 -# define MOT_7 CH_4 -# define MOT_8 CH_3 -#elif FRAME_CONFIG == OCTA_QUAD_FRAME # define MOT_1 CH_1 # define MOT_2 CH_2 # define MOT_3 CH_3 @@ -94,17 +41,7 @@ # define MOT_6 CH_8 # define MOT_7 CH_10 # define MOT_8 CH_11 -#else // HELI -# define MOT_1 CH_1 -# define MOT_2 CH_2 -# define MOT_3 CH_3 -# define MOT_4 CH_4 -# define MOT_5 CH_5 -# define MOT_6 CH_6 -# define MOT_7 CH_7 -# define MOT_8 CH_8 -#endif // FRAME_CONFIG -#endif // CONFIG_APM_HARDWARE +#endif // // @@ -123,10 +60,10 @@ // // #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 -/* Camera Pitch and Camera Roll: Not yet defined for APM2 +/* Camera Pitch and Camera Roll: Not yet defined for APM2 * They will likely be dependent on the frame config */ -# define CH_CAM_PITCH (-1) -# define CH_CAM_ROLL (-1) +# define CH_CAM_PITCH CH_11 +# define CH_CAM_ROLL CH_10 #elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM1 # define CH_CAM_PITCH CH_5 # define CH_CAM_ROLL CH_6 diff --git a/ArduCopter/motors_hexa.pde b/ArduCopter/motors_hexa.pde index 302e5b3b66..0dcd19daa7 100644 --- a/ArduCopter/motors_hexa.pde +++ b/ArduCopter/motors_hexa.pde @@ -37,41 +37,42 @@ static void output_motors_armed() g.rc_3.calc_pwm(); g.rc_4.calc_pwm(); - if(g.frame_orientation == X_FRAME){ + if(g.frame_orientation == X_FRAME){ roll_out = g.rc_1.pwm_out / 2; pitch_out = (float)g.rc_2.pwm_out * .866; - //LEFT SIDE - motor_out[MOT_2] = g.rc_3.radio_out + roll_out + pitch_out; // FRONT CW - motor_out[MOT_3] = g.rc_3.radio_out + g.rc_1.pwm_out; // MIDDLE CCW - motor_out[MOT_4] = g.rc_3.radio_out + roll_out - pitch_out; // BACK CW + //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 - //RIGHT SIDE - motor_out[MOT_1] = g.rc_3.radio_out - roll_out + pitch_out; // FRONT CCW - motor_out[MOT_6] = g.rc_3.radio_out - g.rc_1.pwm_out; // MIDDLE CW - motor_out[MOT_5] = g.rc_3.radio_out - roll_out - pitch_out; // BACK CCW - } else { + //right side + motor_out[MOT_1] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW Middle + motor_out[MOT_5] = g.rc_3.radio_out - roll_out + pitch_out; // CCW Front + motor_out[MOT_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW Back + + }else{ roll_out = (float)g.rc_1.pwm_out * .866; pitch_out = g.rc_2.pwm_out / 2; - //FRONT SIDE - motor_out[MOT_5] = g.rc_3.radio_out - roll_out + pitch_out; // FRONT RIGHT CCW - motor_out[MOT_6] = g.rc_3.radio_out + g.rc_2.pwm_out; // FRONT CW - motor_out[MOT_1] = g.rc_3.radio_out + roll_out + pitch_out; // FRONT LEFT CCW + //Front side + motor_out[MOT_1] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW FRONT + motor_out[MOT_5] = g.rc_3.radio_out + roll_out + pitch_out; // CCW FRONT LEFT + motor_out[MOT_4] = g.rc_3.radio_out - roll_out + pitch_out; // CCW FRONT RIGHT - //BACK SIDE - motor_out[MOT_2] = g.rc_3.radio_out + roll_out - pitch_out; // BACK LEFT CW - motor_out[MOT_3] = g.rc_3.radio_out - g.rc_2.pwm_out; // BACK CCW - motor_out[MOT_4] = g.rc_3.radio_out - roll_out - pitch_out; // BACK RIGHT CW - } + //Back side + motor_out[MOT_2] = g.rc_3.radio_out - g.rc_2.pwm_out; // CCW BACK + motor_out[MOT_3] = g.rc_3.radio_out + roll_out - pitch_out; // CW, BACK LEFT + motor_out[MOT_6] = g.rc_3.radio_out - roll_out - pitch_out; // CW BACK RIGHT + } // 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_2] += g.rc_4.pwm_out; // CCW + motor_out[MOT_5] += g.rc_4.pwm_out; // CCW + motor_out[MOT_4] += 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_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 @@ -99,9 +100,9 @@ static void output_motors_armed() motor_out[MOT_1] = g.rc_3.radio_min; motor_out[MOT_2] = g.rc_3.radio_min; motor_out[MOT_3] = g.rc_3.radio_min; - motor_out[MOT_4] = g.rc_3.radio_min; - motor_out[MOT_5] = g.rc_3.radio_min; - motor_out[MOT_6] = g.rc_3.radio_min; + motor_out[MOT_4] = g.rc_3.radio_min; + motor_out[MOT_5] = g.rc_3.radio_min; + motor_out[MOT_6] = g.rc_3.radio_min; } #endif @@ -166,43 +167,46 @@ static void output_motor_test() if(g.frame_orientation == X_FRAME){ - +// 31 +// 24 if(g.rc_1.control_in > 3000){ // right - motor_out[MOT_6] += 100; + motor_out[MOT_1] += 100; } if(g.rc_1.control_in < -3000){ // left - motor_out[MOT_3] += 100; + motor_out[MOT_2] += 100; } if(g.rc_2.control_in > 3000){ // back - motor_out[MOT_5] += 100; + motor_out[MOT_6] += 100; motor_out[MOT_4] += 100; } if(g.rc_2.control_in < -3000){ // front - motor_out[MOT_1] += 100; - motor_out[MOT_2] += 100; + motor_out[MOT_5] += 100; + motor_out[MOT_3] += 100; } }else{ - +// 3 +// 2 1 +// 4 if(g.rc_1.control_in > 3000){ // right - motor_out[MOT_1] += 100; + motor_out[MOT_4] += 100; motor_out[MOT_6] += 100; } if(g.rc_1.control_in < -3000){ // left + motor_out[MOT_5] += 100; motor_out[MOT_3] += 100; - motor_out[MOT_4] += 100; } if(g.rc_2.control_in > 3000){ // back - motor_out[MOT_5] += 100; + motor_out[MOT_2] += 100; } if(g.rc_2.control_in < -3000){ // front - motor_out[MOT_2] += 100; + motor_out[MOT_1] += 100; } } diff --git a/ArduCopter/motors_octa.pde b/ArduCopter/motors_octa.pde index 162fea7c0d..87cc1cd043 100644 --- a/ArduCopter/motors_octa.pde +++ b/ArduCopter/motors_octa.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#if FRAME_CONFIG == OCTA_FRAME +#if FRAME_CONFIG == OCTA_FRAME static void init_motors_out() { @@ -44,42 +44,42 @@ static void output_motors_armed() pitch_out = (float)g.rc_2.pwm_out * 0.4; //Front side - motor_out[MOT_2] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT - motor_out[MOT_3] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT + motor_out[MOT_1] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT + motor_out[MOT_5] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT //Back side - motor_out[MOT_6] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out; // CW BACK LEFT - motor_out[MOT_7] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out; // CCW BACK RIGHT + motor_out[MOT_2] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out; // CW BACK LEFT + motor_out[MOT_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out; // CCW BACK RIGHT //Left side - motor_out[MOT_4] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out; // CW LEFT FRONT - motor_out[MOT_5] = g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out; // CCW LEFT BACK + motor_out[MOT_7] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out; // CW LEFT FRONT + motor_out[MOT_6] = g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out; // CCW LEFT BACK //Right side - motor_out[MOT_1] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out; // CCW RIGHT FRONT - motor_out[MOT_8] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out; // CW RIGHT BACK + motor_out[MOT_8] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out; // CW RIGHT BACK + motor_out[MOT_3] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out; // CCW RIGHT FRONT }else if(g.frame_orientation == PLUS_FRAME){ roll_out = (float)g.rc_1.pwm_out * 0.71; pitch_out = (float)g.rc_2.pwm_out * 0.71; //Front side - motor_out[MOT_1] = g.rc_3.radio_out - roll_out + pitch_out; // CCW FRONT RIGHT - motor_out[MOT_2] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW FRONT - motor_out[MOT_3] = g.rc_3.radio_out + roll_out + pitch_out; // CCW FRONT LEFT + motor_out[MOT_1] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW FRONT + motor_out[MOT_3] = g.rc_3.radio_out - roll_out + pitch_out; // CCW FRONT RIGHT + motor_out[MOT_5] = g.rc_3.radio_out + roll_out + pitch_out; // CCW FRONT LEFT //Left side - motor_out[MOT_4] = g.rc_3.radio_out + g.rc_1.pwm_out; // CW LEFT + motor_out[MOT_7] = g.rc_3.radio_out + g.rc_1.pwm_out; // CW LEFT //Right side - motor_out[MOT_8] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW RIGHT + motor_out[MOT_8] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW RIGHT //Back side - motor_out[MOT_7] = g.rc_3.radio_out - roll_out - pitch_out; // CCW BACK RIGHT - motor_out[MOT_6] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW BACK - motor_out[MOT_5] = g.rc_3.radio_out + roll_out - pitch_out; // CCW BACK LEFT + motor_out[MOT_2] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW BACK + motor_out[MOT_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW BACK RIGHT + motor_out[MOT_6] = g.rc_3.radio_out + roll_out - pitch_out; // CCW BACK LEFT - }else if(g.frame_orientation == V_FRAME){ + }else if(g.frame_orientation == V_FRAME){ int roll_out2, pitch_out2; int roll_out3, pitch_out3; @@ -95,68 +95,55 @@ static void output_motors_armed() pitch_out4 = (float)g.rc_2.pwm_out * 0.98; //Front side - motor_out[MOT_4] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT - motor_out[MOT_6] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT + motor_out[MOT_7] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT + motor_out[MOT_5] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT //Left side - motor_out[MOT_2] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out2; // CW LEFT FRONT - motor_out[MOT_8] = g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out3; // CCW LEFT BACK + motor_out[MOT_1] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out2; // CW LEFT FRONT + motor_out[MOT_3] = g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out3; // CCW LEFT BACK //Right side - motor_out[MOT_5] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out3; // CW RIGHT BACK - motor_out[MOT_1] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out2; // CCW RIGHT FRONT + motor_out[MOT_2] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out3; // CW RIGHT BACK + motor_out[MOT_6] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out2; // CCW RIGHT FRONT //Back side - motor_out[MOT_3] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out4; // CW BACK LEFT - motor_out[MOT_7] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out4; // CCW BACK RIGHT + motor_out[MOT_8] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out4; // CW BACK LEFT + motor_out[MOT_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out4; // CCW BACK RIGHT - } + } - if ( g.frame_orientation == X_FRAME || g.frame_orientation == PLUS_FRAME ) { - // 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_3] += g.rc_4.pwm_out; // CCW + motor_out[MOT_4] += g.rc_4.pwm_out; // CCW + motor_out[MOT_5] += g.rc_4.pwm_out; // CCW + motor_out[MOT_6] += 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_1] -= g.rc_4.pwm_out; // CW + motor_out[MOT_2] -= g.rc_4.pwm_out; // CW + motor_out[MOT_7] -= g.rc_4.pwm_out; // CW + motor_out[MOT_8] -= g.rc_4.pwm_out; // CW - } else if ( g.frame_orientation == V_FRAME ) { - // Yaw of each motor is diferent in V frames. - motor_out[MOT_1] -= g.rc_4.pwm_out; // CW - motor_out[MOT_2] -= g.rc_4.pwm_out; // CW - motor_out[MOT_3] -= g.rc_4.pwm_out; // CW - motor_out[MOT_4] -= g.rc_4.pwm_out; // CW - - motor_out[MOT_5] += g.rc_4.pwm_out; // CCW - motor_out[MOT_6] += g.rc_4.pwm_out; // CCW - motor_out[MOT_7] += g.rc_4.pwm_out; // CCW - motor_out[MOT_8] += g.rc_4.pwm_out; // CCW - } // TODO add stability patch motor_out[MOT_1] = min(motor_out[MOT_1], out_max); motor_out[MOT_2] = min(motor_out[MOT_2], out_max); motor_out[MOT_3] = min(motor_out[MOT_3], out_max); motor_out[MOT_4] = min(motor_out[MOT_4], out_max); - motor_out[MOT_5] = min(motor_out[MOT_5], out_max); - motor_out[MOT_6] = min(motor_out[MOT_6], out_max); - motor_out[MOT_7] = min(motor_out[MOT_7], out_max); - motor_out[MOT_8] = min(motor_out[MOT_8], out_max); + motor_out[MOT_5] = min(motor_out[MOT_5], out_max); + motor_out[MOT_6] = min(motor_out[MOT_6], out_max); + motor_out[MOT_7] = min(motor_out[MOT_7], out_max); + motor_out[MOT_8] = min(motor_out[MOT_8], 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_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_7] = max(motor_out[MOT_7], out_min); - motor_out[MOT_8] = max(motor_out[MOT_8], out_min); + motor_out[MOT_6] = max(motor_out[MOT_6], out_min); + motor_out[MOT_7] = max(motor_out[MOT_7], out_min); + motor_out[MOT_8] = max(motor_out[MOT_8], out_min); #if CUT_MOTORS == ENABLED @@ -165,11 +152,11 @@ static void output_motors_armed() motor_out[MOT_1] = g.rc_3.radio_min; motor_out[MOT_2] = g.rc_3.radio_min; motor_out[MOT_3] = g.rc_3.radio_min; - motor_out[MOT_4] = g.rc_3.radio_min; - motor_out[MOT_5] = g.rc_3.radio_min; - motor_out[MOT_6] = g.rc_3.radio_min; - motor_out[MOT_7] = g.rc_3.radio_min; - motor_out[MOT_8] = g.rc_3.radio_min; + motor_out[MOT_4] = g.rc_3.radio_min; + motor_out[MOT_5] = g.rc_3.radio_min; + motor_out[MOT_6] = g.rc_3.radio_min; + motor_out[MOT_7] = g.rc_3.radio_min; + motor_out[MOT_8] = g.rc_3.radio_min; } #endif @@ -218,12 +205,13 @@ static void output_motors_disarmed() // Send commands to motors APM_RC.OutputCh(MOT_1, g.rc_3.radio_min); APM_RC.OutputCh(MOT_2, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_5, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_6, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_8, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_3, g.rc_3.radio_min); APM_RC.OutputCh(MOT_4, g.rc_3.radio_min); - APM_RC.OutputCh(MOT_5, g.rc_3.radio_min); - APM_RC.OutputCh(MOT_6, g.rc_3.radio_min); APM_RC.OutputCh(MOT_7, g.rc_3.radio_min); - APM_RC.OutputCh(MOT_8, g.rc_3.radio_min); } static void output_motor_test() @@ -300,3 +288,4 @@ static void output_motor_test() } #endif + diff --git a/ArduCopter/motors_octa_quad.pde b/ArduCopter/motors_octa_quad.pde index 2dfae46ff7..cb331ec95d 100644 --- a/ArduCopter/motors_octa_quad.pde +++ b/ArduCopter/motors_octa_quad.pde @@ -40,63 +40,79 @@ static void output_motors_armed() g.rc_4.calc_pwm(); if(g.frame_orientation == X_FRAME){ - roll_out = g.rc_1.pwm_out * .707; - pitch_out = g.rc_2.pwm_out * .707; + roll_out = (float)g.rc_1.pwm_out * .707; + pitch_out = (float)g.rc_2.pwm_out * .707; - motor_out[MOT_1] = g.rc_3.radio_out - roll_out + pitch_out; // OUT 1 FRONT RIGHT CCW BOTTOM - motor_out[MOT_2] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // OUT 2 FRONT RIGHT CW TOP - motor_out[MOT_3] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // OUT 3 FRONT LEFT CCW TOP - motor_out[MOT_4] = g.rc_3.radio_out + roll_out + pitch_out; // OUT 4 FRONT LEFT CW BOTTOM - motor_out[MOT_5] = g.rc_3.radio_out + roll_out - pitch_out; // OUT 5 BACK LEFT CCW BOTTOM - motor_out[MOT_6] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out - pitch_out); // OUT 6 BACK LEFT CW TOP - motor_out[MOT_7] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out - pitch_out); // OUT 7 BACK RIGHT CCW TOP - motor_out[MOT_8] = g.rc_3.radio_out - roll_out - pitch_out; // OUT 8 BACK RIGHT CW BOTTOM + // Front Left + motor_out[MOT_5] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // CCW TOP + motor_out[MOT_6] = g.rc_3.radio_out + roll_out + pitch_out; // CW + + // Front Right + motor_out[MOT_7] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // CCW TOP + motor_out[MOT_8] = g.rc_3.radio_out - roll_out + pitch_out; // CW + + // Back Left + motor_out[MOT_3] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out - pitch_out); // CCW TOP + motor_out[MOT_4] = g.rc_3.radio_out + roll_out - pitch_out; // CW + + // Back Right + motor_out[MOT_1] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out - pitch_out); // CCW TOP + motor_out[MOT_2] = g.rc_3.radio_out - roll_out - pitch_out; // CW - }else{ + + }if(g.frame_orientation == PLUS_FRAME){ roll_out = g.rc_1.pwm_out; pitch_out = g.rc_2.pwm_out; - motor_out[MOT_1] = g.rc_3.radio_out + pitch_out; //OUT 1 FRONT CCW BOTTOM - motor_out[MOT_2] = (g.rc_3.radio_out * g.top_bottom_ratio) + pitch_out; //OUT 2 FRONT CW TOP - motor_out[MOT_3] = (g.rc_3.radio_out * g.top_bottom_ratio) - roll_out; //OUT 3 LEFT CCW TOP - motor_out[MOT_4] = g.rc_3.radio_out - roll_out; //OUT 4 LEFT CW BOTTOM - motor_out[MOT_5] = g.rc_3.radio_out - pitch_out; //OUT 5 BACK CCW BOTTOM - motor_out[MOT_6] = (g.rc_3.radio_out * g.top_bottom_ratio) - pitch_out; //OUT 6 BACK CW TOP - motor_out[MOT_7] = (g.rc_3.radio_out * g.top_bottom_ratio) + roll_out; //OUT 7 RIGHT CCW TOP - motor_out[MOT_8] = g.rc_3.radio_out + roll_out; //OUT 8 RIGHT CW BOTTOM + // Left + motor_out[MOT_5] = (g.rc_3.radio_out * g.top_bottom_ratio) - roll_out; // CCW TOP + motor_out[MOT_6] = g.rc_3.radio_out - roll_out; // CW + + // Right + motor_out[MOT_1] = (g.rc_3.radio_out * g.top_bottom_ratio) + roll_out; // CCW TOP + motor_out[MOT_2] = g.rc_3.radio_out + roll_out; // CW + + // Front + motor_out[MOT_7] = (g.rc_3.radio_out * g.top_bottom_ratio) + pitch_out; // CCW TOP + motor_out[MOT_8] = g.rc_3.radio_out + pitch_out; // CW + + // Back + motor_out[MOT_3] = (g.rc_3.radio_out * g.top_bottom_ratio) - pitch_out; // CCW TOP + motor_out[MOT_4] = g.rc_3.radio_out - pitch_out; // CW + } // 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_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_8] -= g.rc_4.pwm_out; // CW // TODO add stability patch motor_out[MOT_1] = min(motor_out[MOT_1], out_max); motor_out[MOT_2] = min(motor_out[MOT_2], out_max); motor_out[MOT_3] = min(motor_out[MOT_3], out_max); motor_out[MOT_4] = min(motor_out[MOT_4], out_max); - motor_out[MOT_5] = min(motor_out[MOT_5], out_max); - motor_out[MOT_6] = min(motor_out[MOT_6], out_max); - motor_out[MOT_7] = min(motor_out[MOT_7], out_max); - motor_out[MOT_8] = min(motor_out[MOT_8], out_max); + motor_out[MOT_5] = min(motor_out[MOT_5], out_max); + motor_out[MOT_6] = min(motor_out[MOT_6], out_max); + motor_out[MOT_7] = min(motor_out[MOT_7], out_max); + motor_out[MOT_8] = min(motor_out[MOT_8], 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_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_7] = max(motor_out[MOT_7], out_min); - motor_out[MOT_8] = max(motor_out[MOT_8], out_min); + motor_out[MOT_6] = max(motor_out[MOT_6], out_min); + motor_out[MOT_7] = max(motor_out[MOT_7], out_min); + motor_out[MOT_8] = max(motor_out[MOT_8], out_min); #if CUT_MOTORS == ENABLED // if we are not sending a throttle output, we cut the motors @@ -104,11 +120,11 @@ static void output_motors_armed() motor_out[MOT_1] = g.rc_3.radio_min; motor_out[MOT_2] = g.rc_3.radio_min; motor_out[MOT_3] = g.rc_3.radio_min; - motor_out[MOT_4] = g.rc_3.radio_min; - motor_out[MOT_5] = g.rc_3.radio_min; - motor_out[MOT_6] = g.rc_3.radio_min; - motor_out[MOT_7] = g.rc_3.radio_min; - motor_out[MOT_8] = g.rc_3.radio_min; + motor_out[MOT_4] = g.rc_3.radio_min; + motor_out[MOT_5] = g.rc_3.radio_min; + motor_out[MOT_6] = g.rc_3.radio_min; + motor_out[MOT_7] = g.rc_3.radio_min; + motor_out[MOT_8] = g.rc_3.radio_min; } #endif diff --git a/ArduCopter/motors_tri.pde b/ArduCopter/motors_tri.pde index 4b6f06c67f..3fbdb49e8f 100644 --- a/ArduCopter/motors_tri.pde +++ b/ArduCopter/motors_tri.pde @@ -4,7 +4,7 @@ static void init_motors_out() { #if INSTANT_PWM == 0 - APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) ); + APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_4) ); #endif } @@ -40,46 +40,46 @@ static void output_motors_armed() //right front motor_out[MOT_1] = g.rc_3.radio_out - roll_out + pitch_out; // rear - motor_out[MOT_3] = g.rc_3.radio_out - g.rc_2.pwm_out; + motor_out[MOT_4] = g.rc_3.radio_out - g.rc_2.pwm_out; - //motor_out[MOT_3] += (float)(abs(g.rc_4.control_in)) * .013; + //motor_out[MOT_4] += (float)(abs(g.rc_4.control_in)) * .013; // Tridge's stability patch if (motor_out[MOT_1] > out_max) { motor_out[MOT_2] -= (motor_out[MOT_1] - out_max) >> 1; - motor_out[MOT_3] -= (motor_out[MOT_1] - out_max) >> 1; + motor_out[MOT_4] -= (motor_out[MOT_1] - out_max) >> 1; motor_out[MOT_1] = out_max; } if (motor_out[MOT_2] > out_max) { motor_out[MOT_1] -= (motor_out[MOT_2] - out_max) >> 1; - motor_out[MOT_3] -= (motor_out[MOT_2] - out_max) >> 1; + motor_out[MOT_4] -= (motor_out[MOT_2] - out_max) >> 1; motor_out[MOT_2] = out_max; } - if (motor_out[MOT_3] > out_max) { - motor_out[MOT_1] -= (motor_out[MOT_3] - out_max) >> 1; - motor_out[MOT_2] -= (motor_out[MOT_3] - out_max) >> 1; - motor_out[MOT_3] = out_max; + if (motor_out[MOT_4] > out_max) { + motor_out[MOT_1] -= (motor_out[MOT_4] - out_max) >> 1; + motor_out[MOT_2] -= (motor_out[MOT_4] - out_max) >> 1; + motor_out[MOT_4] = 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); #if CUT_MOTORS == ENABLED // if we are not sending a throttle output, we cut the motors if(g.rc_3.servo_out == 0){ motor_out[MOT_1] = g.rc_3.radio_min; motor_out[MOT_2] = g.rc_3.radio_min; - motor_out[MOT_3] = g.rc_3.radio_min; + motor_out[MOT_4] = g.rc_3.radio_min; } #endif 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]); + APM_RC.OutputCh(MOT_4, motor_out[MOT_4]); #if INSTANT_PWM == 1 // InstantPWM @@ -104,14 +104,14 @@ static void output_motors_disarmed() // Send commands to motors APM_RC.OutputCh(MOT_1, g.rc_3.radio_min); APM_RC.OutputCh(MOT_2, g.rc_3.radio_min); - APM_RC.OutputCh(MOT_3, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_4, g.rc_3.radio_min); } static void output_motor_test() { motor_out[MOT_1] = g.rc_3.radio_min; motor_out[MOT_2] = g.rc_3.radio_min; - motor_out[MOT_3] = g.rc_3.radio_min; + motor_out[MOT_4] = g.rc_3.radio_min; if(g.rc_1.control_in > 3000){ // right @@ -123,12 +123,12 @@ static void output_motor_test() } if(g.rc_2.control_in > 3000){ // back - motor_out[MOT_3] += 100; + motor_out[MOT_4] += 100; } 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]); + APM_RC.OutputCh(MOT_4, motor_out[MOT_4]); } -#endif \ No newline at end of file +#endif diff --git a/ArduCopter/motors_y6.pde b/ArduCopter/motors_y6.pde index 646c887198..991ced7e49 100644 --- a/ArduCopter/motors_y6.pde +++ b/ArduCopter/motors_y6.pde @@ -38,61 +38,71 @@ static void output_motors_armed() g.rc_3.calc_pwm(); g.rc_4.calc_pwm(); - // MULTI-WII MIX + // Multi-Wii Mix + //left + motor_out[MOT_2] = (g.rc_3.radio_out * g.top_bottom_ratio) + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // LEFT TOP - CW + motor_out[MOT_3] = g.rc_3.radio_out + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // BOTTOM_LEFT - CCW + //right + motor_out[MOT_5] = (g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // RIGHT TOP - CW + motor_out[MOT_1] = g.rc_3.radio_out - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // BOTTOM_RIGHT - CCW + //back + motor_out[MOT_6] = (g.rc_3.radio_out * g.top_bottom_ratio) - (g.rc_2.pwm_out * 4 / 3); // REAR TOP - CCW + motor_out[MOT_4] = g.rc_3.radio_out - (g.rc_2.pwm_out * 4 / 3); // BOTTOM_REAR - CW - motor_out[MOT_1] = g.rc_3.radio_out - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // OUT 1 RIGHT BOTTOM CCW - motor_out[MOT_2] = (g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // OUT 2 RIGHT TOP CW - motor_out[MOT_3] = g.rc_3.radio_out + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // OUT 3 LEFT BOTTOM CCW - motor_out[MOT_4] = (g.rc_3.radio_out * g.top_bottom_ratio) + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // OUT 4 LEFT TOP CW - motor_out[MOT_5] = (g.rc_3.radio_out * g.top_bottom_ratio) - (g.rc_2.pwm_out * 4 / 3); // OUT 5 REAR TOP CCW - motor_out[MOT_6] = g.rc_3.radio_out - (g.rc_2.pwm_out * 4 / 3); // OUT 6 REAR BOTTOM CW - - // YAW - - motor_out[MOT_1] += YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT BOTTOM CCW - motor_out[MOT_2] -= YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT TOP CW - motor_out[MOT_3] += YAW_DIRECTION * g.rc_4.pwm_out; // LEFT BOTTOM CCW - motor_out[MOT_4] -= YAW_DIRECTION * g.rc_4.pwm_out; // LEFT TOP CW - motor_out[MOT_5] += YAW_DIRECTION * g.rc_4.pwm_out; // REAR TOP CCW - motor_out[MOT_6] -= YAW_DIRECTION * g.rc_4.pwm_out; // REAR BOTTOM CW + //left + motor_out[MOT_2] -= YAW_DIRECTION * g.rc_4.pwm_out; // LEFT TOP - CW + motor_out[MOT_3] += YAW_DIRECTION * g.rc_4.pwm_out; // LEFT BOTTOM - CCW + //right + motor_out[MOT_5] -= YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT TOP - CW + motor_out[MOT_1] += YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT BOTTOM - CCW + //back + motor_out[MOT_6] += YAW_DIRECTION * g.rc_4.pwm_out; // REAR TOP - CCW + motor_out[MOT_4] -= YAW_DIRECTION * g.rc_4.pwm_out; // REAR BOTTOM - CW /* int roll_out = (float)g.rc_1.pwm_out * .866; int pitch_out = g.rc_2.pwm_out / 2; - motor_out[MOT_1] = g.rc_3.radio_out - roll_out + pitch_out; // RIGHT BOTTOM CCW - motor_out[MOT_2] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // RIGHT TOP CW - motor_out[MOT_3] = g.rc_3.radio_out + roll_out + pitch_out; // LEFT BOTTOM CCW - motor_out[MOT_4] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // LEFT TOP CW - motor_out[MOT_5] = ((g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_2.pwm_out); // REAR TOP CCW - motor_out[MOT_6] = g.rc_3.radio_out - g.rc_2.pwm_out; // REAR BOTTOM CW + //left + motor_out[MOT_2] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // CCW TOP + motor_out[MOT_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW + + //right + motor_out[MOT_5] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // CCW TOP + motor_out[MOT_1] = g.rc_3.radio_out - roll_out + pitch_out; // CW + + //back + motor_out[MOT_6] = ((g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_2.pwm_out); // CCW TOP + motor_out[MOT_4] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW // Yaw + //top + motor_out[MOT_2] += g.rc_4.pwm_out; // CCW + motor_out[MOT_5] += g.rc_4.pwm_out; // CCW + motor_out[MOT_6] += g.rc_4.pwm_out; // CCW - motor_out[MOT_1] += g.rc_4.pwm_out; // CCW - motor_out[MOT_2] -= g.rc_4.pwm_out; // CW - motor_out[MOT_3] += g.rc_4.pwm_out; // CCW - motor_out[MOT_4] -= g.rc_4.pwm_out; // CW - motor_out[MOT_5] += g.rc_4.pwm_out; // CCW - motor_out[MOT_6] -= g.rc_4.pwm_out; // CW + //bottom + motor_out[MOT_3] -= g.rc_4.pwm_out; // CW + motor_out[MOT_1] -= g.rc_4.pwm_out; // CW + motor_out[MOT_4] -= g.rc_4.pwm_out; // CW */ // TODO: add stability patch - motor_out[MOT_1] = min(motor_out[MOT_1], out_max); - motor_out[MOT_2] = min(motor_out[MOT_2], out_max); - motor_out[MOT_3] = min(motor_out[MOT_3], out_max); - motor_out[MOT_4] = min(motor_out[MOT_4], out_max); - motor_out[MOT_5] = min(motor_out[MOT_5], out_max); - motor_out[MOT_6] = min(motor_out[MOT_6], out_max); + motor_out[MOT_1] = min(motor_out[MOT_1], out_max); + motor_out[MOT_2] = min(motor_out[MOT_2], out_max); + motor_out[MOT_3] = min(motor_out[MOT_3], out_max); + motor_out[MOT_4] = min(motor_out[MOT_4], out_max); + motor_out[MOT_5] = min(motor_out[MOT_5], out_max); + motor_out[MOT_6] = min(motor_out[MOT_6], 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 @@ -100,9 +110,9 @@ static void output_motors_armed() motor_out[MOT_1] = g.rc_3.radio_min; motor_out[MOT_2] = g.rc_3.radio_min; motor_out[MOT_3] = g.rc_3.radio_min; - motor_out[MOT_4] = g.rc_3.radio_min; - motor_out[MOT_5] = g.rc_3.radio_min; - motor_out[MOT_6] = g.rc_3.radio_min; + motor_out[MOT_4] = g.rc_3.radio_min; + motor_out[MOT_5] = g.rc_3.radio_min; + motor_out[MOT_6] = g.rc_3.radio_min; } #endif @@ -167,17 +177,17 @@ static void output_motor_test() if(g.rc_1.control_in > 3000){ // right motor_out[MOT_1] += 100; - motor_out[MOT_2] += 100; + motor_out[MOT_5] += 100; } if(g.rc_1.control_in < -3000){ // left - motor_out[MOT_4] += 100; + motor_out[MOT_2] += 100; motor_out[MOT_3] += 100; } if(g.rc_2.control_in > 3000){ // back motor_out[MOT_6] += 100; - motor_out[MOT_5] += 100; + motor_out[MOT_4] += 100; } APM_RC.OutputCh(MOT_1, motor_out[MOT_1]);