Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
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.inoindex 1164a2d..1c09067 100644--- a/flix/control.ino+++ b/flix/control.ino@@ -107,15 +107,12 @@ void controlTorque() {if (!torqueTarget.valid()) return; // skip torque controlif (!armed) {- memset(motors, 0, sizeof(motors)); // stop motors if disarmed+ for (float& m : motors) m = 0; // stop motors if disarmedreturn;}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 thrustreturn;}@@ -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.hindex 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);