static void servoMixer(void)
{
int16_t input[INPUT_ITEMS];
static int16_t currentOutput[MAX_SERVO_RULES];
uint8_t i;
if (f.PASSTHRU_MODE) {
// Direct passthru from RX
input[INPUT_ROLL] = rcCommand[ROLL];
input[INPUT_PITCH] = rcCommand[PITCH];
input[INPUT_YAW] = rcCommand[YAW];
} else {
// Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
input[INPUT_ROLL] = axisPID[ROLL];
input[INPUT_PITCH] = axisPID[PITCH];
input[INPUT_YAW] = axisPID[YAW];
}
input[INPUT_THROTTLE] = motor[0];
input[INPUT_AUX1] = mcfg.midrc - rcData[AUX1];
input[INPUT_AUX2] = mcfg.midrc - rcData[AUX2];
input[INPUT_AUX3] = mcfg.midrc - rcData[AUX3];
input[INPUT_AUX4] = mcfg.midrc - rcData[AUX4];
input[INPUT_RC_ROLL] = mcfg.midrc - rcData[ROLL];
input[INPUT_RC_PITCH] = mcfg.midrc - rcData[PITCH];
input[INPUT_RC_YAW] = mcfg.midrc - rcData[YAW];
input[INPUT_RC_THROTTLE] = mcfg.midrc - rcData[THROTTLE];
for (i = 0; i < MAX_SERVOS; i++)
servo[i] = servoMiddle(i);
// mix servos according to rules
for (i = 0; i < numberRules; i++) {
// consider rule if no box assigned or box is active
if (currentServoMixer[i].box == 0 || rcOptions[BOXSERVO1 + currentServoMixer[i].box - 1]) {
uint8_t target = currentServoMixer[i].targetChannel;
uint8_t from = currentServoMixer[i].fromChannel;
uint16_t servo_width = cfg.servoConf[target].max - cfg.servoConf[target].min;
int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2;
int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2;
if (currentServoMixer[i].speed == 0)
currentOutput[i] = input[from];
else {
if (currentOutput[i] < input[from])
currentOutput[i] = constrain(currentOutput[i] + currentServoMixer[i].speed, currentOutput[i], input[from]);
else if (currentOutput[i] > input[from])
currentOutput[i] = constrain(currentOutput[i] - currentServoMixer[i].speed, input[from], currentOutput[i]);
}
servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max);
} else
currentOutput[i] = 0;
}
// servo rates
for (i = 0; i < MAX_SERVOS; i++)
servo[i] = ((int32_t)cfg.servoConf[i].rate * servo[i]) / 100;
}
static void resetServos(void)
{
int i;
// reset all servos to their middle value
for (i = 0; i < MAX_SERVOS; i++)
servo[i] = servoMiddle(i);
}
void mixTable(void)
{
int16_t maxMotor;
uint32_t i;
if (numberMotor > 3) {
// prevent "yaw jump" during yaw correction
axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW]));
}
// motors for non-servo mixes
if (numberMotor > 1)
{
for (i = 0; i < numberMotor; i++)
{
motor[i] =
rcCommand[THROTTLE] * currentMixer[i].throttle +
axisPID[PITCH] * currentMixer[i].pitch +
axisPID[ROLL] * currentMixer[i].roll +
-cfg.yaw_direction * axisPID[YAW] * currentMixer[i].yaw;
}
}
if (f.FIXED_WING) {
if (!f.ARMED)
motor[0] = mcfg.mincommand; // Kill throttle when disarmed
else
motor[0] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle);
}
// reset all servos
if (core.useServo)
resetServos();
if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL) {
// set servo output for gimbal type
servo[0] = (((int32_t)cfg.servoConf[0].rate * angle[PITCH]) / 50) + servoMiddle(0);
servo[1] = (((int32_t)cfg.servoConf[1].rate * angle[ROLL]) / 50) + servoMiddle(1);
}
// set camstab servo output before applying servo mixer rules
if (feature(FEATURE_SERVO_TILT)) {
// vary either pitch or roll by RC channel
if (rcOptions[BOXCAMSTAB]) {
if (cfg.gimbal_flags & GIMBAL_MIXTILT) {
servo[0] -= (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 - (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50;
servo[1] += (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 + (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50;
} else {
servo[0] += (int32_t)cfg.servoConf[0].rate * angle[PITCH] / 50;
servo[1] += (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50;
}
}
}
// run the servo mixer if necessary
if (core.useServo)
servoMixer();
// constrain servos
for (i = 0; i < MAX_SERVOS; i++)
servo[i] = constrain(servo[i], cfg.servoConf[i].min, cfg.servoConf[i].max); // limit the values
maxMotor = motor[0];
for (i = 1; i < numberMotor; i++)
if (motor[i] > maxMotor)
maxMotor = motor[i];
for (i = 0; i < numberMotor; i++)
{
if (maxMotor > mcfg.maxthrottle && !f.FIXED_WING) // this is a way to still have good gyro corrections if at least one motor reaches its max.
motor[i] -= maxMotor - mcfg.maxthrottle;
if (feature(FEATURE_3D))
{
if ((rcData[THROTTLE]) > mcfg.midrc)
{
motor[i] = constrain(motor[i], mcfg.deadband3d_high, mcfg.maxthrottle);
if ((mcfg.mixerConfiguration) == MULTITYPE_TRI) {
servo[5] = constrain(servo[5], cfg.servoConf[5].min, cfg.servoConf[5].max);
}
}
else
{
motor[i] = constrain(motor[i], mcfg.mincommand, mcfg.deadband3d_low);
if ((mcfg.mixerConfiguration) == MULTITYPE_TRI) {
servo[5] = constrain(servo[5], cfg.servoConf[5].max, cfg.servoConf[5].min);
}
}
}
else
{
motor[i] = constrain(motor[i], mcfg.minthrottle, mcfg.maxthrottle);
if ((rcData[THROTTLE]) < mcfg.mincheck)
{
if (!feature(FEATURE_MOTOR_STOP))
motor[i] = mcfg.minthrottle;
else {
motor[i] = mcfg.mincommand;
f.MOTORS_STOPPED = 1;
}
}
else
{
f.MOTORS_STOPPED = 0;
}
}
if (!f.ARMED) {
motor[i] = motor_disarmed[i];
}
}
}
static void servoMixer(void)
{
int16_t input[INPUT_ITEMS];
static int16_t currentOutput[MAX_SERVO_RULES];
uint8_t i;
if (f.PASSTHRU_MODE) {
// Direct passthru from RX
input[INPUT_ROLL] = rcCommand[ROLL];
input[INPUT_PITCH] = rcCommand[PITCH];
input[INPUT_YAW] = rcCommand[YAW];
} else {
// Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
input[INPUT_ROLL] = axisPID[ROLL];
input[INPUT_PITCH] = axisPID[PITCH];
input[INPUT_YAW] = axisPID[YAW];
}
input[INPUT_THROTTLE] = motor[0];
// center the RC input value around the RC middle value
// by subtracting the RC middle value from the RC input value, we get:
// data - middle = input
// 2000 - 1500 = +500
// 1500 - 1500 = 0
// 1000 - 1500 = -500
input[INPUT_AUX1] = rcData[AUX1] - mcfg.midrc;
input[INPUT_AUX2] = rcData[AUX2] - mcfg.midrc;
input[INPUT_AUX3] = rcData[AUX3] - mcfg.midrc;
input[INPUT_AUX4] = rcData[AUX4] - mcfg.midrc;
input[INPUT_RC_ROLL] = rcData[ROLL] - mcfg.midrc;
input[INPUT_RC_PITCH] = rcData[PITCH] - mcfg.midrc;
input[INPUT_RC_YAW] = rcData[YAW] - mcfg.midrc;
input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - mcfg.midrc;
// mix servos according to rules
for (i = 0; i < numberRules; i++) {
// consider rule if no box assigned or box is active
if (currentServoMixer[i].box == 0 || rcOptions[BOXSERVO1 + currentServoMixer[i].box - 1]) {
uint8_t target = currentServoMixer[i].targetChannel;
uint8_t from = currentServoMixer[i].fromChannel;
uint16_t servo_width = cfg.servoConf[target].max - cfg.servoConf[target].min;
int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2;
int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2;
if (currentServoMixer[i].speed == 0) {
// directly use the input value if speed is not provided
currentOutput[i] = input[from];
} else {
// apply speed constraints
if (currentOutput[i] < input[from])
currentOutput[i] = constrain(currentOutput[i] + currentServoMixer[i].speed, currentOutput[i], input[from]);
else if (currentOutput[i] > input[from])
currentOutput[i] = constrain(currentOutput[i] - currentServoMixer[i].speed, input[from], currentOutput[i]);
}
// start with the output value
servo[target] = (int16_t)currentOutput[i];
// apply rate from mixer rule
servo[target] *= ((float)currentServoMixer[i].rate / 100);
// apply rate fro m servoconfiguration
servo[target] *= ((float)cfg.servoConf[target].rate / 100);
// constrain the width of the servo's movement
servo[target] = constrain(servo[target], min, max);
// reverse direction if necessary
servo[target] *= servoDirection(target, from);
// center the servo around its middle
servo[target] += servoMiddle(i);
}
}
}
void mixTable(void)
{
int16_t maxMotor;
uint32_t i;
if (numberMotor > 3) {
// prevent "yaw jump" during yaw correction
axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW]));
}
// motors for non-servo mixes
if (numberMotor > 1)
for (i = 0; i < numberMotor; i++)
motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + -cfg.yaw_direction * axisPID[YAW] * currentMixer[i].yaw;
if (f.FIXED_WING) {
if (!f.ARMED)
motor[0] = mcfg.mincommand; // Kill throttle when disarmed
else
motor[0] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle);
}
// airplane / servo mixes
switch (mcfg.mixerConfiguration) {
case MULTITYPE_CUSTOM_PLANE:
case MULTITYPE_FLYING_WING:
case MULTITYPE_AIRPLANE:
case MULTITYPE_BI:
case MULTITYPE_TRI:
case MULTITYPE_DUALCOPTER:
case MULTITYPE_SINGLECOPTER:
servoMixer();
break;
case MULTITYPE_GIMBAL:
servo[0] = (((int32_t)cfg.servoConf[0].rate * angle[PITCH]) / 50) + servoMiddle(0);
servo[1] = (((int32_t)cfg.servoConf[1].rate * angle[ROLL]) / 50) + servoMiddle(1);
break;
}
// do camstab
if (feature(FEATURE_SERVO_TILT)) {
// center at fixed position, or vary either pitch or roll by RC channel
servo[0] = servoMiddle(0);
servo[1] = servoMiddle(1);
if (rcOptions[BOXCAMSTAB]) {
if (cfg.gimbal_flags & GIMBAL_MIXTILT) {
servo[0] -= (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 - (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50;
servo[1] += (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 + (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50;
} else {
servo[0] += (int32_t)cfg.servoConf[0].rate * angle[PITCH] / 50;
servo[1] += (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50;
}
}
}
// constrain servos
for (i = 0; i < MAX_SERVOS; i++)
servo[i] = constrain(servo[i], cfg.servoConf[i].min, cfg.servoConf[i].max); // limit the values
// forward AUX1-4 to servo outputs (not constrained)
if (cfg.gimbal_flags & GIMBAL_FORWARDAUX) {
int offset = core.numServos - 4;
// offset servos based off number already used in mixer types
// airplane and servo_tilt together can't be used
// calculate offset by taking 4 from core.numServos
for (i = 0; i < 4; i++)
pwmWriteServo(i + offset, rcData[AUX1 + i]);
}
maxMotor = motor[0];
for (i = 1; i < numberMotor; i++)
if (motor[i] > maxMotor)
maxMotor = motor[i];
for (i = 0; i < numberMotor; i++) {
if (maxMotor > mcfg.maxthrottle) // this is a way to still have good gyro corrections if at least one motor reaches its max.
motor[i] -= maxMotor - mcfg.maxthrottle;
if (feature(FEATURE_3D)) {
if ((rcData[THROTTLE]) > mcfg.midrc) {
motor[i] = constrain(motor[i], mcfg.deadband3d_high, mcfg.maxthrottle);
if ((mcfg.mixerConfiguration) == MULTITYPE_TRI) {
servo[5] = constrain(servo[5], cfg.servoConf[5].min, cfg.servoConf[5].max);
}
} else {
motor[i] = constrain(motor[i], mcfg.mincommand, mcfg.deadband3d_low);
if ((mcfg.mixerConfiguration) == MULTITYPE_TRI) {
servo[5] = constrain(servo[5], cfg.servoConf[5].max, cfg.servoConf[5].min);
}
}
} else {
motor[i] = constrain(motor[i], mcfg.minthrottle, mcfg.maxthrottle);
if ((rcData[THROTTLE]) < mcfg.mincheck) {
if (!feature(FEATURE_MOTOR_STOP))
motor[i] = mcfg.minthrottle;
else {
motor[i] = mcfg.mincommand;
f.MOTORS_STOPPED = 1;
}
} else {
f.MOTORS_STOPPED = 0;
}
}
if (!f.ARMED) {
motor[i] = motor_disarmed[i];
}
}
}