From 695f47944b67c331e4fc720f57ded0dc5ac92db2 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 20 Nov 2016 09:20:53 +0000 Subject: [PATCH] Tidy PID controller --- src/main/fc/mw.c | 4 +--- src/main/flight/pid.c | 46 +++++++++++++++---------------------------- src/main/flight/pid.h | 7 +------ 3 files changed, 18 insertions(+), 39 deletions(-) diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index db0b280c3..f5775476b 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -101,8 +101,6 @@ static bool armingCalibrationWasInitialised; float setpointRate[3]; float rcInput[3]; -extern pidControllerFuncPtr pid_controller; - void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta) { masterConfig.accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll; @@ -695,7 +693,7 @@ void subTaskPidController(void) ¤tProfile->pidProfile, masterConfig.max_angle_inclination, &masterConfig.accelerometerTrims, - &masterConfig.rxConfig + masterConfig.rxConfig.midrc ); if (debugMode == DEBUG_PIDLOOP) {debug[2] = micros() - startTime;} } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index c2a236493..90c1a4708 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -21,7 +21,6 @@ #include -#include "build/build_config.h" #include "build/debug.h" #include "common/axis.h" @@ -36,10 +35,6 @@ #include "flight/navigation.h" #include "flight/gtune.h" -#include "io/gps.h" - -#include "rx/rx.h" - #include "sensors/gyro.h" #include "sensors/acceleration.h" @@ -47,8 +42,7 @@ extern float rcInput[3]; extern float setpointRate[3]; uint32_t targetPidLooptime; -bool pidStabilisationEnabled; -uint8_t PIDweight[3]; +static bool pidStabilisationEnabled; float axisPIDf[3]; @@ -59,10 +53,7 @@ uint8_t PIDweight[3]; int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; #endif -int32_t errorGyroI[3]; -float errorGyroIf[3]; - -pidControllerFuncPtr pid_controller; // which pid controller are we using +static float errorGyroIf[3]; void setTargetPidLooptime(uint32_t pidLooptime) { @@ -72,7 +63,6 @@ void setTargetPidLooptime(uint32_t pidLooptime) void pidResetErrorGyroState(void) { for (int axis = 0; axis < 3; axis++) { - errorGyroI[axis] = 0; errorGyroIf[axis] = 0.0f; } } @@ -125,24 +115,18 @@ static void pidInitFilters(const pidProfile_t *pidProfile) { // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. // Based on 2DOF reference design (matlab) void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) + const rollAndPitchTrims_t *angleTrim, uint16_t midrc) { - float errorRate = 0, rD = 0, PVRate = 0, dynC; - float ITerm,PTerm,DTerm; static float lastRateError[2]; static float Kp[3], Ki[3], Kd[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3], relaxFactor[3]; - float delta; - int axis; - float horizonLevelStrength = 1; - - float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float pidInitFilters(pidProfile); + float horizonLevelStrength = 1; if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions - const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); - const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); + const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, midrc)); + const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, midrc)); const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); // Progressively turn off the horizon self level strength as the stick is banged over horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection @@ -172,7 +156,8 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio } // ----------PID controller---------- - for (axis = 0; axis < 3; axis++) { + const float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float + for (int axis = 0; axis < 3; axis++) { static uint8_t configP[3], configI[3], configD[3]; @@ -222,16 +207,16 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio } } - PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec + const float PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec // --------low-level gyro-based PID based on 2DOF PID controller. ---------- // ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ---------- // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes // ----- calculate error / angle rates ---------- - errorRate = setpointRate[axis] - PVRate; // r - y + const float errorRate = setpointRate[axis] - PVRate; // r - y // -----calculate P component and add Dynamic Part based on stick input - PTerm = Kp[axis] * errorRate * tpaFactor; + float PTerm = Kp[axis] * errorRate * tpaFactor; // -----calculate I component. // Reduce strong Iterm accumulation during higher stick inputs @@ -245,12 +230,13 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f); // I coefficient (I8) moved before integration to make limiting independent from PID settings - ITerm = errorGyroIf[axis]; + const float ITerm = errorGyroIf[axis]; //-----calculate D-term (Yaw D not yet supported) + float DTerm; if (axis != YAW) { static float previousSetpoint[3]; - dynC = c[axis]; + float dynC = c[axis]; if (pidProfile->setpointRelaxRatio < 100) { dynC = c[axis]; if (setpointRate[axis] > 0) { @@ -262,8 +248,8 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio } } previousSetpoint[axis] = setpointRate[axis]; - rD = dynC * setpointRate[axis] - PVRate; // cr - y - delta = rD - lastRateError[axis]; + const float rD = dynC * setpointRate[axis] - PVRate; // cr - y + float delta = rD - lastRateError[axis]; lastRateError[axis] = rD; // Divide delta by targetLooptime to get differential (ie dr/dt) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 0719f78b3..ab75e9463 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -90,14 +90,9 @@ typedef struct pidProfile_s { #endif } pidProfile_t; -struct controlRateConfig_s; union rollAndPitchTrims_u; -struct rxConfig_s; -typedef void (*pidControllerFuncPtr)(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); // pid controller function prototype - void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); + const union rollAndPitchTrims_u *angleTrim, uint16_t midrc); extern float axisPIDf[3]; extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];