diff --git a/ArduCopter/config_channels.h b/ArduCopter/config_channels.h index b3a1e7263d..bf9113a5cf 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,6 +23,8 @@ // 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 @@ -33,6 +35,77 @@ # 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_X_FRAME +// X orientation (what will be the best way to set 'if' 'else'? ) +# 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 == HEXA_PLUS_FRAME +# define MOT_1 CH_4 +# define MOT_2 CH_1 +# define MOT_3 CH_7 +# define MOT_4 CH_3 +# define MOT_5 CH_2 +# define MOT_6 CH_8 +// 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_V_FRAME +# define MOT_1 CH_4 +# define MOT_2 CH_2 +# define MOT_3 CH_8 +# define MOT_4 CH_10 +# define MOT_5 CH_7 +# define MOT_6 CH_1 +# define MOT_7 CH_3 +# define MOT_8 CH_11 +#elif FRAME_CONFIG == OCTA_QUAD_FRAME # define MOT_1 CH_1 # define MOT_2 CH_2 # define MOT_3 CH_3 @@ -41,7 +114,17 @@ # define MOT_6 CH_8 # define MOT_7 CH_10 # define MOT_8 CH_11 -#endif +#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 // // @@ -60,10 +143,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 CH_11 -# define CH_CAM_ROLL CH_10 +# define CH_CAM_PITCH (-1) +# define CH_CAM_ROLL (-1) #elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM1 # define CH_CAM_PITCH CH_5 # define CH_CAM_ROLL CH_6 diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index d4601ec17f..060298de05 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -46,11 +46,13 @@ // Frame types #define QUAD_FRAME 0 #define TRI_FRAME 1 -#define HEXA_FRAME 2 +#define HEXA_X_FRAME 2 #define Y6_FRAME 3 #define OCTA_FRAME 4 #define HELI_FRAME 5 #define OCTA_QUAD_FRAME 6 +#define HEXA_PLUS_FRAME 7 +#define OCTA_V_FRAME 8 #define PLUS_FRAME 0 #define X_FRAME 1 diff --git a/ArduCopter/motors_hexa.pde b/ArduCopter/motors_hexa.pde index 0dcd19daa7..9ba93413c6 100644 --- a/ArduCopter/motors_hexa.pde +++ b/ArduCopter/motors_hexa.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#if FRAME_CONFIG == HEXA_FRAME +#if (FRAME_CONFIG == HEXA_X_FRAME) || (FRAME_CONFIG == HEXA_PLUS_FRAME) static void init_motors_out() { @@ -37,42 +37,41 @@ static void output_motors_armed() g.rc_3.calc_pwm(); g.rc_4.calc_pwm(); - if(g.frame_orientation == X_FRAME){ +#if FRAME_CONFIG == HEXA_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 + 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 + //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 - //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{ + //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 +#elif FRAME_CONFIG == HEXA_PLUS_FRAME roll_out = (float)g.rc_1.pwm_out * .866; pitch_out = g.rc_2.pwm_out / 2; - //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 + //FRONT SIDE + motor_out[MOT_1] = g.rc_3.radio_out - roll_out + pitch_out; // FRONT RIGHT CCW + motor_out[MOT_2] = g.rc_3.radio_out + g.rc_2.pwm_out; // FRONT CW + motor_out[MOT_3] = g.rc_3.radio_out + roll_out + pitch_out; // FRONT LEFT CCW - //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 - } + //BACK SIDE + motor_out[MOT_4] = g.rc_3.radio_out + roll_out - pitch_out; // BACK LEFT CW + motor_out[MOT_5] = g.rc_3.radio_out - g.rc_2.pwm_out; // BACK CCW + motor_out[MOT_6] = g.rc_3.radio_out - roll_out - pitch_out; // BACK RIGHT CW +#endif // Yaw - 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_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_3] -= 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_4] -= g.rc_4.pwm_out; // CW motor_out[MOT_6] -= g.rc_4.pwm_out; // CW @@ -100,9 +99,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,46 +166,43 @@ static void output_motor_test() if(g.frame_orientation == X_FRAME){ -// 31 -// 24 + if(g.rc_1.control_in > 3000){ // right - motor_out[MOT_1] += 100; + motor_out[MOT_6] += 100; } if(g.rc_1.control_in < -3000){ // left - 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; } if(g.rc_2.control_in < -3000){ // front - motor_out[MOT_5] += 100; - motor_out[MOT_3] += 100; + motor_out[MOT_1] += 100; + motor_out[MOT_2] += 100; } }else{ -// 3 -// 2 1 -// 4 + if(g.rc_1.control_in > 3000){ // right - motor_out[MOT_4] += 100; + motor_out[MOT_1] += 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_2] += 100; + motor_out[MOT_5] += 100; } if(g.rc_2.control_in < -3000){ // front - motor_out[MOT_1] += 100; + motor_out[MOT_2] += 100; } } diff --git a/ArduCopter/motors_octa.pde b/ArduCopter/motors_octa.pde index 87cc1cd043..357b62a893 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) || (FRAME_CONFIG == OCTA_V_FRAME) static void init_motors_out() { @@ -39,47 +39,48 @@ static void output_motors_armed() g.rc_3.calc_pwm(); g.rc_4.calc_pwm(); +#if FRAME_TYPE == OCTA_FRAME if(g.frame_orientation == X_FRAME){ roll_out = (float)g.rc_1.pwm_out * 0.4; pitch_out = (float)g.rc_2.pwm_out * 0.4; //Front side - 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 + 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 //Back side - 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 + 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 //Left side - 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 + 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 //Right side - 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 + 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 }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 + 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 + 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 //Left side - motor_out[MOT_7] = g.rc_3.radio_out + g.rc_1.pwm_out; // CW LEFT + motor_out[MOT_4] = 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_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){ + 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 + } +#elif FRAME_TYPE == OCTA_V_FRAME int roll_out2, pitch_out2; int roll_out3, pitch_out3; @@ -95,33 +96,33 @@ static void output_motors_armed() pitch_out4 = (float)g.rc_2.pwm_out * 0.98; //Front side - motor_out[MOT_7] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT + motor_out[MOT_4] = 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_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 + motor_out[MOT_6] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out2; // CW LEFT FRONT + motor_out[MOT_7] = g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out3; // CCW LEFT BACK //Right side - 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 + motor_out[MOT_3] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out3; // CW RIGHT BACK + motor_out[MOT_2] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out2; // CCW RIGHT FRONT //Back side 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 + motor_out[MOT_1] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out4; // CCW BACK RIGHT - } +#endif // FRAME_TYPE // Yaw + motor_out[MOT_1] += g.rc_4.pwm_out; // CCW 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_5] += g.rc_4.pwm_out; // CCW + motor_out[MOT_7] += g.rc_4.pwm_out; // CCW - 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 + 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 @@ -129,21 +130,21 @@ static void output_motors_armed() 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 @@ -152,11 +153,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 @@ -205,13 +206,12 @@ 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() @@ -288,4 +288,3 @@ static void output_motor_test() } #endif - diff --git a/ArduCopter/motors_octa_quad.pde b/ArduCopter/motors_octa_quad.pde index cb331ec95d..2dfae46ff7 100644 --- a/ArduCopter/motors_octa_quad.pde +++ b/ArduCopter/motors_octa_quad.pde @@ -40,79 +40,63 @@ static void output_motors_armed() g.rc_4.calc_pwm(); if(g.frame_orientation == X_FRAME){ - roll_out = (float)g.rc_1.pwm_out * .707; - pitch_out = (float)g.rc_2.pwm_out * .707; + roll_out = g.rc_1.pwm_out * .707; + pitch_out = g.rc_2.pwm_out * .707; - // 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 + 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 - - }if(g.frame_orientation == PLUS_FRAME){ + }else{ roll_out = g.rc_1.pwm_out; pitch_out = g.rc_2.pwm_out; - // 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 - + 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 } // 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 @@ -120,11 +104,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 3fbdb49e8f..4b6f06c67f 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_4) ); + APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) ); #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_4] = g.rc_3.radio_out - g.rc_2.pwm_out; + motor_out[MOT_3] = g.rc_3.radio_out - g.rc_2.pwm_out; - //motor_out[MOT_4] += (float)(abs(g.rc_4.control_in)) * .013; + //motor_out[MOT_3] += (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_4] -= (motor_out[MOT_1] - out_max) >> 1; + motor_out[MOT_3] -= (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_4] -= (motor_out[MOT_2] - out_max) >> 1; + motor_out[MOT_3] -= (motor_out[MOT_2] - out_max) >> 1; motor_out[MOT_2] = 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; + 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; } // 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_4] = max(motor_out[MOT_4], out_min); + motor_out[MOT_3] = max(motor_out[MOT_3], 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_4] = g.rc_3.radio_min; + motor_out[MOT_3] = 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_4, motor_out[MOT_4]); + APM_RC.OutputCh(MOT_3, motor_out[MOT_3]); #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_4, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_3, 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_4] = g.rc_3.radio_min; + motor_out[MOT_3] = 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_4] += 100; + motor_out[MOT_3] += 100; } APM_RC.OutputCh(MOT_1, motor_out[MOT_1]); APM_RC.OutputCh(MOT_2, motor_out[MOT_2]); - APM_RC.OutputCh(MOT_4, motor_out[MOT_4]); + APM_RC.OutputCh(MOT_3, motor_out[MOT_3]); } -#endif +#endif \ No newline at end of file diff --git a/ArduCopter/motors_y6.pde b/ArduCopter/motors_y6.pde index 991ced7e49..646c887198 100644 --- a/ArduCopter/motors_y6.pde +++ b/ArduCopter/motors_y6.pde @@ -38,71 +38,61 @@ static void output_motors_armed() g.rc_3.calc_pwm(); g.rc_4.calc_pwm(); - // 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 + // MULTI-WII MIX - //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 + 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 /* int roll_out = (float)g.rc_1.pwm_out * .866; int pitch_out = g.rc_2.pwm_out / 2; - //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 + 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 // 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 - //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 + 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 */ // 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 @@ -110,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 @@ -177,17 +167,17 @@ static void output_motor_test() if(g.rc_1.control_in > 3000){ // right motor_out[MOT_1] += 100; - motor_out[MOT_5] += 100; + motor_out[MOT_2] += 100; } if(g.rc_1.control_in < -3000){ // left - motor_out[MOT_2] += 100; + motor_out[MOT_4] += 100; motor_out[MOT_3] += 100; } if(g.rc_2.control_in > 3000){ // back motor_out[MOT_6] += 100; - motor_out[MOT_4] += 100; + motor_out[MOT_5] += 100; } APM_RC.OutputCh(MOT_1, motor_out[MOT_1]);