1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 22:05:17 +03:00

add untested mixers - HEX6H, dual/single copter, to match mixer IDs with MultiWiiConf

This commit is contained in:
dongie 2013-11-02 08:17:31 +09:00
parent 203e46daa6
commit 7eec3aec9e
3 changed files with 54 additions and 3 deletions

View file

@ -37,7 +37,9 @@ static const char * const mixerNames[] = {
"TRI", "QUADP", "QUADX", "BI", "TRI", "QUADP", "QUADX", "BI",
"GIMBAL", "Y6", "HEX6", "GIMBAL", "Y6", "HEX6",
"FLYING_WING", "Y4", "HEX6X", "OCTOX8", "OCTOFLATP", "OCTOFLATX", "FLYING_WING", "Y4", "HEX6X", "OCTOX8", "OCTOFLATP", "OCTOFLATX",
"AIRPLANE", "HELI_120_CCPM", "HELI_90_DEG", "VTAIL4", "CUSTOM", NULL "AIRPLANE", "HELI_120_CCPM", "HELI_90_DEG", "VTAIL4",
"HEX6H", "PPM_TO_SERVO", "DUALCOPTER", "SINGLECOPTER",
"CUSTOM", NULL
}; };
// sync this with AvailableFeatures enum from board.h // sync this with AvailableFeatures enum from board.h

View file

@ -106,6 +106,20 @@ static const motorMixer_t mixerVtail4[] = {
{ 1.0f, 1.0f, -1.0f, -0.0f }, // FRONT_L { 1.0f, 1.0f, -1.0f, -0.0f }, // FRONT_L
}; };
static const motorMixer_t mixerHex6H[] = {
{ 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R
{ 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R
{ 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L
{ 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L
{ 1.0f, 0.0f, 0.0f, 0.0f }, // RIGHT
{ 1.0f, 0.0f, 0.0f, 0.0f }, // LEFT
};
static const motorMixer_t mixerDualcopter[] = {
{ 1.0f, 0.0f, 0.0f, -1.0f }, // LEFT
{ 1.0f, 0.0f, 0.0f, 1.0f }, // RIGHT
};
// Keep this synced with MultiType struct in mw.h! // Keep this synced with MultiType struct in mw.h!
const mixer_t mixers[] = { const mixer_t mixers[] = {
// Mo Se Mixtable // Mo Se Mixtable
@ -127,6 +141,10 @@ const mixer_t mixers[] = {
{ 0, 1, NULL }, // * MULTITYPE_HELI_120_CCPM { 0, 1, NULL }, // * MULTITYPE_HELI_120_CCPM
{ 0, 1, NULL }, // * MULTITYPE_HELI_90_DEG { 0, 1, NULL }, // * MULTITYPE_HELI_90_DEG
{ 4, 0, mixerVtail4 }, // MULTITYPE_VTAIL4 { 4, 0, mixerVtail4 }, // MULTITYPE_VTAIL4
{ 6, 0, mixerHex6H }, // MULTITYPE_HEX6H
{ 0, 1, NULL }, // * MULTITYPE_PPM_TO_SERVO
{ 2, 1, mixerDualcopter }, // MULTITYPE_DUALCOPTER
{ 1, 1, NULL }, // MULTITYPE_SINGLECOPTER
{ 0, 0, NULL }, // MULTITYPE_CUSTOM { 0, 0, NULL }, // MULTITYPE_CUSTOM
}; };
@ -249,6 +267,18 @@ void writeServos(void)
pwmWriteServo(1, servo[1]); pwmWriteServo(1, servo[1]);
break; break;
case MULTITYPE_DUALCOPTER:
pwmWriteServo(0, servo[4]);
pwmWriteServo(1, servo[5]);
break;
case MULTITYPE_SINGLECOPTER:
pwmWriteServo(0, servo[3]);
pwmWriteServo(1, servo[4]);
pwmWriteServo(2, servo[5]);
pwmWriteServo(3, servo[6]);
break;
default: default:
// Two servos for SERVO_TILT, if enabled // Two servos for SERVO_TILT, if enabled
if (feature(FEATURE_SERVO_TILT)) { if (feature(FEATURE_SERVO_TILT)) {
@ -382,6 +412,21 @@ void mixTable(void)
servo[3] += servoMiddle(3); servo[3] += servoMiddle(3);
servo[4] += servoMiddle(4); servo[4] += servoMiddle(4);
break; break;
case MULTITYPE_DUALCOPTER:
for (i = 4; i < 6; i++ ) {
servo[i] = axisPID[5 - i] * servoDirection(i, 1); // mix and setup direction
servo[i] += servoMiddle(i);
}
break;
case MULTITYPE_SINGLECOPTER:
for (i = 3; i < 7; i++) {
servo[i] = (axisPID[YAW] * servoDirection(i, 2)) + (axisPID[(6 - i) >> 1] * servoDirection(i, 1)); // mix and setup direction
servo[i] += servoMiddle(i);
}
motor[0] = rcCommand[THROTTLE];
break;
} }
// do camstab // do camstab

View file

@ -40,8 +40,12 @@ typedef enum MultiType
MULTITYPE_HELI_120_CCPM = 15, MULTITYPE_HELI_120_CCPM = 15,
MULTITYPE_HELI_90_DEG = 16, MULTITYPE_HELI_90_DEG = 16,
MULTITYPE_VTAIL4 = 17, MULTITYPE_VTAIL4 = 17,
MULTITYPE_CUSTOM = 18, // no current GUI displays this MULTITYPE_HEX6H = 18,
MULTITYPE_LAST = 19 MULTITYPE_PPM_TO_SERVO = 19, // PPM -> servo relay
MULTITYPE_DUALCOPTER = 20,
MULTITYPE_SINGLECOPTER = 21,
MULTITYPE_CUSTOM = 22, // no current GUI displays this
MULTITYPE_LAST = 23
} MultiType; } MultiType;
typedef enum GimbalFlags { typedef enum GimbalFlags {