Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Implement motors output desaturation So the drone continues stabilization on max thrust.

Oleg Kalachev2ccda03573e6
diff --git a/flix/control.ino b/flix/control.ino
index 1c1a8b7..aa7edb6 100644
--- a/flix/control.ino
+++ b/flix/control.ino
@@ -149,12 +149,25 @@ void controlTorque() {
motors[MOTOR_REAR_LEFT] = thrustTarget + torqueTarget.x + torqueTarget.y - torqueTarget.z;
motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.x + torqueTarget.y + torqueTarget.z;
+ desaturate(motors[MOTOR_FRONT_LEFT], motors[MOTOR_FRONT_RIGHT], motors[MOTOR_REAR_LEFT], motors[MOTOR_REAR_RIGHT]);
+
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);
}
+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;
+ }
+}
+
const char* getModeName() {
switch (mode) {
case RAW: return "RAW";
diff --git a/gazebo/Arduino.h b/gazebo/Arduino.h
index 2cbaaf0..d0512a1 100644
--- a/gazebo/Arduino.h
+++ b/gazebo/Arduino.h
@@ -21,6 +21,8 @@
#define degrees(rad) ((rad)*RAD_TO_DEG)
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
+template<typename T> T max(T a, T b) { return a > b ? a : b; }
+template<typename T> T min(T a, T b) { return a < b ? a : b; }
long map(long x, long in_min, long in_max, long out_min, long out_max) {
const long run = in_max - in_min;
diff --git a/gazebo/flix.h b/gazebo/flix.h
index 96a3aad..899245f 100644
--- a/gazebo/flix.h
+++ b/gazebo/flix.h
@@ -32,6 +32,7 @@ void interpretControls();
void controlAttitude();
void controlRates();
void controlTorque();
+void desaturate(float& a, float& b, float& c, float& d);
const char* getModeName();
void sendMotors();
int getDutyCycle(float value);