Implement motors output desaturation

So the drone continues stabilization on max thrust.

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

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;

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