From 82c80405206bf28a04f6e7cb848c2241caaddf9c Mon Sep 17 00:00:00 2001
From: philsson <philsson@gmail.com>
Date: Fri, 14 Oct 2016 18:07:38 +0200
Subject: [PATCH] Airmode is fixed. Still need to think about the "non Airmode"
 implementation

---
 UAV-ControlSystem/src/drivers/motormix.c | 40 +++++++++++++++++-------
 1 file changed, 29 insertions(+), 11 deletions(-)

diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c
index fa54b27..b370f55 100644
--- a/UAV-ControlSystem/src/drivers/motormix.c
+++ b/UAV-ControlSystem/src/drivers/motormix.c
@@ -127,6 +127,7 @@ void mix()
 		int16_t throttleMin = mixerConfig.minThrottle;	// Import system variable
 		int16_t throttleMax = mixerConfig.maxCommand; // Import
 		int16_t throttleRange = throttleMax - throttleMin; // The throttle range we have with current defines
+		int16_t throttleMid = (throttleMax + throttleMin) / 2;
 
 		//TODO: Seems to be causing an error when maxing any of the stick with low throttle
 		/* Check if we have enough interval for the adjustments */
@@ -143,14 +144,16 @@ void mix()
 				RPY_Mix[i] = RPY_Mix[i] * mixReduction;
 
 				//TEMPFIX recalculate
-				if (RPY_Mix[i] > RPY_Mix_Max) RPY_Mix_Max = RPY_Mix[i];
-				if (RPY_Mix[i] < RPY_Mix_Min) RPY_Mix_Min = RPY_Mix[i];
+//				if (RPY_Mix[i] > RPY_Mix_Max) RPY_Mix_Max = RPY_Mix[i];
+//				if (RPY_Mix[i] < RPY_Mix_Min) RPY_Mix_Min = RPY_Mix[i];
 			}
 
-			//Temp fix may not be right
-			RPY_MixRange = RPY_Mix_Max - RPY_Mix_Min;
-			throttleMin += (RPY_MixRange / 2);
-			throttleMax -= (RPY_MixRange / 2);
+			//Temp fix may not be right - Set throttle to exakt half (As this craft is symmetric)
+			throttle = mixerConfig.minThrottle + mixReduction * RPY_Mix_Max;
+			//throttle += mixerConfig.minThrottle + throttleRange / 2;
+//			RPY_MixRange = RPY_Mix_Max - RPY_Mix_Min;
+//			throttleMin += (RPY_MixRange / 2);
+//			throttleMax -= (RPY_MixRange / 2);
 		}
 		// If we have the needed range no scaling is needed
 		else
@@ -159,15 +162,30 @@ void mix()
 
 			/* Update min and max throttle so we can add the
 			 * calculated adjustments and still just max out */
-			throttleMin += (RPY_MixRange / 2);
-			throttleMax -= (RPY_MixRange / 2);
-		}
+			throttleMin += (RPY_MixRange / 2); 	// Can be removed. Just made to be sure
+			throttleMax -= (RPY_MixRange / 2);	// Can be removed. Just made to be sure
 
+			/* The scaling of the throttle in the room thats left from the mixer */
+			float throttleRescale = ((float)(throttleRange - RPY_MixRange)) / (float)throttleRange;
+
+			/* Make sure throttle mostly is inside range. Mostly above minthrottle before scaling */
+			throttle = constrain(throttle, mixerConfig.minThrottle, mixerConfig.maxCommand);
+
+			/* Converting throttle to value centered around 0 */
+			throttle = (throttle - (mixerConfig.maxCommand - throttleRange / 2));
+
+			/* Rescaling throttle */
+			throttle = throttle*throttleRescale;
+
+			/* Adding new scaled throttle to throttleMid */
+			throttle = throttleMid + throttle;
+
+		}
 
 		// Now we add desired throttle
 		for (int i = 0; i < MOTOR_COUNT; i++)
 		{
-			// Constrain in within the regulation of the mix
+			// Constrain in within the regulation of the mix - OBS. Constrain can be removed. Just to make sure
 			motor_output[i] = RPY_Mix[i] + constrain(throttle * mixerUAV[i].throttle, throttleMin, throttleMax);
 			//motor_output[i] = constrain(motor_output[i], mixerConfig.minCommand, mixerConfig.maxCommand);
 		}
@@ -221,7 +239,7 @@ void mix()
 	for (int i = 0; i < MOTOR_COUNT; i++)
 	{
 		/* If engines are armed then give the output to the motors */
-		if (flags_IsSet_ID(systemFlags_armed_id)) // TODO: replace with check for armed (IF ARMED)
+		if (flags_IsSet_ID(systemFlags_armed_id))
 			motor_output[i] = constrain(motor_output[i], mixerConfig.minCommand, mixerConfig.maxCommand);
 		/* If not then stop motors */
 		else