Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Simplify and improve control code Add lower bound desaturation. Remove constrain calls as they are performed in motors subsystem.

Oleg Kalacheve7100f03ce8a
diff --git a/flix/control.ino b/flix/control.ino
index 1164a2d..1c09067 100644
--- a/flix/control.ino
+++ b/flix/control.ino
@@ -107,15 +107,12 @@ void controlTorque() {
if (!torqueTarget.valid()) return; // skip torque control
if (!armed) {
- memset(motors, 0, sizeof(motors)); // stop motors if disarmed
+ for (float& m : motors) m = 0; // stop motors if disarmed
return;
}
if (thrustTarget < 0.1) {
- motors[0] = 0.1; // idle thrust
- motors[1] = 0.1;
- motors[2] = 0.1;
- motors[3] = 0.1;
+ for (float& m : motors) m = 0.1; // idle thrust
return;
}
@@ -124,23 +121,27 @@ void controlTorque() {
motors[MOT_RL] = thrustTarget + torqueTarget.x + torqueTarget.y - torqueTarget.z;
motors[MOT_RR] = thrustTarget - torqueTarget.x + torqueTarget.y + torqueTarget.z;
- // Prioritize angle control over thrust control
- desaturate(motors[MOT_FL], motors[MOT_FR], motors[MOT_RL], motors[MOT_RR]);
-
- motors[0] = constrain(motors[0], 0, 1);
- motors[1] = constrain(motors[1], 0, 1);
- motors[2] = constrain(motors[2], 0, 1);
- motors[3] = constrain(motors[3], 0, 1);
+ desaturate(); // prioritize angle control over thrust control
}
-void desaturate(float& a, float& b, float& c, float& d) {
- float maxThrust = max(max(a, b), max(c, d));
- if (maxThrust > 1) {
- float diff = maxThrust - 1;
- a -= diff;
- b -= diff;
- c -= diff;
- d -= diff;
+void desaturate() {
+ float max_ = -INFINITY, min_ = INFINITY;
+ float correction = 0;
+
+ // Find maximum and minimum thrust
+ for (float m : motors) {
+ if (m > max_) max_ = m;
+ if (m < min_) min_ = m;
+ }
+
+ if (max_ > 1) { // thrust is above maximum
+ correction = max_ - 1;
+ } else if (min_ < 0) { // thrust is below minimum
+ correction = min_;
+ }
+
+ for (float& m : motors) {
+ m -= correction;
}
}
diff --git a/gazebo/flix.h b/gazebo/flix.h
index d9a33fc..cdfb624 100644
--- a/gazebo/flix.h
+++ b/gazebo/flix.h
@@ -36,7 +36,7 @@ void interpretControls();
void controlAttitude();
void controlRates();
void controlTorque();
-void desaturate(float& a, float& b, float& c, float& d);
+void desaturate();
const char* getModeName();
void sendMotors();
int getDutyCycle(float value);