void gyroUpdate(void)
{
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
int axis;
// range: +/- 8192; +/- 2000 deg/sec
if (!gyro.read(gyroADCRaw)) {
return;
}
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
if (debugMode == DEBUG_GYRO) debug[axis] = gyroADC[axis];
gyroADC[axis] = gyroADCRaw[axis];
}
alignSensors(gyroADC, gyroADC, gyroAlign);
if (gyroLpfCutFreq) {
if (!gyroFilterStateIsSet) initGyroFilterCoefficients(); /* initialise filter coefficients */
if (gyroFilterStateIsSet) {
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], &gyroFilterState[axis]));
}
}
if (!isGyroCalibrationComplete()) {
performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold);
}
applyGyroZero();
}
static void updateBatteryVoltage(void)
{
uint16_t vbatSample;
// store the battery voltage with some other recent battery voltage readings
vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY);
vbatSample = applyBiQuadFilter(vbatSample, &vbatFilterState);
vbat = batteryAdcToVoltage(vbatSample);
}
static void imuCalculateEstimatedAttitude(void)
{
static biquad_t accLPFState[3];
static bool accStateIsSet;
static uint32_t previousIMUUpdateTime;
float rawYawError = 0;
int32_t axis;
bool useAcc = false;
bool useMag = false;
bool useYaw = false;
uint32_t currentTime = micros();
uint32_t deltaT = currentTime - previousIMUUpdateTime;
previousIMUUpdateTime = currentTime;
// Smooth and use only valid accelerometer readings
for (axis = 0; axis < 3; axis++) {
if (imuRuntimeConfig->acc_cut_hz) {
if (accStateIsSet) {
accSmooth[axis] = lrintf(applyBiQuadFilter((float) accADC[axis], &accLPFState[axis]));
} else {
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(imuRuntimeConfig->acc_cut_hz, &accLPFState[axis], 1000);
accStateIsSet = true;
}
} else {
accSmooth[axis] = accADC[axis];
}
}
if (imuIsAccelerometerHealthy()) {
useAcc = true;
}
if (sensors(SENSOR_MAG) && isMagnetometerHealthy()) {
useMag = true;
}
#if defined(GPS)
else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5 && GPS_speed >= 300) {
// In case of a fixed-wing aircraft we can use GPS course over ground to correct heading
rawYawError = DECIDEGREES_TO_RADIANS(attitude.values.yaw - GPS_ground_course);
useYaw = true;
}
#endif
imuMahonyAHRSupdate(deltaT * 1e-6f,
gyroADC[X] * gyroScale, gyroADC[Y] * gyroScale, gyroADC[Z] * gyroScale,
useAcc, accSmooth[X], accSmooth[Y], accSmooth[Z],
useMag, magADC[X], magADC[Y], magADC[Z],
useYaw, rawYawError);
imuUpdateEulerAngles();
imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame
}
void gyroUpdate(void)
{
// range: +/- 8192; +/- 2000 deg/sec
if (!gyro.read(gyroADC)) {
return;
}
alignSensors(gyroADC, gyroADC, gyroAlign);
if (gyroLpfCutFreq) {
if (!gyroFilterStateIsSet) {
initGyroFilterCoefficients();
} else {
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], &gyroBiQuadState[axis]));
}
}
if (!isGyroCalibrationComplete()) {
performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold);
}
applyGyroZero();
}
STATIC_UNIT_TESTED int16_t pidLuxFloatCore(int axis, const pidProfile_t *pidProfile, float gyroRate, float angleRate)
{
static float lastRateForDelta[3];
static float deltaState[3][DTERM_AVERAGE_COUNT];
SET_PID_LUX_FLOAT_CORE_LOCALS(axis);
const float rateError = angleRate - gyroRate;
// -----calculate P component
float PTerm = luxPTermScale * rateError * pidProfile->P8[axis] * PIDweight[axis] / 100;
// Constrain YAW by yaw_p_limit value if not servo driven, in that case servolimits apply
if (axis == YAW && pidProfile->yaw_p_limit && motorCount >= 4) {
PTerm = constrainf(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
}
// -----calculate I component
float ITerm = lastITermf[axis] + luxITermScale * rateError * dT * pidProfile->I8[axis];
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
ITerm = constrainf(ITerm, -PID_MAX_I, PID_MAX_I);
// Anti windup protection
if (rcModeIsActive(BOXAIRMODE)) {
if (STATE(ANTI_WINDUP) || motorLimitReached) {
ITerm = constrainf(ITerm, -ITermLimitf[axis], ITermLimitf[axis]);
} else {
ITermLimitf[axis] = ABS(ITerm);
}
}
lastITermf[axis] = ITerm;
// -----calculate D component
float DTerm;
if (pidProfile->D8[axis] == 0) {
// optimisation for when D8 is zero, often used by YAW axis
DTerm = 0;
} else {
// delta calculated from measurement
float delta = -(gyroRate - lastRateForDelta[axis]);
lastRateForDelta[axis] = gyroRate;
// Divide delta by dT to get differential (ie dr/dt)
delta *= (1.0f / dT);
if (pidProfile->dterm_cut_hz) {
// DTerm delta low pass filter
delta = applyBiQuadFilter(delta, &deltaFilterState[axis]);
} else {
// When DTerm low pass filter disabled apply moving average to reduce noise
delta = filterApplyAveragef(delta, DTERM_AVERAGE_COUNT, deltaState[axis]);
}
DTerm = luxDTermScale * delta * pidProfile->D8[axis] * PIDweight[axis] / 100;
DTerm = constrainf(DTerm, -PID_MAX_D, PID_MAX_D);
}
#ifdef BLACKBOX
axisPID_P[axis] = PTerm;
axisPID_I[axis] = ITerm;
axisPID_D[axis] = DTerm;
#endif
GET_PID_LUX_FLOAT_CORE_LOCALS(axis);
// -----calculate total PID output
return lrintf(PTerm + ITerm + DTerm);
}
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
{
float RateError, AngleRate, gyroRate;
float ITerm,PTerm,DTerm;
static float lastError[3];
float delta;
int axis;
float horizonLevelStrength = 1;
static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0);
deltaStateIsSet = true;
}
if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions
const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc));
const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle);
// Progressively turn off the horizon self level strength as the stick is banged over
horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection
if(pidProfile->H_sensitivity == 0){
horizonLevelStrength = 0;
} else {
horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->H_sensitivity)) + 1, 0, 1);
}
}
// ----------PID controller----------
for (axis = 0; axis < 3; axis++) {
uint8_t rate = 10;
// -----Get the desired angle rate depending on flight mode
if (axis == YAW || !pidProfile->airModeInsaneAcrobilityFactor || !IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
rate = controlRateConfig->rates[axis];
}
if (axis == FD_YAW) {
// YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
AngleRate = (float)((rate + 10) * rcCommand[YAW]) / 50.0f;
} else {
// ACRO mode, control is GYRO based, direct sticks control is applied to rate PID
AngleRate = (float)((rate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max roll/pitch rate
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
// calculate error angle and limit the angle to the max inclination
#ifdef GPS
const float errorAngle = (constrain(rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
#else
const float errorAngle = (constrain(rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
#endif
if (FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode - control is angle based, so control loop is needed
AngleRate = errorAngle * pidProfile->A_level;
} else {
// HORIZON mode - direct sticks control is applied to rate PID
// mix up angle error to desired AngleRate to add a little auto-level feel
AngleRate += errorAngle * pidProfile->H_level * horizonLevelStrength;
}
}
}
gyroRate = gyroADC[axis] * gyro.scale; // gyro output scaled to dps
// --------low-level gyro-based PID. ----------
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
// -----calculate scaled error.AngleRates
// multiplication of rcCommand corresponds to changing the sticks scaling here
RateError = AngleRate - gyroRate;
// -----calculate P component
PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100;
// -----calculate I component.
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
airModePlus(&airModePlusAxisState[axis], axis, pidProfile);
errorGyroIf[axis] *= airModePlusAxisState[axis].iTermScaler;
}
if (allowITermShrinkOnly || motorLimitReached) {
if (ABS(errorGyroIf[axis]) < ABS(previousErrorGyroIf[axis])) {
previousErrorGyroIf[axis] = errorGyroIf[axis];
} else {
errorGyroIf[axis] = constrain(errorGyroIf[axis], -ABS(previousErrorGyroIf[axis]), ABS(previousErrorGyroIf[axis]));
}
} else {
previousErrorGyroIf[axis] = errorGyroIf[axis];
}
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
ITerm = errorGyroIf[axis];
//-----calculate D-term
delta = RateError - lastError[axis];
lastError[axis] = RateError;
if (deltaStateIsSet) {
delta = applyBiQuadFilter(delta, &deltaBiQuadState[axis]);
}
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
// would be scaled by different dt each time. Division by dT fixes that.
delta *= (1.0f / dT);
DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
axisPID[axis] = lrintf(airModePlusAxisState[axis].factor + airModePlusAxisState[axis].wowFactor * axisPID[axis]);
}
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(axis);
}
#endif
#ifdef BLACKBOX
axisPID_P[axis] = PTerm;
axisPID_I[axis] = ITerm;
axisPID_D[axis] = DTerm;
#endif
}
}