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_UNIT_TESTED void servoMixer(void)
{
int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]
static int16_t currentOutput[MAX_SERVO_RULES];
uint8_t i;
if (FLIGHT_MODE(PASSTHRU_MODE)) {
// Direct passthru from RX
input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL];
input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH];
input[INPUT_STABILIZED_YAW] = rcCommand[YAW];
} else {
// Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
input[INPUT_STABILIZED_ROLL] = axisPID[ROLL];
input[INPUT_STABILIZED_PITCH] = axisPID[PITCH];
input[INPUT_STABILIZED_YAW] = axisPID[YAW];
// Reverse yaw servo when inverted in 3D mode
if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {
input[INPUT_STABILIZED_YAW] *= -1;
}
}
input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -1800, 1800, -500, +500);
input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -1800, 1800, -500, +500);
input[INPUT_STABILIZED_THROTTLE] = motor[0] - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500]
// 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_RC_ROLL] = rcData[ROLL] - rxConfig->midrc;
input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig->midrc;
input[INPUT_RC_YAW] = rcData[YAW] - rxConfig->midrc;
input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig->midrc;
input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig->midrc;
input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig->midrc;
input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig->midrc;
input[INPUT_RC_AUX4] = rcData[AUX4] - rxConfig->midrc;
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++)
servo[i] = 0;
// mix servos according to rules
for (i = 0; i < servoRuleCount; i++) {
// consider rule if no box assigned or box is active
if (currentServoMixer[i].box == 0 || IS_RC_MODE_ACTIVE(BOXSERVO1 + currentServoMixer[i].box - 1)) {
uint8_t target = currentServoMixer[i].targetChannel;
uint8_t from = currentServoMixer[i].inputSource;
uint16_t servo_width = servoConf[target].max - 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;
}
}
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L;
servo[i] += determineServoMiddleOrForwardFromChannel(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);
}
}
}
