Move to pid.c, add relax to offset drag based component, make configurable
This commit is contained in:
parent
4442de5591
commit
d4512672fe
|
@ -178,6 +178,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
|||
.launchControlAngleLimit = 0,
|
||||
.launchControlGain = 40,
|
||||
.launchControlAllowTriggerReset = true,
|
||||
.use_integrated_yaw = false,
|
||||
.integrated_yaw_relax = 200
|
||||
);
|
||||
#ifdef USE_DYN_LPF
|
||||
pidProfile->dterm_lowpass_hz = 150;
|
||||
|
@ -436,6 +438,8 @@ static FAST_RAM_ZERO_INIT uint8_t launchControlMode;
|
|||
static FAST_RAM_ZERO_INIT uint8_t launchControlAngleLimit;
|
||||
static FAST_RAM_ZERO_INIT float launchControlKi;
|
||||
#endif
|
||||
static FAST_RAM_ZERO_INIT bool useIntegratedYaw;
|
||||
static FAST_RAM_ZERO_INIT uint8_t integratedYawRelax;
|
||||
|
||||
void pidResetIterm(void)
|
||||
{
|
||||
|
@ -573,6 +577,8 @@ void pidInitConfig(const pidProfile_t *pidProfile)
|
|||
}
|
||||
launchControlKi = ITERM_SCALE * pidProfile->launchControlGain;
|
||||
#endif
|
||||
useIntegratedYaw = pidProfile->use_integrated_yaw;
|
||||
integratedYawRelax = pidProfile->integrated_yaw_relax;
|
||||
}
|
||||
|
||||
void pidInit(const pidProfile_t *pidProfile)
|
||||
|
@ -1241,7 +1247,13 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
|
|||
}
|
||||
#endif
|
||||
// calculating the PID sum
|
||||
pidData[axis].Sum = 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;
|
||||
if (axis == FD_YAW && useIntegratedYaw) {
|
||||
pidData[axis].Sum += pidSum * dT * 100.0f;
|
||||
pidData[axis].Sum -= pidData[axis].Sum * integratedYawRelax / 100000.0f * dT / 0.000125f;
|
||||
} else {
|
||||
pidData[axis].Sum = pidSum;
|
||||
}
|
||||
}
|
||||
|
||||
// Disable PID control if at zero throttle or if gyro overflow detected
|
||||
|
|
|
@ -156,8 +156,11 @@ typedef struct pidProfile_s {
|
|||
uint8_t launchControlAngleLimit; // Optional launch control angle limit (requires ACC)
|
||||
uint8_t launchControlGain; // Iterm gain used while launch control is active
|
||||
uint8_t launchControlAllowTriggerReset; // Controls trigger behavior and whether the trigger can be reset
|
||||
uint8_t use_integrated_yaw; // Selects whether the yaw pidsum should integrated
|
||||
uint8_t integrated_yaw_relax; // Specifies how much integrated yaw should be reduced to offset the drag based yaw component
|
||||
} pidProfile_t;
|
||||
|
||||
|
||||
PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles);
|
||||
|
||||
typedef struct pidConfig_s {
|
||||
|
|
|
@ -936,6 +936,10 @@ const clivalue_t valueTable[] = {
|
|||
{ "abs_control_error_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 45 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_error_limit) },
|
||||
#endif
|
||||
|
||||
{ "use_integrated_yaw", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, use_integrated_yaw) },
|
||||
{ "integrated_yaw_relax", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, integrated_yaw_relax) },
|
||||
|
||||
|
||||
#ifdef USE_LAUNCH_CONTROL
|
||||
{ "launch_control_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LAUNCH_CONTROL_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlMode) },
|
||||
{ "launch_trigger_allow_reset", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlAllowTriggerReset) },
|
||||
|
|
Loading…
Reference in New Issue