void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
{
float horizonLevelStrength = 0.0f;
if (FLIGHT_MODE(HORIZON_MODE)) {
// (convert 0-100 range to 0.0-1.0 range)
horizonLevelStrength = (float)calcHorizonLevelStrength(rxConfig->midrc, pidProfile->horizon_tilt_effect,
pidProfile->horizon_tilt_mode, pidProfile->D8[PIDLEVEL]) / 100.0f;
}
// ----------PID controller----------
for (int axis = 0; axis < 3; axis++) {
const uint8_t rate = controlRateConfig->rates[axis];
// -----Get the desired angle rate depending on flight mode
float angleRate;
if (axis == FD_YAW) {
// YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
angleRate = (float)((rate + 27) * rcCommand[YAW]) / 32.0f;
} else {
// control is GYRO based for ACRO and HORIZON - direct sticks control is applied to rate PID
angleRate = (float)((rate + 27) * rcCommand[axis]) / 16.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
// multiplication of rcCommand corresponds to changing the sticks scaling here
#ifdef GPS
const float errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)max_angle_inclination), max_angle_inclination)
- attitude.raw[axis] + angleTrim->raw[axis];
#else
const float errorAngle = constrain(2 * rcCommand[axis], -((int)max_angle_inclination), max_angle_inclination)
- attitude.raw[axis] + angleTrim->raw[axis];
#endif
if (FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode
angleRate = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f;
} else {
// HORIZON mode
// mix in errorAngle to desired angleRate to add a little auto-level feel.
// horizonLevelStrength has been scaled to the stick input
angleRate += errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f;
}
}
}
// --------low-level gyro-based PID. ----------
const float gyroRate = luxGyroScale * gyroADCf[axis] * gyro.scale;
axisPID[axis] = pidLuxFloatCore(axis, pidProfile, gyroRate, angleRate);
//axisPID[axis] = constrain(axisPID[axis], -PID_LUX_FLOAT_MAX_PID, PID_LUX_FLOAT_MAX_PID);
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(axis);
}
#endif
}
}
// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
// Based on 2DOF reference design (matlab)
void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
{
float errorRate = 0, rP = 0, rD = 0, PVRate = 0;
float ITerm,PTerm,DTerm;
static float lastRateError[2];
static float Kp[3], Ki[3], Kd[3], b[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3];
float delta;
int axis;
float horizonLevelStrength = 1;
float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
initFilters(pidProfile);
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->D8[PIDLEVEL] == 0){
horizonLevelStrength = 0;
} else {
horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->D8[PIDLEVEL])) + 1, 0, 1);
}
}
// Yet Highly experimental and under test and development
// Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols)
static float kiThrottleGain = 1.0f;
if (pidProfile->itermThrottleGain) {
const uint16_t maxLoopCount = 20000 / targetPidLooptime;
const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f;
static int16_t previousThrottle;
static uint16_t loopIncrement;
if (loopIncrement >= maxLoopCount) {
kiThrottleGain = 1.0f + constrainf((float)(ABS(rcCommand[THROTTLE] - previousThrottle)) * throttleItermGain, 0.0f, 5.0f); // Limit to factor 5
previousThrottle = rcCommand[THROTTLE];
loopIncrement = 0;
} else {
loopIncrement++;
}
}
// ----------PID controller----------
for (axis = 0; axis < 3; axis++) {
static uint8_t configP[3], configI[3], configD[3];
// Prevent unnecessary computing and check for changed PIDs. No need for individual checks. Only pids is fine for now
// Prepare all parameters for PID controller
if ((pidProfile->P8[axis] != configP[axis]) || (pidProfile->I8[axis] != configI[axis]) || (pidProfile->D8[axis] != configD[axis])) {
Kp[axis] = PTERM_SCALE * pidProfile->P8[axis];
Ki[axis] = ITERM_SCALE * pidProfile->I8[axis];
Kd[axis] = DTERM_SCALE * pidProfile->D8[axis];
b[axis] = pidProfile->ptermSetpointWeight / 100.0f;
c[axis] = pidProfile->dtermSetpointWeight / 100.0f;
yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * getdT();
rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * getdT();
configP[axis] = pidProfile->P8[axis];
configI[axis] = pidProfile->I8[axis];
configD[axis] = pidProfile->D8[axis];
}
// Limit abrupt yaw inputs / stops
float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity;
if (maxVelocity) {
float currentVelocity = setpointRate[axis] - previousSetpoint[axis];
if (ABS(currentVelocity) > maxVelocity) {
setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity;
}
previousSetpoint[axis] = setpointRate[axis];
}
// Yaw control is GYRO based, direct sticks control is applied to rate PID
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {
// calculate error angle and limit the angle to the max inclination
#ifdef GPS
const float errorAngle = (constrain(2 * 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(2 * 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
setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f;
} 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
setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f);
}
}
PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec
// --------low-level gyro-based PID based on 2DOF PID controller. ----------
// ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ----------
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
// ----- calculate error / angle rates ----------
errorRate = setpointRate[axis] - PVRate; // r - y
rP = b[axis] * setpointRate[axis] - PVRate; // br - y
// Slowly restore original setpoint with more stick input
float diffRate = errorRate - rP;
rP += diffRate * rcInput[axis];
// Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount
float dynReduction = tpaFactor;
if (pidProfile->toleranceBand) {
const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f;
static uint8_t zeroCrossCount[3];
static uint8_t currentErrorPolarity[3];
if (ABS(errorRate) < pidProfile->toleranceBand) {
if (zeroCrossCount[axis]) {
if (currentErrorPolarity[axis] == POSITIVE_ERROR) {
if (errorRate < 0 ) {
zeroCrossCount[axis]--;
currentErrorPolarity[axis] = NEGATIVE_ERROR;
}
} else {
if (errorRate > 0 ) {
zeroCrossCount[axis]--;
currentErrorPolarity[axis] = POSITIVE_ERROR;
}
}
} else {
dynReduction *= constrainf(ABS(errorRate) / pidProfile->toleranceBand, minReduction, 1.0f);
}
} else {
zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount;
currentErrorPolarity[axis] = (errorRate > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR;
}
}
// -----calculate P component
PTerm = Kp[axis] * rP * dynReduction;
// -----calculate I component.
// Reduce strong Iterm accumulation during higher stick inputs
float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
float setpointRateScaler = constrainf(1.0f - (1.5f * (ABS(setpointRate[axis]) / accumulationThreshold)), 0.0f, 1.0f);
// Handle All windup Scenarios
// limit maximum integrator value to prevent WindUp
float itermScaler = setpointRateScaler * kiThrottleGain;
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f);
// I coefficient (I8) moved before integration to make limiting independent from PID settings
ITerm = errorGyroIf[axis];
//-----calculate D-term (Yaw D not yet supported)
if (axis == YAW) {
if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
axisPID[axis] = lrintf(PTerm + ITerm);
DTerm = 0.0f; // needed for blackbox
} else {
rD = c[axis] * setpointRate[axis] - PVRate; // cr - y
delta = rD - lastRateError[axis];
lastRateError[axis] = rD;
// Divide delta by targetLooptime to get differential (ie dr/dt)
delta *= (1.0f / getdT());
if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * dynReduction;
// Filter delta
if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta);
if (pidProfile->dterm_lpf_hz) {
if (dtermBiquadLpfInitialised) {
delta = biquadFilterApply(&dtermFilterLpf[axis], delta);
} else {
delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
}
}
DTerm = Kd[axis] * delta * dynReduction;
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900);
}
// Disable PID control at zero throttle
if (!pidStabilisationEnabled) axisPID[axis] = 0;
#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
}
}
void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
{
pidFilterIsSetCheck(pidProfile);
float horizonLevelStrength;
if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the most deflected stick position
const int32_t stickPosAil = ABS(getRcStickDeflection(ROLL, rxConfig->midrc));
const int32_t stickPosEle = ABS(getRcStickDeflection(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->D8[PIDLEVEL] == 0){
horizonLevelStrength = 0;
} else {
horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->D8[PIDLEVEL])) + 1, 0, 1);
}
}
// ----------PID controller----------
for (int axis = 0; axis < 3; axis++) {
const uint8_t rate = controlRateConfig->rates[axis];
// -----Get the desired angle rate depending on flight mode
float angleRate;
if (axis == FD_YAW) {
// YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
angleRate = (float)((rate + 27) * rcCommand[YAW]) / 32.0f;
} else {
// control is GYRO based for ACRO and HORIZON - direct sticks control is applied to rate PID
angleRate = (float)((rate + 27) * rcCommand[axis]) / 16.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
// multiplication of rcCommand corresponds to changing the sticks scaling here
#ifdef GPS
const float errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)max_angle_inclination), max_angle_inclination)
- attitude.raw[axis] + angleTrim->raw[axis];
#else
const float errorAngle = constrain(2 * rcCommand[axis], -((int)max_angle_inclination), max_angle_inclination)
- attitude.raw[axis] + angleTrim->raw[axis];
#endif
if (FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode
angleRate = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f;
} else {
// HORIZON mode
// mix in errorAngle to desired angleRate to add a little auto-level feel.
// horizonLevelStrength has been scaled to the stick input
angleRate += errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f;
}
}
}
// --------low-level gyro-based PID. ----------
const float gyroRate = luxGyroScale * gyroADC[axis] * gyro.scale;
axisPID[axis] = pidLuxFloatCore(axis, pidProfile, gyroRate, angleRate);
//axisPID[axis] = constrain(axisPID[axis], -PID_LUX_FLOAT_MAX_PID, PID_LUX_FLOAT_MAX_PID);
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(axis);
}
#endif
}
}
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
}
}
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
{
float RateError, errorAngle, AngleRate, gyroRate;
float ITerm,PTerm,DTerm;
int32_t stickPosAil, stickPosEle, mostDeflectedPos;
static float lastError[3];
static float delta1[3], delta2[3];
float delta, deltaSum;
float dT;
int axis;
float horizonLevelStrength = 1;
dT = (float)cycleTime * 0.000001f;
if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions
stickPosAil = getRcStickDeflection(FD_ROLL, rxConfig->midrc);
stickPosEle = getRcStickDeflection(FD_PITCH, rxConfig->midrc);
if(ABS(stickPosAil) > ABS(stickPosEle)){
mostDeflectedPos = ABS(stickPosAil);
}
else {
mostDeflectedPos = ABS(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++) {
// -----Get the desired angle rate depending on flight mode
uint8_t 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 {
// calculate error and limit the angle to the max inclination
#ifdef GPS
errorAngle = (constrain(rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
#else
errorAngle = (constrain(rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
#endif
if (FLIGHT_MODE(ANGLE_MODE)) {
// it's the ANGLE mode - control is angle based, so control loop is needed
AngleRate = errorAngle * pidProfile->A_level;
} else {
//control is GYRO based (ACRO and HORIZON - 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(HORIZON_MODE)) {
// 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;
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
}
// -----calculate I component.
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
// 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;
// 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);
// add moving average here to reduce noise
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
// Dterm low pass
if (pidProfile->dterm_cut_hz) {
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz);
}
DTerm = constrainf((deltaSum / 3.0f) * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
#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
}
}
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
{
UNUSED(rxConfig);
int axis, prop;
int32_t error, errorAngle;
int32_t PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
static int16_t lastGyro[3] = { 0, 0, 0 };
static int32_t delta1[3], delta2[3];
int32_t deltaSum;
int32_t delta;
UNUSED(controlRateConfig);
// **** PITCH & ROLL & YAW PID ****
prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 500); // range [0;500]
for (axis = 0; axis < 3; axis++) {
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC
// observe max inclination
#ifdef GPS
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
#else
errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
#endif
PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
PTermACC = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5);
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp
ITermACC = (errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12;
}
if (!FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || axis == FD_YAW) { // MODE relying on GYRO or YAW axis
error = (int32_t) rcCommand[axis] * 10 * 8 / pidProfile->P8[axis];
error -= gyroADC[axis] / 4;
PTermGYRO = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
if ((ABS(gyroADC[axis]) > (640 * 4)) || (axis == FD_YAW && ABS(rcCommand[axis]) > 100))
errorGyroI[axis] = 0;
ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) / 64;
}
if (FLIGHT_MODE(HORIZON_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) {
PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500;
ITerm = (ITermACC * (500 - prop) + ITermGYRO * prop) / 500;
} else {
if (FLIGHT_MODE(ANGLE_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) {
PTerm = PTermACC;
ITerm = ITermACC;
} else {
PTerm = PTermGYRO;
ITerm = ITermGYRO;
}
}
PTerm -= ((int32_t)gyroADC[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
}
delta = (gyroADC[axis] - lastGyro[axis]) / 4;
lastGyro[axis] = gyroADC[axis];
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
// Dterm low pass
if (pidProfile->dterm_cut_hz) {
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz);
}
DTerm = (deltaSum * dynD8[axis]) / 32;
axisPID[axis] = PTerm + ITerm - DTerm;
#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
}
