void gyroUpdate(void)
{
// range: +/- 8192; +/- 2000 deg/sec
if (!gyro.read(gyroADCRaw)) {
return;
}
gyroADC[X] = gyroADCRaw[X];
gyroADC[Y] = gyroADCRaw[Y];
gyroADC[Z] = gyroADCRaw[Z];
alignSensors(gyroADC, gyroADC, gyroAlign);
if (!isGyroCalibrationComplete()) {
performAcclerationCalibration(gyroConfig()->gyroMovementCalibrationThreshold);
}
gyroADC[X] -= gyroZero[X];
gyroADC[Y] -= gyroZero[Y];
gyroADC[Z] -= gyroZero[Z];
if (gyroConfig()->gyro_soft_lpf_hz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
if (debugMode == DEBUG_GYRO)
debug[axis] = gyroADC[axis];
if (gyroConfig()->gyro_soft_type == FILTER_BIQUAD)
gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], (float) gyroADC[axis]);
else
gyroADCf[axis] = pt1FilterApply(&gyroFilterPt1[axis], (float) gyroADC[axis]);
if (debugMode == DEBUG_NOTCH)
debug[axis] = lrintf(gyroADCf[axis]);
if (gyroConfig()->gyro_soft_notch_hz)
gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch[axis], gyroADCf[axis]);
gyroADC[axis] = lrintf(gyroADCf[axis]);
}
} else {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADCf[axis] = gyroADC[axis];
}
}
}
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);
}