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

Merge pull request #9207 from iNavFlight/dzikuvx-small-pidloop-optimizations

Browse source 
Get rid of some not needed floating point divisions
This commit is contained in:
Paweł Spychalski 2023-08-02 21:47:35 +02:00 • committed by GitHub
commit fd59159f71
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
3 changed files with 21 additions and 17 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -154,7 +154,7 @@ static EXTENDED_FASTRAM float iTermAntigravityGain;
#endif
static EXTENDED_FASTRAM uint8_t usedPidControllerType;
typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t axis, float dT);
typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv);
static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
static EXTENDED_FASTRAM bool levelingEnabled = false;
@ -625,7 +625,7 @@ static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynam
}
}
float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f);
float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f);
// Apply simple LPF to angleRateTarget to make response less jerky
// Ideas behind this:
@ -681,13 +681,13 @@ static float pTermProcess(pidState_t *pidState, float rateError, float dT) {
}
#ifdef USE_D_BOOST
static float FAST_CODE applyDBoost(pidState_t *pidState, float currentRateTarget, float dT) {
static float FAST_CODE applyDBoost(pidState_t *pidState, float currentRateTarget, float dT, float dT_inv) {
float dBoost = 1.0f;
const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) / dT;
const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) * dT_inv;
const float dBoostGyroAcceleration = fabsf(biquadFilterApply(&pidState->dBoostGyroLpf, dBoostGyroDelta));
const float dBoostRateAcceleration = fabsf((currentRateTarget - pidState->previousRateTarget) / dT);
const float dBoostRateAcceleration = fabsf((currentRateTarget - pidState->previousRateTarget) * dT_inv);
if (dBoostGyroAcceleration >= dBoostRateAcceleration) {
//Gyro is accelerating faster than setpoint, we want to smooth out
@ -710,7 +710,7 @@ static float applyDBoost(pidState_t *pidState, float dT) {
}
#endif
static float dTermProcess(pidState_t *pidState, float currentRateTarget, float dT) {
static float dTermProcess(pidState_t *pidState, float currentRateTarget, float dT, float dT_inv) {
// Calculate new D-term
float newDTerm = 0;
if (pidState->kD == 0) {
@ -722,7 +722,7 @@ static float dTermProcess(pidState_t *pidState, float currentRateTarget, float d
delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta);
// Calculate derivative
newDTerm = delta * (pidState->kD / dT) * applyDBoost(pidState, currentRateTarget, dT);
newDTerm = delta * (pidState->kD * dT_inv) * applyDBoost(pidState, currentRateTarget, dT, dT_inv);
}
return(newDTerm);
}
@ -736,19 +736,20 @@ static void applyItermLimiting(pidState_t *pidState) {
}
}
static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) {
static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv) {
UNUSED(pidState);
UNUSED(axis);
UNUSED(dT);
UNUSED(dT_inv);
}
static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv)
{
const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget);
const float rateError = rateTarget - pidState->gyroRate;
const float newPTerm = pTermProcess(pidState, rateError, dT);
const float newDTerm = dTermProcess(pidState, rateTarget, dT);
const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv);
const float newFFTerm = rateTarget * pidState->kFF;
/*
@ -806,14 +807,14 @@ static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint,
return itermErrorRate;
}
static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv)
{
const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget);
const float rateError = rateTarget - pidState->gyroRate;
const float newPTerm = pTermProcess(pidState, rateError, dT);
const float newDTerm = dTermProcess(pidState, rateTarget, dT);
const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv);
const float rateTargetDelta = rateTarget - pidState->previousRateTarget;
const float rateTargetDeltaFiltered = pt3FilterApply(&pidState->rateTargetFilter, rateTargetDelta);
@ -1062,6 +1063,9 @@ void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis
void FAST_CODE pidController(float dT)
{
const float dT_inv = 1.0f / dT;
if (!pidFiltersConfigured) {
return;
}
@ -1135,7 +1139,7 @@ void FAST_CODE pidController(float dT)
}
// Prevent strong Iterm accumulation during stick inputs
antiWindupScaler = constrainf((1.0f - getMotorMixRange()) / motorItermWindupPoint, 0.0f, 1.0f);
antiWindupScaler = constrainf((1.0f - getMotorMixRange()) * motorItermWindupPoint, 0.0f, 1.0f);
for (int axis = 0; axis < 3; axis++) {
// Apply setpoint rate of change limits
@ -1145,7 +1149,7 @@ void FAST_CODE pidController(float dT)
checkItermLimitingActive(&pidState[axis]);
checkItermFreezingActive(&pidState[axis], axis);
pidControllerApplyFn(&pidState[axis], axis, dT);
pidControllerApplyFn(&pidState[axis], axis, dT, dT_inv);
}
}
@ -1177,7 +1181,7 @@ void pidInit(void)
itermRelax = pidProfile()->iterm_relax;
yawLpfHz = pidProfile()->yaw_lpf_hz;
motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f);
motorItermWindupPoint = 1.0f / (1.0f - (pidProfile()->itermWindupPointPercent / 100.0f));
#ifdef USE_D_BOOST
dBoostMin = pidProfile()->dBoostMin;

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

@ -47,7 +47,7 @@ FP-PID has been rescaled to match LuxFloat (and MWRewrite) from Cleanflight 1.13
#define FP_PID_RATE_I_MULTIPLIER 4.0f
#define FP_PID_RATE_D_MULTIPLIER 1905.0f
#define FP_PID_RATE_D_FF_MULTIPLIER 7270.0f
#define FP_PID_LEVEL_P_MULTIPLIER 6.56f // Level P gain units is [1/sec] and angle error is [deg] => [deg/s]
#define FP_PID_LEVEL_P_MULTIPLIER 1.0f / 6.56f // Level P gain units is [1/sec] and angle error is [deg] => [deg/s]
#define FP_PID_YAWHOLD_P_MULTIPLIER 80.0f
#define MC_ITERM_RELAX_SETPOINT_THRESHOLD 40.0f

2
src/main/flight/pid_autotune.c
View file

@ -187,7 +187,7 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa
// Use different max rate in ANLGE mode
if (FLIGHT_MODE(ANGLE_MODE)) {
float maxDesiredRateInAngleMode = DECIDEGREES_TO_DEGREES(pidProfile()->max_angle_inclination[axis] * 1.0f) * pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER;
float maxDesiredRateInAngleMode = DECIDEGREES_TO_DEGREES(pidProfile()->max_angle_inclination[axis] * 1.0f) * pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER;
maxDesiredRate = MIN(maxRateSetting, maxDesiredRateInAngleMode);
}