Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 09:16:01 +03:00
Code Issues Wiki Activity

Merge pull request #5092 from iNavFlight/dzikuvx-unify-pterm-between-pidff-controllers

Browse source 
Unify P term processing for PID and PIFF controllers
This commit is contained in:
Paweł Spychalski 2019-09-28 16:57:38 +02:00 • committed by GitHub
commit 0353cc4802
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
3 changed files with 31 additions and 23 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/common/filter.c
View file

@ -72,7 +72,7 @@ float pt1FilterApply3(pt1Filter_t *filter, float input, float dT)
return filter->state;
}
float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dT)
float FAST_CODE NOINLINE pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dT)
{
// Pre calculate and store RC
if (!filter->RC) {

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

@ -136,6 +136,9 @@ static EXTENDED_FASTRAM float dBoostFactor;
static EXTENDED_FASTRAM float dBoostMaxAtAlleceleration;
#endif
static EXTENDED_FASTRAM uint16_t yawPLimit;
static EXTENDED_FASTRAM uint8_t yawLpfHz;
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 10);
PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
@ -257,6 +260,13 @@ void pidInit(void)
itermRelaxType = pidProfile()->iterm_relax_type;
itermRelaxSetpointThreshold = MC_ITERM_RELAX_SETPOINT_THRESHOLD * MC_ITERM_RELAX_CUTOFF_DEFAULT / pidProfile()->iterm_relax_cutoff;
if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) {
yawPLimit = pidProfile()->yaw_p_limit;
} else {
yawPLimit = 0;
}
yawLpfHz = pidProfile()->yaw_lpf_hz;
#ifdef USE_D_BOOST
dBoostFactor = pidProfile()->dBoostFactor;
dBoostMaxAtAlleceleration = pidProfile()->dBoostMaxAtAlleceleration;
@ -579,18 +589,28 @@ bool isFixedWingItermLimitActive(float stickPosition)
return fabsf(stickPosition) > pidProfile()->fixedWingItermLimitOnStickPosition;
}
static FAST_CODE NOINLINE float pTermProcess(pidState_t *pidState, flight_dynamics_index_t axis, float rateError, float dT) {
float newPTerm = rateError * pidState->kP;
if (axis == FD_YAW) {
// Constrain YAW by yaw_p_limit value if not servo driven (in that case servo limits apply)
if (yawPLimit) {
newPTerm = constrain(newPTerm, -yawPLimit, yawPLimit);
}
if (yawLpfHz) {
newPTerm = pt1FilterApply4(&pidState->ptermLpfState, newPTerm, yawLpfHz, dT);
}
}
return newPTerm;
}
static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
{
const float rateError = pidState->rateTarget - pidState->gyroRate;
// Calculate new P-term and FF-term
float newPTerm = rateError * pidState->kP;
float newFFTerm = pidState->rateTarget * pidState->kFF;
// Additional P-term LPF on YAW axis
if (axis == FD_YAW && pidProfile()->yaw_lpf_hz) {
newPTerm = pt1FilterApply4(&pidState->ptermLpfState, newPTerm, pidProfile()->yaw_lpf_hz, dT);
}
const float newPTerm = pTermProcess(pidState, axis, rateError, dT);
const float newFFTerm = pidState->rateTarget * pidState->kFF;
// Calculate integral
pidState->errorGyroIf += rateError * pidState->kI * dT;
@ -688,18 +708,7 @@ static float applyDBoost(pidState_t *pidState, flight_dynamics_index_t axis, flo
static void FAST_CODE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
{
const float rateError = pidState->rateTarget - pidState->gyroRate;
// Calculate new P-term
float newPTerm = rateError * pidState->kP;
// Constrain YAW by yaw_p_limit value if not servo driven (in that case servo limits apply)
if (axis == FD_YAW && (getMotorCount() >= 4 && pidProfile()->yaw_p_limit)) {
newPTerm = constrain(newPTerm, -pidProfile()->yaw_p_limit, pidProfile()->yaw_p_limit);
}
// Additional P-term LPF on YAW axis
if (axis == FD_YAW && pidProfile()->yaw_lpf_hz) {
newPTerm = pt1FilterApply4(&pidState->ptermLpfState, newPTerm, pidProfile()->yaw_lpf_hz, dT);
}
const float newPTerm = pTermProcess(pidState, axis, rateError, dT);
// Calculate new D-term
float newDTerm;

1
src/main/flight/pid.h
View file

@ -108,7 +108,6 @@ typedef struct pidProfile_s {
uint8_t dterm_lpf_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
uint8_t use_dterm_fir_filter; // Use classical INAV FIR differentiator. Very noise robust, can be quite slowish
uint8_t yaw_pterm_lpf_hz; // Used for filering Pterm noise on noisy frames
uint8_t yaw_lpf_hz;
uint16_t yaw_p_limit;