Added total thrust saturation in MotorMixer.

1 files changed, 23 insertions, 6 deletions

diff --git a/motormixer.cpp b/motormixer.cpp
index 1951f29..5718526 100644
--- a/motormixer.cpp
+++ b/motormixer.cpp
@@ -3,6 +3,15 @@
 #include <ch.h>
 #include <hal.h>
 
+template<class T>
+inline T abs(T val) {
+	if(val > 0) {
+		return val;
+	} else {
+		return -val;
+	}
+}
+
 namespace {
 	uint16_t motors[4];
 	
@@ -49,13 +58,21 @@ void MotorMixer::update(int16_t thrust, int16_t pitch, int16_t roll, int16_t yaw
 		thrust = 1000;
 	}
 	
-	//pitch = ((ppmsum.data[1] - 500) * thrust) >> 10;
-	//roll  = ((ppmsum.data[0] - 500) * thrust) >> 10;
-	//yaw   = ((ppmsum.data[3] - 500) * thrust) >> 10;
+	// Check that total thrust will not be exceeded.
+	if(abs(pitch) + abs(roll) > thrust) {
+		// If saturated by pitch/roll, drop yaw component.
+		yaw = 0;
+		
+		// Scale pitch and roll.
+		int32_t df = ((abs(pitch) + abs(roll)) << 16) / thrust;
+		
+		pitch = (pitch << 16) / df;
+		roll = (roll << 16) / df;
 	
-	//pitch = (-i2c_thread.x) >> 5;
-	//roll = i2c_thread.y >> 5;
-	//yaw = i2c_thread.z >> 5;
+	} else if(abs(pitch) + abs(roll) + abs(yaw) > thrust) {
+		// Scale yaw value.
+		yaw = (yaw > 0 ? 1 : -1) * (thrust - abs(pitch) - abs(roll));
+	}
 	
 	motors[0] = thrust + pitch + roll - yaw;
 	motors[1] = thrust + pitch - roll + yaw;