mirror of https://github.com/ArduPilot/ardupilot
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.
This commit is contained in:
Pat Hickey
parent
297e9052a6
commit
b65eb110bd
|
|
@ -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
|
||||
|
||||
//
|
||||
//
|
||||
|
|
@ -125,8 +62,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 (-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
|
||||
|
|
|
|||
|
|
@ -41,37 +41,38 @@ static void output_motors_armed()
|
|||
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_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
|
||||
|
||||
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -44,40 +44,40 @@ 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_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
|
||||
|
||||
//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){
|
||||
|
||||
|
|
@ -95,47 +95,34 @@ 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
|
||||
|
||||
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_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_7] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[MOT_8] += 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
|
||||
|
||||
|
||||
// TODO add stability patch
|
||||
motor_out[MOT_1] = min(motor_out[MOT_1], out_max);
|
||||
|
|
@ -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_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);
|
||||
|
||||
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
|
||||
}
|
||||
|
||||
static void output_motor_test()
|
||||
|
|
@ -300,3 +288,4 @@ static void output_motor_test()
|
|||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -40,31 +40,47 @@ 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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -38,44 +38,54 @@ 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
|
||||
|
||||
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
|
||||
//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; // CW
|
||||
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
|
||||
*/
|
||||
|
||||
// TODO: add stability patch
|
||||
|
|
@ -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]);
|
||||
|
|
|
|||
Loading…
Reference in New Issue