Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 11:29:58 +03:00
Code Issues Wiki Activity

S-term (for wings) (#13679)

Browse source 
* S-term for wings

* tabs

Co-authored-by: Petr Ledvina <ledvinap@gmail.com>

* pidData[axis].S

---------

Co-authored-by: Petr Ledvina <ledvinap@gmail.com>
This commit is contained in:
Ivan Efimov 2024-06-20 06:01:13 -04:00 • committed by GitHub
parent 00ca8232d5
commit e2e1a68ba0
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
8 changed files with 51 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files

26
src/main/flight/pid.c
View file

@ -125,8 +125,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
[PID_ROLL] = PID_ROLL_DEFAULT,
[PID_PITCH] = PID_PITCH_DEFAULT,
[PID_YAW] = PID_YAW_DEFAULT,
[PID_LEVEL] = { 50, 75, 75, 50 },
[PID_MAG] = { 40, 0, 0, 0 },
[PID_LEVEL] = { 50, 75, 75, 50, 0 },
[PID_MAG] = { 40, 0, 0, 0, 0 },
},
.pidSumLimit = PIDSUM_LIMIT,
.pidSumLimitYaw = PIDSUM_LIMIT_YAW,
@ -796,6 +796,22 @@ static FAST_CODE_NOINLINE float applyLaunchControl(int axis, const rollAndPitchT
}
#endif
static float getSterm(int axis, const pidProfile_t *pidProfile)
{
#ifdef USE_WING
const float sTerm = getSetpointRate(axis) / getMaxRcRate(axis) * 1000.0f *
(float)pidProfile->pid[axis].S / 100.0f;
DEBUG_SET(DEBUG_S_TERM, axis, lrintf(sTerm));
return sTerm;
#else
UNUSED(axis);
UNUSED(pidProfile);
return 0.0f;
#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 FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTimeUs)
@ -1098,6 +1114,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
pidData[axis].P = 0;
pidData[axis].D = 0;
pidData[axis].F = 0;
pidData[axis].S = 0;
}
}
#endif // USE_YAW_SPIN_RECOVERY
@ -1133,8 +1150,10 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
}
}
pidData[axis].S = getSterm(axis, pidProfile);
// calculating the PID sum
const float pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F;
const float pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F + pidData[axis].S;
#ifdef USE_INTEGRATED_YAW_CONTROL
if (axis == FD_YAW && pidRuntime.useIntegratedYaw) {
pidData[axis].Sum += pidSum * pidRuntime.dT * 100.0f;
@ -1154,6 +1173,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
pidData[axis].I = 0;
pidData[axis].D = 0;
pidData[axis].F = 0;
pidData[axis].S = 0;
pidData[axis].Sum = 0;
}