mirror of https://github.com/ArduPilot/ardupilot
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.
This commit is contained in:
Pat Hickey
parent
1a1f6cac5d
commit
f32d089f33
|
|
@ -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
|
||||
|
||||
//
|
||||
//
|
||||
|
|
@ -62,8 +145,8 @@
|
|||
#if CONFIG_APM_HARDWARE == APM_HARDWARE_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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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]);
|
||||
|
|
|
|||
Loading…
Reference in New Issue