void servoTable(void)
{
// airplane / servo mixes
switch (currentMixerMode) {
case MIXER_CUSTOM_AIRPLANE:
case MIXER_FLYING_WING:
case MIXER_AIRPLANE:
case MIXER_BICOPTER:
case MIXER_CUSTOM_TRI:
case MIXER_TRI:
case MIXER_DUALCOPTER:
case MIXER_SINGLECOPTER:
case MIXER_GIMBAL:
servoMixer();
break;
/*
case MIXER_GIMBAL:
servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
break;
*/
default:
break;
}
// camera stabilization
if (feature(FEATURE_SERVO_TILT)) {
// center at fixed position, or vary either pitch or roll by RC channel
servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) {
if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) {
servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
} else {
servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch / 50;
servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
}
}
}
// constrain servos
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
servo[i] = constrain(servo[i], servoConf[i].min, servoConf[i].max); // limit the values
}
}
void mixTable(void)
{
uint32_t i;
if (motorCount >= 4 && mixerConfig->yaw_jump_prevention_limit < YAW_JUMP_PREVENTION_LIMIT_HIGH) {
// prevent "yaw jump" during yaw correction
axisPID[YAW] = constrain(axisPID[YAW], -mixerConfig->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig->yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
}
// motors for non-servo mixes
for (i = 0; i < motorCount; i++) {
motor[i] =
rcCommand[THROTTLE] * currentMixer[i].throttle +
axisPID[PITCH] * currentMixer[i].pitch +
axisPID[ROLL] * currentMixer[i].roll +
-mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw;
}
if (ARMING_FLAG(ARMED)) {
bool isFailsafeActive = failsafeIsActive();
// Find the maximum motor output.
int16_t maxMotor = motor[0];
for (i = 1; i < motorCount; i++) {
// If one motor is above the maxthrottle threshold, we reduce the value
// of all motors by the amount of overshoot. That way, only one motor
// is at max and the relative power of each motor is preserved.
if (motor[i] > maxMotor) {
maxMotor = motor[i];
}
}
int16_t maxThrottleDifference = 0;
if (maxMotor > escAndServoConfig->maxthrottle) {
maxThrottleDifference = maxMotor - escAndServoConfig->maxthrottle;
}
for (i = 0; i < motorCount; i++) {
// this is a way to still have good gyro corrections if at least one motor reaches its max.
motor[i] -= maxThrottleDifference;
if (feature(FEATURE_3D)) {
if (mixerConfig->pid_at_min_throttle
|| rcData[THROTTLE] <= rxConfig->midrc - flight3DConfig->deadband3d_throttle
|| rcData[THROTTLE] >= rxConfig->midrc + flight3DConfig->deadband3d_throttle) {
if (rcData[THROTTLE] > rxConfig->midrc) {
motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle);
} else {
motor[i] = constrain(motor[i], escAndServoConfig->mincommand, flight3DConfig->deadband3d_low);
}
} else {
if (rcData[THROTTLE] > rxConfig->midrc) {
motor[i] = flight3DConfig->deadband3d_high;
} else {
motor[i] = flight3DConfig->deadband3d_low;
}
}
} else {
if (isFailsafeActive) {
motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle);
} else {
// If we're at minimum throttle and FEATURE_MOTOR_STOP enabled,
// do not spin the motors.
motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
if ((rcData[THROTTLE]) < rxConfig->mincheck) {
if (feature(FEATURE_MOTOR_STOP)) {
motor[i] = escAndServoConfig->mincommand;
} else if (mixerConfig->pid_at_min_throttle == 0) {
motor[i] = escAndServoConfig->minthrottle;
}
}
}
}
}
} else {
for (i = 0; i < motorCount; i++) {
motor[i] = motor_disarmed[i];
}
}
// motor outputs are used as sources for servo mixing, so motors must be calculated before servos.
#if !defined(USE_QUAD_MIXER_ONLY) && defined(USE_SERVOS)
// airplane / servo mixes
switch (currentMixerMode) {
case MIXER_CUSTOM_AIRPLANE:
case MIXER_FLYING_WING:
case MIXER_AIRPLANE:
case MIXER_BICOPTER:
case MIXER_CUSTOM_TRI:
case MIXER_TRI:
case MIXER_DUALCOPTER:
case MIXER_SINGLECOPTER:
case MIXER_GIMBAL:
servoMixer();
break;
/*
case MIXER_GIMBAL:
servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
break;
*/
default:
break;
}
// camera stabilization
if (feature(FEATURE_SERVO_TILT)) {
// center at fixed position, or vary either pitch or roll by RC channel
servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) {
if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) {
servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
} else {
servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch / 50;
servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
}
}
}
// constrain servos
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
servo[i] = constrain(servo[i], servoConf[i].min, servoConf[i].max); // limit the values
}
#endif
}
void taskMainPidLoop(void)
{
cycleTime = getTaskDeltaTime(TASK_SELF);
dT = (float)cycleTime * 0.000001f;
imuUpdateAccelerometer();
imuUpdateGyroAndAttitude();
annexCode();
if (masterConfig.rxConfig.rcSmoothing) {
filterRc(isRXDataNew);
}
#if defined(NAV)
if (isRXDataNew) {
updateWaypointsAndNavigationMode();
}
#endif
isRXDataNew = false;
#if defined(NAV)
updatePositionEstimator();
applyWaypointNavigationAndAltitudeHold();
#endif
// If we're armed, at minimum throttle, and we do arming via the
// sticks, do not process yaw input from the rx. We do this so the
// motors do not spin up while we are trying to arm or disarm.
// Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
#ifndef USE_QUAD_MIXER_ONLY
#ifdef USE_SERVOS
&& !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.servoMixerConfig.tri_unarmed_servo)
#endif
&& masterConfig.mixerMode != MIXER_AIRPLANE
&& masterConfig.mixerMode != MIXER_FLYING_WING
&& masterConfig.mixerMode != MIXER_CUSTOM_AIRPLANE
#endif
) {
rcCommand[YAW] = 0;
}
// Apply throttle tilt compensation
if (!STATE(FIXED_WING)) {
int16_t thrTiltCompStrength = 0;
if (navigationRequiresThrottleTiltCompensation()) {
thrTiltCompStrength = 100;
}
else if (currentProfile->throttle_tilt_compensation_strength && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
thrTiltCompStrength = currentProfile->throttle_tilt_compensation_strength;
}
if (thrTiltCompStrength) {
rcCommand[THROTTLE] = constrain(masterConfig.motorConfig.minthrottle
+ (rcCommand[THROTTLE] - masterConfig.motorConfig.minthrottle) * calculateThrottleTiltCompensationFactor(thrTiltCompStrength),
masterConfig.motorConfig.minthrottle,
masterConfig.motorConfig.maxthrottle);
}
}
pidController(¤tProfile->pidProfile, currentControlRateProfile, &masterConfig.rxConfig);
#ifdef HIL
if (hilActive) {
hilUpdateControlState();
motorControlEnable = false;
}
#endif
mixTable();
#ifdef USE_SERVOS
if (isMixerUsingServos()) {
servoMixer(currentProfile->flaperon_throw_offset, currentProfile->flaperon_throw_inverted);
}
if (feature(FEATURE_SERVO_TILT)) {
processServoTilt();
}
//Servos should be filtered or written only when mixer is using servos or special feaures are enabled
if (isServoOutputEnabled()) {
filterServos();
writeServos();
}
#endif
if (motorControlEnable) {
writeMotors();
}
#ifdef USE_SDCARD
afatfs_poll();
#endif
#ifdef BLACKBOX
if (!cliMode && feature(FEATURE_BLACKBOX)) {
handleBlackbox();
}
#endif
}
void mixTable(void *pidProfilePtr)
{
uint32_t i = 0;
fix12_t vbatCompensationFactor = 0;
static fix12_t mixReduction;
bool use_vbat_compensation = false;
pidProfile_t *pidProfile = (pidProfile_t *) pidProfilePtr;
if (batteryConfig && pidProfile->vbatPidCompensation) {
use_vbat_compensation = true;
vbatCompensationFactor = calculateVbatPidCompensation();
}
bool isFailsafeActive = failsafeIsActive(); // TODO - Find out if failsafe checks are really needed here in mixer code
// Initial mixer concept by bdoiron74 reused and optimized for Air Mode
int16_t rollPitchYawMix[MAX_SUPPORTED_MOTORS];
int16_t rollPitchYawMixMax = 0; // assumption: symetrical about zero.
int16_t rollPitchYawMixMin = 0;
if (use_vbat_compensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]); // Add voltage compensation
// Find roll/pitch/yaw desired output
for (i = 0; i < motorCount; i++) {
rollPitchYawMix[i] =
axisPID[PITCH] * currentMixer[i].pitch +
axisPID[ROLL] * currentMixer[i].roll +
-mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw;
if (use_vbat_compensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]); // Add voltage compensation
if (rollPitchYawMix[i] > rollPitchYawMixMax) rollPitchYawMixMax = rollPitchYawMix[i];
if (rollPitchYawMix[i] < rollPitchYawMixMin) rollPitchYawMixMin = rollPitchYawMix[i];
if (debugMode == DEBUG_MIXER && i < 5) debug[i] = rollPitchYawMix[i];
}
// Scale roll/pitch/yaw uniformly to fit within throttle range
int16_t rollPitchYawMixRange = rollPitchYawMixMax - rollPitchYawMixMin;
int16_t throttleRange, throttle;
int16_t throttleMin, throttleMax;
static int16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions
// Find min and max throttle based on condition.
if (feature(FEATURE_3D)) {
if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
if ((rcCommand[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling
throttleMax = flight3DConfig->deadband3d_low;
throttleMin = escAndServoConfig->minthrottle;
throttlePrevious = throttle = rcCommand[THROTTLE];
} else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling
throttleMax = escAndServoConfig->maxthrottle;
throttleMin = flight3DConfig->deadband3d_high;
throttlePrevious = throttle = rcCommand[THROTTLE];
} else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive
throttle = throttleMax = flight3DConfig->deadband3d_low;
throttleMin = escAndServoConfig->minthrottle;
} else { // Deadband handling from positive to negative
throttleMax = escAndServoConfig->maxthrottle;
throttle = throttleMin = flight3DConfig->deadband3d_high;
}
} else {
throttle = rcCommand[THROTTLE];
throttleMin = escAndServoConfig->minthrottle;
throttleMax = escAndServoConfig->maxthrottle;
}
throttleRange = throttleMax - throttleMin;
if (rollPitchYawMixRange > throttleRange) {
mixReduction = qConstruct(throttleRange, rollPitchYawMixRange);
for (i = 0; i < motorCount; i++) {
rollPitchYawMix[i] = qMultiply(mixReduction,rollPitchYawMix[i]);
}
// Get the maximum correction by setting offset to center
throttleMin = throttleMax = throttleMin + (throttleRange / 2);
} else {
throttleMin = throttleMin + (rollPitchYawMixRange / 2);
throttleMax = throttleMax - (rollPitchYawMixRange / 2);
}
// Now add in the desired throttle, but keep in a range that doesn't clip adjusted
// roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
for (i = 0; i < motorCount; i++) {
motor[i] = rollPitchYawMix[i] + constrain(throttle * currentMixer[i].throttle, throttleMin, throttleMax);
if (isFailsafeActive) {
motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle);
} else if (feature(FEATURE_3D)) {
if (throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle)) {
motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, flight3DConfig->deadband3d_low);
} else {
motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle);
}
} else {
motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
}
// Motor stop handling
if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) {
if (((rcData[THROTTLE]) < rxConfig->mincheck)) {
motor[i] = escAndServoConfig->mincommand;
}
}
}
// Disarmed mode
if (!ARMING_FLAG(ARMED)) {
for (i = 0; i < motorCount; i++) {
motor[i] = motor_disarmed[i];
}
}
// motor outputs are used as sources for servo mixing, so motors must be calculated before servos.
#if !defined(USE_QUAD_MIXER_ONLY) || defined(USE_SERVOS)
// airplane / servo mixes
switch (currentMixerMode) {
case MIXER_CUSTOM_AIRPLANE:
case MIXER_FLYING_WING:
case MIXER_AIRPLANE:
case MIXER_BICOPTER:
case MIXER_CUSTOM_TRI:
case MIXER_TRI:
case MIXER_DUALCOPTER:
case MIXER_SINGLECOPTER:
case MIXER_GIMBAL:
servoMixer();
break;
/*
case MIXER_GIMBAL:
servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
break;
*/
default:
break;
}
// camera stabilization
if (feature(FEATURE_SERVO_TILT)) {
// center at fixed position, or vary either pitch or roll by RC channel
servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) {
if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) {
servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
} else {
servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch / 50;
servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
}
}
}
// constrain servos
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
servo[i] = constrain(servo[i], servoConf[i].min, servoConf[i].max); // limit the values
}
#endif
}
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];
}
}
}
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];
}
}
}
