1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 08:15:26 +03:00

Drop gyro method of ITerm relax

This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-07-31 20:46:18 +02:00
parent 2025c15c3c
commit f812d5c6ac
6 changed files with 10 additions and 48 deletions

View file

@ -63,7 +63,6 @@ typedef enum {
DEBUG_REM_FLIGHT_TIME, DEBUG_REM_FLIGHT_TIME,
DEBUG_SMARTAUDIO, DEBUG_SMARTAUDIO,
DEBUG_ACC, DEBUG_ACC,
DEBUG_ITERM_RELAX,
DEBUG_ERPM, DEBUG_ERPM,
DEBUG_RPM_FILTER, DEBUG_RPM_FILTER,
DEBUG_RPM_FREQ, DEBUG_RPM_FREQ,

View file

@ -142,15 +142,6 @@ int32_t applyDeadband(int32_t value, int32_t deadband)
return value; return value;
} }
float fapplyDeadbandf(float value, float deadband)
{
if (fabsf(value) < deadband) {
return 0;
}
return value >= 0 ? value - deadband : value + deadband;
}
int constrain(int amt, int low, int high) int constrain(int amt, int low, int high)
{ {
if (amt < low) if (amt < low)

View file

@ -130,7 +130,6 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu
int gcd(int num, int denom); int gcd(int num, int denom);
int32_t applyDeadband(int32_t value, int32_t deadband); int32_t applyDeadband(int32_t value, int32_t deadband);
float fapplyDeadbandf(float value, float deadband);
int constrain(int amt, int low, int high); int constrain(int amt, int low, int high);
float constrainf(float amt, float low, float high); float constrainf(float amt, float low, float high);

View file

@ -82,7 +82,7 @@ tables:
- name: debug_modes - name: debug_modes
values: ["NONE", "GYRO", "AGL", "FLOW_RAW", values: ["NONE", "GYRO", "AGL", "FLOW_RAW",
"FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE", "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
"VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ITERM_RELAX", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
"ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY", "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
"IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574"] "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574"]
- name: async_mode - name: async_mode
@ -124,9 +124,6 @@ tables:
- name: iterm_relax - name: iterm_relax
values: ["OFF", "RP", "RPY"] values: ["OFF", "RP", "RPY"]
enum: itermRelax_e enum: itermRelax_e
- name: iterm_relax_type
values: ["GYRO", "SETPOINT"]
enum: itermRelaxType_e
- name: airmodeHandlingType - name: airmodeHandlingType
values: ["STICK_CENTER", "THROTTLE_THRESHOLD"] values: ["STICK_CENTER", "THROTTLE_THRESHOLD"]
- name: nav_extra_arming_safety - name: nav_extra_arming_safety
@ -1727,9 +1724,6 @@ groups:
field: navFwPosHdgPidsumLimit field: navFwPosHdgPidsumLimit
min: PID_SUM_LIMIT_MIN min: PID_SUM_LIMIT_MIN
max: PID_SUM_LIMIT_MAX max: PID_SUM_LIMIT_MAX
- name: mc_iterm_relax_type
field: iterm_relax_type
table: iterm_relax_type
- name: mc_iterm_relax - name: mc_iterm_relax
field: iterm_relax field: iterm_relax
table: iterm_relax table: iterm_relax

View file

@ -124,7 +124,6 @@ int32_t axisPID_P[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_I[FLIGHT_DYNAMICS_INDEX_
static EXTENDED_FASTRAM pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT]; static EXTENDED_FASTRAM pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT];
static EXTENDED_FASTRAM pt1Filter_t windupLpf[XYZ_AXIS_COUNT]; static EXTENDED_FASTRAM pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
static EXTENDED_FASTRAM uint8_t itermRelax; static EXTENDED_FASTRAM uint8_t itermRelax;
static EXTENDED_FASTRAM uint8_t itermRelaxType;
#ifdef USE_ANTIGRAVITY #ifdef USE_ANTIGRAVITY
static EXTENDED_FASTRAM pt1Filter_t antigravityThrottleLpf; static EXTENDED_FASTRAM pt1Filter_t antigravityThrottleLpf;
@ -155,7 +154,7 @@ static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn; static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn;
static EXTENDED_FASTRAM bool levelingEnabled = false; static EXTENDED_FASTRAM bool levelingEnabled = false;
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 14); PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 15);
PG_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.bank_mc = { .bank_mc = {
@ -264,7 +263,6 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.navVelXyDtermAttenuation = 90, .navVelXyDtermAttenuation = 90,
.navVelXyDtermAttenuationStart = 10, .navVelXyDtermAttenuationStart = 10,
.navVelXyDtermAttenuationEnd = 60, .navVelXyDtermAttenuationEnd = 60,
.iterm_relax_type = ITERM_RELAX_SETPOINT,
.iterm_relax_cutoff = MC_ITERM_RELAX_CUTOFF_DEFAULT, .iterm_relax_cutoff = MC_ITERM_RELAX_CUTOFF_DEFAULT,
.iterm_relax = ITERM_RELAX_RP, .iterm_relax = ITERM_RELAX_RP,
.dBoostFactor = 1.25f, .dBoostFactor = 1.25f,
@ -636,31 +634,20 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
#endif #endif
} }
static void FAST_CODE applyItermRelax(const int axis, const float gyroRate, float currentPidSetpoint, float *itermErrorRate) static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint, float itermErrorRate)
{ {
const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf);
if (itermRelax) { if (itermRelax) {
if (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY) { if (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY) {
const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf);
const float itermRelaxFactor = MAX(0, 1 - setpointHpf / MC_ITERM_RELAX_SETPOINT_THRESHOLD); const float itermRelaxFactor = MAX(0, 1 - setpointHpf / MC_ITERM_RELAX_SETPOINT_THRESHOLD);
return itermErrorRate * itermRelaxFactor;
if (itermRelaxType == ITERM_RELAX_SETPOINT) { }
*itermErrorRate *= itermRelaxFactor;
} else if (itermRelaxType == ITERM_RELAX_GYRO ) {
*itermErrorRate = fapplyDeadbandf(setpointLpf - gyroRate, setpointHpf);
} else {
*itermErrorRate = 0.0f;
} }
if (axis == FD_ROLL) { return itermErrorRate;
DEBUG_SET(DEBUG_ITERM_RELAX, 0, lrintf(setpointHpf));
DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(itermRelaxFactor * 100.0f));
DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(*itermErrorRate));
}
}
}
} }
#ifdef USE_D_BOOST #ifdef USE_D_BOOST
static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) { static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) {
@ -727,8 +714,7 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm; const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm;
const float newOutputLimited = constrainf(newOutput, -pidState->pidSumLimit, +pidState->pidSumLimit); const float newOutputLimited = constrainf(newOutput, -pidState->pidSumLimit, +pidState->pidSumLimit);
float itermErrorRate = rateError; float itermErrorRate = applyItermRelax(axis, pidState->rateTarget, rateError);
applyItermRelax(axis, pidState->gyroRate, pidState->rateTarget, &itermErrorRate);
#ifdef USE_ANTIGRAVITY #ifdef USE_ANTIGRAVITY
itermErrorRate *= iTermAntigravityGain; itermErrorRate *= iTermAntigravityGain;
@ -1046,7 +1032,6 @@ void pidInit(void)
pidGainsUpdateRequired = false; pidGainsUpdateRequired = false;
itermRelax = pidProfile()->iterm_relax; itermRelax = pidProfile()->iterm_relax;
itermRelaxType = pidProfile()->iterm_relax_type;
yawLpfHz = pidProfile()->yaw_lpf_hz; yawLpfHz = pidProfile()->yaw_lpf_hz;
motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f); motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f);

