mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 00:05:28 +03:00
Drop gyro method of ITerm relax
This commit is contained in:
parent
2025c15c3c
commit
f812d5c6ac
6 changed files with 10 additions and 48 deletions
|
@ -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,
|
||||||
|
|
|
@ -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)
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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) {
|
|
||||||
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));
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
return 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);
|
||||||
|
|
|
@ -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
|
||||||
|
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue