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 control to simplify the code and modifications

Browse Source 
Each PID uses its internal dt, so may be various contexts with different rate.
PID has max dt, so no need to reset explicitly.
This commit is contained in:
Oleg Kalachev
2025-10-20 22:54:18 +03:00
parent d06eb2a1aa
commit ca595edce5
2 changed files with 30 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
flix/control.ino
View File

@@ -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;
}

30
flix/pid.h
View File

@@ -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;
};