View file

@ -97,11 +97,6 @@ typedef enum {
ITERM_RELAX_RPY ITERM_RELAX_RPY
} itermRelax_e; } itermRelax_e;
typedef enum {
ITERM_RELAX_GYRO = 0,
ITERM_RELAX_SETPOINT
} itermRelaxType_e;
typedef struct pidProfile_s { typedef struct pidProfile_s {
uint8_t pidControllerType; uint8_t pidControllerType;
pidBank_t bank_fw; pidBank_t bank_fw;
@ -138,7 +133,6 @@ typedef struct pidProfile_s {
uint8_t navVelXyDtermAttenuation; // VEL_XY dynamic Dterm scale: Dterm will be attenuatedby this value (in percent) when UAV is traveling with more than navVelXyDtermAttenuationStart percents of max velocity uint8_t navVelXyDtermAttenuation; // VEL_XY dynamic Dterm scale: Dterm will be attenuatedby this value (in percent) when UAV is traveling with more than navVelXyDtermAttenuationStart percents of max velocity
uint8_t navVelXyDtermAttenuationStart; // VEL_XY dynamic Dterm scale: Dterm attenuation will begin at this percent of max velocity uint8_t navVelXyDtermAttenuationStart; // VEL_XY dynamic Dterm scale: Dterm attenuation will begin at this percent of max velocity
uint8_t navVelXyDtermAttenuationEnd; // VEL_XY dynamic Dterm scale: Dterm will be fully attenuated at this percent of max velocity uint8_t navVelXyDtermAttenuationEnd; // VEL_XY dynamic Dterm scale: Dterm will be fully attenuated at this percent of max velocity
uint8_t iterm_relax_type; // Specifies type of relax algorithm
uint8_t iterm_relax_cutoff; // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint uint8_t iterm_relax_cutoff; // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint
uint8_t iterm_relax; // Enable iterm suppression during stick input uint8_t iterm_relax; // Enable iterm suppression during stick input