Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Refactor PID control to simplify the code and modifications Each PID uses its internal dt, so may be various contexts with different rate. PID has max dt, so no need to reset explicitly.

Oleg Kalachevca595edce5e8
diff --git a/flix/control.ino b/flix/control.ino
index 38c59d7..40c1f21 100644
--- a/flix/control.ino
+++ b/flix/control.ino
@@ -9,7 +9,6 @@
#include "lpf.h"
#include "util.h"
-#define ARMED_THRUST 0.1 // thrust to indicate armed state
#define PITCHRATE_P 0.05
#define PITCHRATE_I 0.2
#define PITCHRATE_D 0.001
@@ -100,12 +99,7 @@ void interpretControls() {
}
void controlAttitude() {
- if (!armed || attitudeTarget.invalid()) { // skip attitude control
- rollPID.reset();
- pitchPID.reset();
- yawPID.reset();
- return;
- }
+ if (!armed || attitudeTarget.invalid()) return; // skip attitude control
const Vector up(0, 0, 1);
Vector upActual = Quaternion::rotateVector(up, attitude);
@@ -113,28 +107,23 @@ void controlAttitude() {
Vector error = Vector::rotationVectorBetween(upTarget, upActual);
- ratesTarget.x = rollPID.update(error.x, dt) + ratesExtra.x;
- ratesTarget.y = pitchPID.update(error.y, dt) + ratesExtra.y;
+ ratesTarget.x = rollPID.update(error.x) + ratesExtra.x;
+ ratesTarget.y = pitchPID.update(error.y) + ratesExtra.y;
float yawError = wrapAngle(attitudeTarget.getYaw() - attitude.getYaw());
- ratesTarget.z = yawPID.update(yawError, dt) + ratesExtra.z;
+ ratesTarget.z = yawPID.update(yawError) + ratesExtra.z;
}
void controlRates() {
- if (!armed || ratesTarget.invalid()) { // skip rates control
- rollRatePID.reset();
- pitchRatePID.reset();
- yawRatePID.reset();
- return;
- }
+ if (!armed || ratesTarget.invalid()) return; // skip rates control
Vector error = ratesTarget - rates;
// Calculate desired torque, where 0 - no torque, 1 - maximum possible torque
- torqueTarget.x = rollRatePID.update(error.x, dt);
- torqueTarget.y = pitchRatePID.update(error.y, dt);
- torqueTarget.z = yawRatePID.update(error.z, dt);
+ torqueTarget.x = rollRatePID.update(error.x);
+ torqueTarget.y = pitchRatePID.update(error.y);
+ torqueTarget.z = yawRatePID.update(error.z);
}
void controlTorque() {
@@ -145,12 +134,11 @@ void controlTorque() {
return;
}
- if (thrustTarget < 0.05) {
- // minimal thrust to indicate armed state
- motors[0] = ARMED_THRUST;
- motors[1] = ARMED_THRUST;
- motors[2] = ARMED_THRUST;
- motors[3] = ARMED_THRUST;
+ if (thrustTarget < 0.1) {
+ motors[0] = 0.1; // idle thrust
+ motors[1] = 0.1;
+ motors[2] = 0.1;
+ motors[3] = 0.1;
return;
}
diff --git a/flix/pid.h b/flix/pid.h
index cdfbcae..ac7a66b 100644
--- a/flix/pid.h
+++ b/flix/pid.h
@@ -9,40 +9,44 @@
class PID {
public:
- float p = 0;
- float i = 0;
- float d = 0;
- float windup = 0;
+ float p, i, d;
+ float windup;
+ float dtMax;
float derivative = 0;
float integral = 0;
LowPassFilter<float> lpf; // low pass filter for derivative term
- PID(float p, float i, float d, float windup = 0, float dAlpha = 1) : p(p), i(i), d(d), windup(windup), lpf(dAlpha) {};
+ PID(float p, float i, float d, float windup = 0, float dAlpha = 1, float dtMax = 0.1) :
+ p(p), i(i), d(d), windup(windup), lpf(dAlpha), dtMax(dtMax) {}
- float update(float error, float dt) {
- integral += error * dt;
+ float update(float error) {
+ float dt = t - prevTime;
- if (isfinite(prevError) && dt > 0) {
- // calculate derivative if both dt and prevError are valid
- derivative = (error - prevError) / dt;
-
- // apply low pass filter to derivative
- derivative = lpf.update(derivative);
+ if (dt > 0 && dt < dtMax) {
+ integral += error * dt;
+ derivative = lpf.update((error - prevError) / dt); // compute derivative and apply low-pass filter
+ } else {
+ integral = 0;
+ derivative = 0;
}
prevError = error;
+ prevTime = t;
return p * error + constrain(i * integral, -windup, windup) + d * derivative; // PID
}
void reset() {
prevError = NAN;
+ prevTime = NAN;
integral = 0;
derivative = 0;
+ lpf.reset();
}
private:
float prevError = NAN;
+ float prevTime = NAN;
};