From 1a1f6cac5d8f4f5a95f67774f19d51ab1b3365f8 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 1 Feb 2012 08:04:18 +0900 Subject: [PATCH 01/13] OptFlow - reenable OF_LOITER pid controller and reduce I term --- ArduCopter/Attitude.pde | 4 ++-- ArduCopter/config.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 26e12f3610..5942e69583 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -441,7 +441,7 @@ get_of_roll(int32_t control_roll) // only stop roll if caller isn't modifying roll if( control_roll == 0 && current_loc.alt < 1500) { - //new_roll = g.pid_optflow_roll.get_pid(-tot_x_cm, 1.0, 1.0); // we could use the last update time to calculate the time change + new_roll = g.pid_optflow_roll.get_pid(-tot_x_cm, 1.0); // we could use the last update time to calculate the time change }else{ g.pid_optflow_roll.reset_I(); tot_x_cm = 0; @@ -475,7 +475,7 @@ get_of_pitch(int32_t control_pitch) // only stop roll if caller isn't modifying pitch if( control_pitch == 0 && current_loc.alt < 1500 ) { - //new_pitch = g.pid_optflow_pitch.get_pid(tot_y_cm, 1.0, 1.0); // we could use the last update time to calculate the time change + new_pitch = g.pid_optflow_pitch.get_pid(tot_y_cm, 1.0); // we could use the last update time to calculate the time change }else{ tot_y_cm = 0; g.pid_optflow_pitch.reset_I(); diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 43abc0c984..2bf3a93d60 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -324,7 +324,7 @@ #define OPTFLOW_ROLL_P 2.5 #endif #ifndef OPTFLOW_ROLL_I - #define OPTFLOW_ROLL_I 6.2 + #define OPTFLOW_ROLL_I 3.2 #endif #ifndef OPTFLOW_ROLL_D #define OPTFLOW_ROLL_D 0.12 @@ -333,7 +333,7 @@ #define OPTFLOW_PITCH_P 2.5 #endif #ifndef OPTFLOW_PITCH_I - #define OPTFLOW_PITCH_I 6.2 + #define OPTFLOW_PITCH_I 3.2 #endif #ifndef OPTFLOW_PITCH_D #define OPTFLOW_PITCH_D 0.12 From f32d089f33d610a9ee23b24fe0dd9d0f6e4af513 Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Sun, 15 Jan 2012 14:57:14 -0800 Subject: [PATCH 02/13] Add Max's changes for new MOT mappings. Defines new FRAME_CONFIG types. * HEXA_FRAME has been eliminated and changed to HEXA_X_FRAME and HEXA_PLUS_FRAME * OCTA_FRAME stays the same name for X and +, but the V frame configuration requires new OCTA_V_FRAME name. --- ArduCopter/config_channels.h | 93 +++++++++++++++++++++++++++-- ArduCopter/defines.h | 4 +- ArduCopter/motors_hexa.pde | 82 ++++++++++++------------- ArduCopter/motors_octa.pde | 99 +++++++++++++++---------------- ArduCopter/motors_octa_quad.pde | 86 +++++++++++---------------- ArduCopter/motors_tri.pde | 34 +++++------ ArduCopter/motors_y6.pde | 102 ++++++++++++++------------------ 7 files changed, 277 insertions(+), 223 deletions(-) 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]); From 8463acc978bd4cf8d11089029a0ee96393e682bf Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Wed, 25 Jan 2012 22:22:28 -0800 Subject: [PATCH 03/13] Arducopter Frames: revert to old HEXA_FRAME and OCTA_FRAME defines. * HEXA_X_FRAME and HEXA_PLUS_FRAME merged back into HEXA_FRAME * OCTA_V_FRAME merged back into OCTA_FRAME --- ArduCopter/config_channels.h | 22 +--------------------- ArduCopter/defines.h | 4 +--- 2 files changed, 2 insertions(+), 24 deletions(-) diff --git a/ArduCopter/config_channels.h b/ArduCopter/config_channels.h index bf9113a5cf..d3f34118ee 100644 --- a/ArduCopter/config_channels.h +++ b/ArduCopter/config_channels.h @@ -56,8 +56,7 @@ # 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'? ) +#elif FRAME_CONFIG == HEXA_FRAME # define MOT_1 CH_7 # define MOT_2 CH_3 # define MOT_3 CH_2 @@ -67,16 +66,6 @@ // 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 @@ -96,15 +85,6 @@ # 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 diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 060298de05..d4601ec17f 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -46,13 +46,11 @@ // Frame types #define QUAD_FRAME 0 #define TRI_FRAME 1 -#define HEXA_X_FRAME 2 +#define HEXA_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 From 5cee2b674b9cf60e8c1fe84094dae8305e9d045c Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Wed, 25 Jan 2012 22:30:01 -0800 Subject: [PATCH 04/13] Arducopter Motors Hexa: Revert HEXA_PLUS_ and HEXA_X_ to single HEXA_FRAME --- 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 9ba93413c6..2befbcb2a4 100644 --- a/ArduCopter/motors_hexa.pde +++ b/ArduCopter/motors_hexa.pde @@ -37,7 +37,7 @@ static void output_motors_armed() g.rc_3.calc_pwm(); g.rc_4.calc_pwm(); -#if FRAME_CONFIG == HEXA_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; @@ -50,7 +50,7 @@ static void output_motors_armed() 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 + } else { roll_out = (float)g.rc_1.pwm_out * .866; pitch_out = g.rc_2.pwm_out / 2; @@ -63,7 +63,7 @@ static void output_motors_armed() 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_1] += g.rc_4.pwm_out; // CCW From 7d264d1150a660ab0ad6a014226a6b09806386b4 Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Wed, 25 Jan 2012 22:30:53 -0800 Subject: [PATCH 05/13] Arducopter Motors Hexa: Change ordering of MOT designations in Plus frame * This will change the output order for APM2, but not APM1. --- ArduCopter/motors_hexa.pde | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ArduCopter/motors_hexa.pde b/ArduCopter/motors_hexa.pde index 2befbcb2a4..1efb2f87fd 100644 --- a/ArduCopter/motors_hexa.pde +++ b/ArduCopter/motors_hexa.pde @@ -55,14 +55,14 @@ static void output_motors_armed() pitch_out = g.rc_2.pwm_out / 2; //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 + 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 //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 + 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 } // Yaw From 41ea9079be959ec21ccf0a20e78a9fccfe914c71 Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Wed, 25 Jan 2012 22:31:56 -0800 Subject: [PATCH 06/13] Arducopter Motors Octa: revert OCTA_V_FRAME back to part of OCTA_FRAME --- ArduCopter/motors_octa.pde | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/ArduCopter/motors_octa.pde b/ArduCopter/motors_octa.pde index 357b62a893..9fe79057ba 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) || (FRAME_CONFIG == OCTA_V_FRAME) +#if FRAME_CONFIG == OCTA_FRAME static void init_motors_out() { @@ -39,7 +39,6 @@ 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; @@ -79,8 +78,8 @@ static void output_motors_armed() 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 + + }else if(g.frame_orientation == V_FRAME){ int roll_out2, pitch_out2; int roll_out3, pitch_out3; @@ -111,8 +110,8 @@ static void output_motors_armed() motor_out[MOT_8] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out4; // CW BACK LEFT 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 From 417d75eb507baeddeef4f50425390ea4f5ce8cd1 Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Wed, 25 Jan 2012 22:32:28 -0800 Subject: [PATCH 07/13] hexa frame FRAME_CONFIG fixup --- ArduCopter/motors_hexa.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/motors_hexa.pde b/ArduCopter/motors_hexa.pde index 1efb2f87fd..302e5b3b66 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_X_FRAME) || (FRAME_CONFIG == HEXA_PLUS_FRAME) +#if FRAME_CONFIG == HEXA_FRAME static void init_motors_out() { From ca18eb3f8f456c9c12d3db27c60529cc9d6865bb Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Wed, 25 Jan 2012 22:38:04 -0800 Subject: [PATCH 08/13] Arducopter Motors Octa: Change V frame MOT_ output ordering. * I backed this out from Max's changes, but I'm not sure whether its correct. --- ArduCopter/motors_octa.pde | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/ArduCopter/motors_octa.pde b/ArduCopter/motors_octa.pde index 9fe79057ba..50f25f6039 100644 --- a/ArduCopter/motors_octa.pde +++ b/ArduCopter/motors_octa.pde @@ -96,19 +96,20 @@ static void output_motors_armed() //Front side 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 + motor_out[MOT_6] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT //Left side - 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 + 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 //Right side - 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 + 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 //Back side - motor_out[MOT_8] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out4; // CW BACK LEFT - motor_out[MOT_1] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out4; // CCW BACK RIGHT + 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 + } From 48b152d642324db3e2b393ea9ed160a6e4f8aca8 Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Tue, 31 Jan 2012 19:37:49 -0800 Subject: [PATCH 09/13] ArduCopter Octa: fix yaw motors for Octa V. * I based this off the APM1 Octa V diagram at http://code.google.com/p/arducopter/wiki/AC2_Multi --- ArduCopter/motors_octa.pde | 34 +++++++++++++++++++++++----------- 1 file changed, 23 insertions(+), 11 deletions(-) diff --git a/ArduCopter/motors_octa.pde b/ArduCopter/motors_octa.pde index 50f25f6039..162fea7c0d 100644 --- a/ArduCopter/motors_octa.pde +++ b/ArduCopter/motors_octa.pde @@ -110,20 +110,32 @@ static void output_motors_armed() 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 - - } - // 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_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 + 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 + 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 + + } 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); From d86dbd1c6f0fd8371757f00bc851b0224304bd95 Mon Sep 17 00:00:00 2001 From: analoguedevices Date: Wed, 1 Feb 2012 04:39:15 +0000 Subject: [PATCH 10/13] Incremented version number to 2.3 --- ArduCopter/ArduCopter.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index cdc503b89c..3d9711b719 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V2.2 b6" +#define THISFIRMWARE "ArduCopter V2.3" /* ArduCopter Version 2.2 Authors: Jason Short @@ -547,7 +547,7 @@ static bool low_batt = false; static int32_t ground_pressure; // The ground temperature at home location - calibrated at arming static int16_t ground_temperature; -// The cm we are off in altitude from next_WP.alt – Positive value means we are below the WP +// The cm we are off in altitude from next_WP.alt – Positive value means we are below the WP static int32_t altitude_error; // The cm/s we are moving up or down - Positive = UP static int16_t climb_rate; From 256893f3856a9ba43598951d7122b209e23ec859 Mon Sep 17 00:00:00 2001 From: analoguedevices Date: Wed, 1 Feb 2012 04:44:36 +0000 Subject: [PATCH 11/13] credits --- ArduCopter/ArduCopter.pde | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 3d9711b719..42ca4ae449 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -2,9 +2,9 @@ #define THISFIRMWARE "ArduCopter V2.3" /* -ArduCopter Version 2.2 +ArduCopter Version 2.3 Authors: Jason Short -Based on code and ideas from the Arducopter team: Jose Julio, Randy Mackay, Jani Hirvinen +Based on code and ideas from the Arducopter team: Randy Mackay, Pat Hickey, Jose Julio, Jani Hirvinen Thanks to: Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert, Benjamin Pelletier This firmware is free software; you can redistribute it and/or @@ -32,6 +32,9 @@ Oliver :Piezo support Guntars :Arming safety suggestion Igor van Airde :Control Law optimization Jean-Louis Naudin :Auto Landing +Sandro Benigno : Camera support +Olivier Adler : PPM Encoder +John Arne Birkeland: PPM Encoder And much more so PLEASE PM me on DIYDRONES to add your contribution to the List From 297e9052a6e7e1b37b1e43dacc0a28fd8aed5395 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Wed, 1 Feb 2012 12:48:46 +0800 Subject: [PATCH 12/13] firmware build --- .../Firmware/firmware2.xml | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml b/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml index ea468fde09..344ff72262 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml +++ b/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml @@ -47,68 +47,68 @@ http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560-2.hex - ArduCopter V2.2 b4 Quad + ArduCopter V2.3 Quad #define FRAME_CONFIG QUAD_FRAME - 114 + 115 http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560-2.hex - ArduCopter V2.2 b4 Tri + ArduCopter V2.3 Tri #define FRAME_CONFIG TRI_FRAME - 114 + 115 http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560-2.hex - ArduCopter V2.2 b4 Hexa X + ArduCopter V2.3 Hexa X #define FRAME_CONFIG HEXA_FRAME - 114 + 115 http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560-2.hex - ArduCopter V2.2 b4 Y6 + ArduCopter V2.3 Y6 #define FRAME_CONFIG Y6_FRAME - 114 + 115 http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octav-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octav-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octav-2560-2.hex - ArduCopter V2.2 b4 Octa V + ArduCopter V2.3 Octa V #define FRAME_CONFIG OCTA_FRAME #define FRAME_ORIENTATION V_FRAME - 114 + 115 http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octa-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octa-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octa-2560-2.hex - ArduCopter V2.2 b4 Octa X + ArduCopter V2.3 Octa X #define FRAME_CONFIG OCTA_FRAME - 114 + 115 http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.hex @@ -162,7 +162,7 @@ http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560-2.hex - ArduCopter V2.2 b4 Quad Hil + ArduCopter V2.3 Quad Hil #define FRAME_CONFIG QUAD_FRAME #define FRAME_ORIENTATION PLUS_FRAME @@ -172,7 +172,7 @@ - 114 + 115 http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-1280.hex From b65eb110bde0847a2dbebe1cbbb8025fed7f02fd Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Wed, 1 Feb 2012 10:10:51 -0800 Subject: [PATCH 13/13] Revert all changes to ArduCopter Motors MOT mappings. Revert "Arducopter Motors Octa: Change V frame MOT_ output ordering." This reverts commit bdab02f408ddc5451fcb4c6390b2475d31ac657f. Revert "hexa frame FRAME_CONFIG fixup" This reverts commit d15e692df613cb728ec671a54dce166f6f1940a0. Revert "Arducopter Motors Octa: revert OCTA_V_FRAME back to part of OCTA_FRAME" This reverts commit cb0a8c62fbd07a8ae9dcb8d4fffce337ace1aa1c. Revert "Arducopter Motors Hexa: Change ordering of MOT designations in Plus frame" This reverts commit 120d7f9050d5ec9f8fbe02c0ed4f38621949f4ee. Revert "Arducopter Motors Hexa: Revert HEXA_PLUS_ and HEXA_X_ to single HEXA_FRAME" This reverts commit 7d65ec311fd2e1222a36d0b34c366e21f3869fcc. Revert "Arducopter Frames: revert to old HEXA_FRAME and OCTA_FRAME defines." This reverts commit 47c6e8662f4d5e8fb920f2049338541343d8d18e. Revert "Add Max's changes for new MOT mappings. Defines new FRAME_CONFIG types." This reverts commit 8259c90ec7cb29dedac19890cd9a4449b7399e36. --- ArduCopter/config_channels.h | 73 ++------------------ ArduCopter/motors_hexa.pde | 80 +++++++++++---------- ArduCopter/motors_octa.pde | 119 +++++++++++++++----------------- ArduCopter/motors_octa_quad.pde | 86 +++++++++++++---------- ArduCopter/motors_tri.pde | 34 ++++----- ArduCopter/motors_y6.pde | 102 +++++++++++++++------------ 6 files changed, 225 insertions(+), 269 deletions(-) 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]);