mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-21 07:15:16 +03:00
Make VelXY Dterm LPF user configurable
This commit is contained in:
parent
e52b5d9480
commit
ddff19f399
6 changed files with 31 additions and 11 deletions
|
@ -1076,6 +1076,10 @@ groups:
|
|||
condition: USE_NAV
|
||||
min: 0
|
||||
max: 255
|
||||
- name: nav_mc_vel_xy_dterm_lpf_hz
|
||||
field: navVelXyDTermLpfHz
|
||||
min: 0
|
||||
max: 100
|
||||
- name: nav_fw_pos_z_p
|
||||
field: bank_fw.pid[PID_POS_Z].P
|
||||
condition: USE_NAV
|
||||
|
|
|
@ -109,7 +109,7 @@ int32_t axisPID_P[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_I[FLIGHT_DYNAMICS_INDEX_
|
|||
|
||||
STATIC_FASTRAM pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT];
|
||||
|
||||
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 6);
|
||||
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 7);
|
||||
|
||||
PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
|
||||
.bank_mc = {
|
||||
|
@ -204,6 +204,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
|
|||
.fixedWingItermLimitOnStickPosition = 0.5f,
|
||||
|
||||
.loiter_direction = NAV_LOITER_RIGHT,
|
||||
.navVelXyDTermLpfHz = 10,
|
||||
);
|
||||
|
||||
void pidInit(void)
|
||||
|
|
|
@ -108,6 +108,7 @@ typedef struct pidProfile_s {
|
|||
float fixedWingItermLimitOnStickPosition; //Do not allow Iterm to grow when stick position is above this point
|
||||
|
||||
uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise)
|
||||
float navVelXyDTermLpfHz;
|
||||
} pidProfile_t;
|
||||
|
||||
typedef struct pidAutotuneConfig_s {
|
||||
|
|
|
@ -1655,7 +1655,11 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu
|
|||
pid->last_input = measurement;
|
||||
}
|
||||
|
||||
newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, NAV_DTERM_CUT_HZ, dt) * gainScaler;
|
||||
if (pid->dTermLpfHz > 0) {
|
||||
newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, pid->dTermLpfHz, dt) * gainScaler;
|
||||
} else {
|
||||
newDerivative = pid->param.kD * newDerivative;
|
||||
}
|
||||
|
||||
if (pidFlags & PID_ZERO_INTEGRATOR) {
|
||||
pid->integrator = 0.0f;
|
||||
|
@ -1714,7 +1718,7 @@ void navPidReset(pidController_t *pid)
|
|||
pid->output_constrained = 0.0f;
|
||||
}
|
||||
|
||||
void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF)
|
||||
void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz)
|
||||
{
|
||||
pid->param.kP = _kP;
|
||||
pid->param.kI = _kI;
|
||||
|
@ -1730,7 +1734,7 @@ void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kF
|
|||
pid->param.kI = 0.0;
|
||||
pid->param.kT = 0.0;
|
||||
}
|
||||
|
||||
pid->dTermLpfHz = _dTermLpfHz;
|
||||
navPidReset(pid);
|
||||
}
|
||||
|
||||
|
@ -3038,7 +3042,8 @@ void navigationUsePIDs(void)
|
|||
navPidInit(&posControl.pids.vel[axis], (float)pidProfile()->bank_mc.pid[PID_VEL_XY].P / 20.0f,
|
||||
(float)pidProfile()->bank_mc.pid[PID_VEL_XY].I / 100.0f,
|
||||
(float)pidProfile()->bank_mc.pid[PID_VEL_XY].D / 100.0f,
|
||||
(float)pidProfile()->bank_mc.pid[PID_VEL_XY].FF / 100.0f
|
||||
(float)pidProfile()->bank_mc.pid[PID_VEL_XY].FF / 100.0f,
|
||||
pidProfile()->navVelXyDTermLpfHz
|
||||
);
|
||||
}
|
||||
|
||||
|
@ -3048,25 +3053,33 @@ void navigationUsePIDs(void)
|
|||
navPidInit(&posControl.pids.vel[Z], (float)pidProfile()->bank_mc.pid[PID_VEL_Z].P / 66.7f,
|
||||
(float)pidProfile()->bank_mc.pid[PID_VEL_Z].I / 20.0f,
|
||||
(float)pidProfile()->bank_mc.pid[PID_VEL_Z].D / 100.0f,
|
||||
0.0f);
|
||||
0.0f,
|
||||
NAV_DTERM_CUT_HZ
|
||||
);
|
||||
|
||||
// Initialize surface tracking PID
|
||||
navPidInit(&posControl.pids.surface, 2.0f,
|
||||
0.0f,
|
||||
0.0f,
|
||||
0.0f);
|
||||
0.0f,
|
||||
NAV_DTERM_CUT_HZ
|
||||
);
|
||||
|
||||
/** Airplane PIDs */
|
||||
// Initialize fixed wing PID controllers
|
||||
navPidInit(&posControl.pids.fw_nav, (float)pidProfile()->bank_fw.pid[PID_POS_XY].P / 100.0f,
|
||||
(float)pidProfile()->bank_fw.pid[PID_POS_XY].I / 100.0f,
|
||||
(float)pidProfile()->bank_fw.pid[PID_POS_XY].D / 100.0f,
|
||||
0.0f);
|
||||
0.0f,
|
||||
NAV_DTERM_CUT_HZ
|
||||
);
|
||||
|
||||
navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 10.0f,
|
||||
(float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 10.0f,
|
||||
(float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 10.0f,
|
||||
0.0f);
|
||||
0.0f,
|
||||
NAV_DTERM_CUT_HZ
|
||||
);
|
||||
}
|
||||
|
||||
void navigationInit(void)
|
||||
|
|
|
@ -253,6 +253,7 @@ typedef struct {
|
|||
bool reset;
|
||||
pidControllerParam_t param;
|
||||
pt1Filter_t dterm_filter_state; // last derivative for low-pass filter
|
||||
float dTermLpfHz; // dTerm low pass filter cutoff frequency
|
||||
float integrator; // integrator value
|
||||
float last_input; // last input for derivative
|
||||
|
||||
|
|
|
@ -33,7 +33,7 @@
|
|||
#define NAV_FW_CONTROL_MONITORING_RATE 2
|
||||
#define NAV_FW_PITCH_CUTOFF_FREQENCY_HZ 2 // low-pass filter on Z (pitch angle) for fixed wing
|
||||
#define NAV_FW_ROLL_CUTOFF_FREQUENCY_HZ 10 // low-pass filter on roll correction for fixed wing
|
||||
#define NAV_DTERM_CUT_HZ 10
|
||||
#define NAV_DTERM_CUT_HZ 10.0f
|
||||
#define NAV_ACCELERATION_XY_MAX 980.0f // cm/s/s // approx 45 deg lean angle
|
||||
|
||||
#define INAV_SURFACE_MAX_DISTANCE 40
|
||||
|
@ -357,7 +357,7 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void);
|
|||
float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags);
|
||||
float navPidApply3(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags, const float gainScaler);
|
||||
void navPidReset(pidController_t *pid);
|
||||
void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF);
|
||||
void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz);
|
||||
void navPInit(pController_t *p, float _kP);
|
||||
|
||||
bool isThrustFacingDownwards(void);
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue