Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 00:05:28 +03:00
Code Issues Wiki Activity

Hardlimit D-term, reduce kI when stick input is above certain limit (#357)

Browse source
This commit is contained in:
Konstantin Sharlaimov 2016-07-21 06:50:43 +03:00 • committed by GitHub
parent 1cab71963f
commit f3d53c3c49
6 changed files with 21 additions and 18 deletions
Download patch file Download diff file Expand all files Collapse all files

5
src/main/config/config.c
View file

@ -170,7 +170,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 119;
static const uint8_t EEPROM_CONF_VERSION = 120;
static void resetAccelerometerTrims(flightDynamicsTrims_t * accZero, flightDynamicsTrims_t * accGain)
{
@ -219,6 +219,9 @@ void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->dterm_lpf_hz = 40;
pidProfile->yaw_lpf_hz = 30;
pidProfile->rollPitchItermIgnoreRate = 200; // dps
pidProfile->yawItermIgnoreRate = 50; // dps
pidProfile->yaw_p_limit = YAW_P_LIMIT_DEFAULT;
pidProfile->mag_hold_rate_limit = MAG_HOLD_RATE_LIMIT_DEFAULT;

1
src/main/config/runtime_config.h
View file

@ -60,7 +60,6 @@ typedef enum {
SMALL_ANGLE = (1 << 3),
FIXED_WING = (1 << 4), // set when in flying_wing or airplane mode. currently used by althold selection code
ANTI_WINDUP = (1 << 5),
PID_ATTENUATE = (1 << 6),
} stateFlags_t;
#define DISABLE_STATE(mask) (stateFlags &= ~(mask))

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

@ -307,15 +307,20 @@ static void pidApplyRateController(const pidProfile_t *pidProfile, pidState_t *p
if (pidProfile->dterm_lpf_hz) {
newDTerm = pt1FilterApply4(&pidState->deltaLpfState, newDTerm, pidProfile->dterm_lpf_hz, dT);
}
// Additionally constrain D
newDTerm = constrainf(newDTerm, -300.0f, 300.0f);
}
// TODO: Get feedback from mixer on available correction range for each axis
const float pidAttenuationFactor = STATE(PID_ATTENUATE) ? 0.33f : 1.0f;
const float newOutput = (newPTerm + newDTerm) * pidAttenuationFactor + pidState->errorGyroIf;
const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf;
const float newOutputLimited = constrainf(newOutput, -PID_MAX_OUTPUT, +PID_MAX_OUTPUT);
// Integrate only if we can do backtracking
pidState->errorGyroIf += (rateError * pidState->kI * dT) + ((newOutputLimited - newOutput) * pidState->kT * dT);
// Prevent strong Iterm accumulation during stick inputs
const float integratorThreshold = (axis == FD_YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
const float antiWindupScaler = constrainf(1.0f - (ABS(pidState->rateTarget) / integratorThreshold), 0.0f, 1.0f);
pidState->errorGyroIf += (rateError * pidState->kI * antiWindupScaler * dT) + ((newOutputLimited - newOutput) * pidState->kT * dT);
// Don't grow I-term if motors are at their limit
if (STATE(ANTI_WINDUP) || motorLimitReached) {

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

@ -60,10 +60,12 @@ typedef struct pidProfile_s {
uint16_t yaw_p_limit;
uint8_t yaw_lpf_hz;
uint16_t rollPitchItermIgnoreRate; // Experimental threshold for ignoring iterm for pitch and roll on certain rates
uint16_t yawItermIgnoreRate; // Experimental threshold for ignoring iterm for yaw on certain rates
int16_t max_angle_inclination[ANGLE_INDEX_COUNT]; // Max possible inclination (roll and pitch axis separately
uint8_t mag_hold_rate_limit; //Maximum rotation rate MAG_HOLD mode cas feed to yaw rate PID controller
uint8_t mag_hold_rate_limit; //Maximum rotation rate MAG_HOLD mode can feed to yaw rate PID controller
} pidProfile_t;
extern int16_t axisPID[];

3
src/main/io/serial_cli.c
View file

@ -771,6 +771,9 @@ const clivalue_t valueTable[] = {
{ "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX }, 0 },
{ "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } },
{ "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } },
#ifdef BLACKBOX
{ "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, .config.minmax = { 1, 32 }, 0 },
{ "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, .config.minmax = { 1, 32 }, 0 },

9
src/main/mw.c
View file

@ -452,17 +452,8 @@ void processRx(void)
DISABLE_STATE(ANTI_WINDUP);
pidResetErrorAccumulators();
}
// Enable low-throttle PID attenuation on multicopters
if (!STATE(FIXED_WING)) {
ENABLE_STATE(PID_ATTENUATE);
}
else {
DISABLE_STATE(PID_ATTENUATE);
}
}
else {
DISABLE_STATE(PID_ATTENUATE);
DISABLE_STATE(ANTI_WINDUP);
}
}