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(pidProfile_t *pidProfile)
{
uint32_t i = 0;
float vbatCompensationFactor = 1;
// Scale roll/pitch/yaw uniformly to fit within throttle range
// Initial mixer concept by bdoiron74 reused and optimized for Air Mode
float motorMix[MAX_SUPPORTED_MOTORS], motorMixMax = 0, motorMixMin = 0;
float throttleRange = 0, throttle = 0;
float motorOutputRange, motorMixRange;
uint16_t motorOutputMin, motorOutputMax;
static uint16_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
motorOutputMax = deadbandMotor3dLow;
motorOutputMin = minMotorOutputNormal;
throttlePrevious = rcCommand[THROTTLE];
throttle = rcCommand[THROTTLE] + flight3DConfig->deadband3d_throttle;
} else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling
motorOutputMax = maxMotorOutputNormal;
motorOutputMin = deadbandMotor3dHigh;
throttlePrevious = rcCommand[THROTTLE];
throttle = rcCommand[THROTTLE] - flight3DConfig->deadband3d_throttle;
} else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive
throttle = deadbandMotor3dLow;
motorOutputMin = motorOutputMax = minMotorOutputNormal;
} else { // Deadband handling from positive to negative
motorOutputMax = maxMotorOutputNormal;
throttle = motorOutputMin = deadbandMotor3dHigh;
}
} else {
throttle = rcCommand[THROTTLE];
motorOutputMin = minMotorOutputNormal;
motorOutputMax = maxMotorOutputNormal;
}
motorOutputRange = motorOutputMax - motorOutputMin;
throttle = constrainf((throttle - rxConfig->mincheck) / rcCommandThrottleRange, 0.0f, 1.0f);
throttleRange = motorOutputMax - motorOutputMin;
float scaledAxisPIDf[3];
// Limit the PIDsum
for (int axis = 0; axis < 3; axis++)
scaledAxisPIDf[axis] = constrainf(axisPIDf[axis] / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimit);
// Calculate voltage compensation
if (batteryConfig && pidProfile->vbatPidCompensation) vbatCompensationFactor = calculateVbatPidCompensation();
// Find roll/pitch/yaw desired output
for (i = 0; i < motorCount; i++) {
motorMix[i] =
scaledAxisPIDf[PITCH] * currentMixer[i].pitch +
scaledAxisPIDf[ROLL] * currentMixer[i].roll +
-mixerConfig->yaw_motor_direction * scaledAxisPIDf[YAW] * currentMixer[i].yaw;
if (vbatCompensationFactor > 1.0f) motorMix[i] *= vbatCompensationFactor; // Add voltage compensation
if (motorMix[i] > motorMixMax) motorMixMax = motorMix[i];
if (motorMix[i] < motorMixMin) motorMixMin = motorMix[i];
}
motorMixRange = motorMixMax - motorMixMin;
if (motorMixRange > 1.0f) {
for (i = 0; i < motorCount; i++) {
motorMix[i] /= motorMixRange;
}
// Get the maximum correction by setting offset to center
throttle = 0.5f;
} else {
float throttleLimitOffset = motorMixRange / 2.0f;
throttle = constrainf(throttle, 0.0f + throttleLimitOffset, 1.0f - throttleLimitOffset);
}
// 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] = lrintf( motorOutputMin + (motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle))) );
if (failsafeIsActive()) {
if (isMotorProtocolDshot())
motor[i] = (motor[i] < motorOutputMin) ? disarmMotorOutput : motor[i]; // Prevent getting into special reserved range
motor[i] = constrain(motor[i], disarmMotorOutput, motorOutputMax);
} else {
motor[i] = constrain(motor[i], motorOutputMin, motorOutputMax);
}
// Motor stop handling
if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) {
if (((rcData[THROTTLE]) < rxConfig->mincheck)) {
motor[i] = disarmMotorOutput;
}
}
}
// Anti Desync feature for ESC's. Limit rapid throttle changes
if (motorConfig->maxEscThrottleJumpMs) {
const int16_t maxThrottleStep = constrain(motorConfig->maxEscThrottleJumpMs / (1000 / targetPidLooptime), 2, 10000);
// Only makes sense when it's within the range
if (maxThrottleStep < throttleRange) {
static int16_t motorPrevious[MAX_SUPPORTED_MOTORS];
motor[i] = constrain(motor[i], motorOutputMin, motorPrevious[i] + maxThrottleStep); // Only limit accelerating situation
motorPrevious[i] = motor[i];
}
}
// Disarmed mode
if (!ARMING_FLAG(ARMED)) {
for (i = 0; i < motorCount; i++) {
motor[i] = motor_disarmed[i];
}
}
}
FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation)
{
if (isFlipOverAfterCrashMode()) {
applyFlipOverAfterCrashModeToMotors();
return;
}
// Find min and max throttle based on conditions. Throttle has to be known before mixing
calculateThrottleAndCurrentMotorEndpoints(currentTimeUs);
// Calculate and Limit the PID sum
const float scaledAxisPidRoll =
constrainf(pidData[FD_ROLL].Sum, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
const float scaledAxisPidPitch =
constrainf(pidData[FD_PITCH].Sum, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
uint16_t yawPidSumLimit = currentPidProfile->pidSumLimitYaw;
#ifdef USE_YAW_SPIN_RECOVERY
const bool yawSpinDetected = gyroYawSpinDetected();
if (yawSpinDetected) {
yawPidSumLimit = PIDSUM_LIMIT_MAX; // Set to the maximum limit during yaw spin recovery to prevent limiting motor authority
}
#endif // USE_YAW_SPIN_RECOVERY
float scaledAxisPidYaw =
constrainf(pidData[FD_YAW].Sum, -yawPidSumLimit, yawPidSumLimit) / PID_MIXER_SCALING;
if (!mixerConfig()->yaw_motors_reversed) {
scaledAxisPidYaw = -scaledAxisPidYaw;
}
// Calculate voltage compensation
const float vbatCompensationFactor = vbatPidCompensation ? calculateVbatPidCompensation() : 1.0f;
// Apply the throttle_limit_percent to scale or limit the throttle based on throttle_limit_type
if (currentControlRateProfile->throttle_limit_type != THROTTLE_LIMIT_TYPE_OFF) {
throttle = applyThrottleLimit(throttle);
}
#ifdef USE_YAW_SPIN_RECOVERY
// 50% throttle provides the maximum authority for yaw recovery when airmode is not active.
// When airmode is active the throttle setting doesn't impact recovery authority.
if (yawSpinDetected && !isAirmodeActive()) {
throttle = 0.5f; //
}
#endif // USE_YAW_SPIN_RECOVERY
// Find roll/pitch/yaw desired output
float motorMix[MAX_SUPPORTED_MOTORS];
float motorMixMax = 0, motorMixMin = 0;
for (int i = 0; i < motorCount; i++) {
float mix =
scaledAxisPidRoll * currentMixer[i].roll +
scaledAxisPidPitch * currentMixer[i].pitch +
scaledAxisPidYaw * currentMixer[i].yaw;
mix *= vbatCompensationFactor; // Add voltage compensation
if (mix > motorMixMax) {
motorMixMax = mix;
} else if (mix < motorMixMin) {
motorMixMin = mix;
}
motorMix[i] = mix;
}
pidUpdateAntiGravityThrottleFilter(throttle);
#if defined(USE_THROTTLE_BOOST)
if (throttleBoost > 0.0f) {
const float throttleHpf = throttle - pt1FilterApply(&throttleLpf, throttle);
throttle = constrainf(throttle + throttleBoost * throttleHpf, 0.0f, 1.0f);
}
#endif
#ifdef USE_GPS_RESCUE
// If gps rescue is active then override the throttle. This prevents things
// like throttle boost or throttle limit from negatively affecting the throttle.
if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
throttle = gpsRescueGetThrottle();
}
#endif
motorMixRange = motorMixMax - motorMixMin;
if (motorMixRange > 1.0f) {
for (int i = 0; i < motorCount; i++) {
motorMix[i] /= motorMixRange;
}
// Get the maximum correction by setting offset to center when airmode enabled
if (isAirmodeActive()) {
throttle = 0.5f;
}
} else {
if (isAirmodeActive() || throttle > 0.5f) { // Only automatically adjust throttle when airmode enabled. Airmode logic is always active on high throttle
const float throttleLimitOffset = motorMixRange / 2.0f;
throttle = constrainf(throttle, 0.0f + throttleLimitOffset, 1.0f - throttleLimitOffset);
}
}
// Apply the mix to motor endpoints
applyMixToMotors(motorMix);
}
