Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Clarify rates control code

Oleg Kalachev476f24f774f1
diff --git a/flix/control.ino b/flix/control.ino
index d563f36..bd145a2 100644
--- a/flix/control.ino
+++ b/flix/control.ino
@@ -137,10 +137,12 @@ void controlRate() {
}
Vector ratesFiltered = ratesFilter.update(rates);
+ Vector error = ratesTarget - ratesFiltered;
- torqueTarget.x = rollRatePID.update(ratesTarget.x - ratesFiltered.x, dt); // un-normalized "torque"
- torqueTarget.y = pitchRatePID.update(ratesTarget.y - ratesFiltered.y, dt);
- torqueTarget.z = yawRatePID.update(ratesTarget.z - ratesFiltered.z, dt);
+ // 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);
}
void controlTorque() {