Skip to content

Commit

Permalink
Get rid of some not needed floating point divisions
Browse files Browse the repository at this point in the history
  • Loading branch information
@DzikuVx
DzikuVx committed Aug 1, 2023
1 parent 37af10e commit 43186ed
Show file tree
Hide file tree
Showing 3 changed files with 21 additions and 17 deletions.
34 changes: 19 additions & 15 deletions src/main/flight/pid.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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
Expand All @@ -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) {
Expand All @@ -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);
}
Expand All @@ -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;

/*
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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
Expand All @@ -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);
}
}

Expand Down Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion src/main/flight/pid.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion src/main/flight/pid_autotune.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

Expand Down

0 comments on commit 43186ed

Please sign in to comment.