Simplify and improve control code

Add lower bound desaturation.
Remove constrain calls as they are performed in motors subsystem.

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
+		for (float& m : motors) m = 0.1; // idle thrust
-		motors[1] = 0.1;
-		motors[2] = 0.1;
-		motors[3] = 0.1;
 		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(); // 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);
 }
 
-void desaturate(float& a, float& b, float& c, float& d) {
+void desaturate() {
-	float maxThrust = max(max(a, b), max(c, d));
+	float max_ = -INFINITY, min_ = INFINITY;
-	if (maxThrust > 1) {
+	float correction = 0;
-		float diff = maxThrust - 1;
+
-		a -= diff;
+	// Find maximum and minimum thrust
-		b -= diff;
+	for (float m : motors) {
-		c -= diff;
+		if (m > max_) max_ = m;
-		d -= diff;
+		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;
 	}
 }
 

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);