aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2026-01-09 12:36:49 +00:00
Code Issues Packages Projects Releases Wiki Activity

Refactor PID controllers

Browse Source 
Use t variable instead of passing dt argument.
Reset PID automatically on large dts.
This commit is contained in:
Oleg Kalachev
2025-10-17 18:53:15 +03:00
parent 48ba55aa7e
commit 5b37c87166
2 changed files with 24 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
flix/control.ino
View File

@@ -100,12 +100,7 @@ void interpretControls() {
}
void controlAttitude() {
if (!armed || attitudeTarget.invalid()) { // skip attitude control
rollPID.reset();
pitchPID.reset();
yawPID.reset();
return;
}
if (!armed || attitudeTarget.invalid() || thrustTarget < 0.1) return; // skip attitude control
const Vector up(0, 0, 1);
Vector upActual = Quaternion::rotateVector(up, attitude);
@@ -113,28 +108,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() || thrustTarget < 0.1) 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() {

29
flix/pid.h
View File

@@ -9,40 +9,43 @@
class PID {
public:
float p = 0;
float i = 0;
float d = 0;
float windup = 0;
float p, i, d;
float windup;
float maxdt;
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 maxdt = 0.1) : p(p), i(i), d(d), windup(windup), lpf(dAlpha), maxdt(maxdt) {}
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 < maxdt) {
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;
};