Tidy PID controller
This commit is contained in:
parent
1c3838320b
commit
695f47944b
|
@ -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;}
|
||||
}
|
||||
|
|
|
@ -21,7 +21,6 @@
|
|||
|
||||
#include <platform.h>
|
||||
|
||||
#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)
|
||||
|
|
|
@ -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];
|
||||
|
|
Loading…
Reference in New Issue