diff --git a/UAV-ControlSystem/inc/drivers/sbus.h b/UAV-ControlSystem/inc/drivers/sbus.h
index 061600b..486268a 100644
--- a/UAV-ControlSystem/inc/drivers/sbus.h
+++ b/UAV-ControlSystem/inc/drivers/sbus.h
@@ -25,7 +25,7 @@
 #define SBUS_FRAME_SIZE 25
 #define SBUS_HEADER 0x0F
 #define SBUS_FOOTER 0x00
-#define USART1_SBUS_DMA_SIZE SBUS_FRAME_SIZE * 5  + 1 // sbus package is 176 bits (22 bytes)
+#define USART1_SBUS_DMA_SIZE SBUS_FRAME_SIZE + 1 // sbus package is 176 bits (22 bytes)
 
 #define SBUS_MAX_CHANNEL 18 // Including two digital
 #define STICK_CHANNEL_COUNT 4
diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c
index c6e7e64..fa54b27 100644
--- a/UAV-ControlSystem/src/drivers/motormix.c
+++ b/UAV-ControlSystem/src/drivers/motormix.c
@@ -128,7 +128,7 @@ void mix()
 		int16_t throttleMax = mixerConfig.maxCommand; // Import
 		int16_t throttleRange = throttleMax - throttleMin; // The throttle range we have with current defines
 
-
+		//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 */
 		// Check if we maxed out
 		if (RPY_MixRange > throttleRange)
@@ -139,7 +139,18 @@ void mix()
 			float mixReduction = (float) throttleRange / RPY_MixRange;
 			// Apply the scaling to all outputs
 			for (int i = 0; i < MOTOR_COUNT; i++)
+			{
 				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];
+			}
+
+			//Temp fix may not be right
+			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
diff --git a/UAV-ControlSystem/src/drivers/sbus.c b/UAV-ControlSystem/src/drivers/sbus.c
index 440f372..3fbc7e4 100644
--- a/UAV-ControlSystem/src/drivers/sbus.c
+++ b/UAV-ControlSystem/src/drivers/sbus.c
@@ -28,7 +28,7 @@
 
 /* This instance is read by the whole system and should contain actual RX data */
 sbusFrame_s sbusChannelData = {0};
-dma_usart_return raw_dma_data_t = {0};
+dma_usart_return raw_dma_data_t;
 
 /* Create a DMA Handler */
 usart_dma_profile dmaHandler;
@@ -107,16 +107,28 @@ void sbus_read()
 	static int sbus_arr_iterator = SBUS_FRAME_SIZE;
 	static bool stop_bit_read = false;
 
+
 	// If continue only if we get new data from DMA
 	if (raw_dma_data_t.new_data)
 	{
 
 		for (int i = 0; i < USART1_SBUS_DMA_SIZE; i++)
 		{
+			uint8_t msg = raw_dma_data_t.buff[i];
 			// Look for the beginning of a sbus frame
 			if ( message_it == 0 ) //&& stop_bit_read)
 			{
-				message_it = (raw_dma_data_t.buff[i] == ((uint8_t)SBUS_HEADER)) ? 1 : 0;
+				//message_it = (raw_dma_data_t.buff[i] == ((uint8_t)SBUS_HEADER)) ? 1 : 0;
+				if (msg == ((uint8_t)SBUS_HEADER))
+				{
+					sbus_arr[(message_it)] = msg;
+					message_it++;
+				}
+				else
+				{
+					message_it = 0;
+				}
+
 //				sbus_arr_iterator = 0;
 //				stop_bit_read = false;
 			}
@@ -125,19 +137,17 @@ void sbus_read()
 			else
 			{
 
-				if (message_it -1 < SBUS_FRAME_SIZE)
+				if ((message_it) < SBUS_FRAME_SIZE)
 				{
-					sbus_arr[message_it - 1] = raw_dma_data_t.buff[i];
+					sbus_arr[(message_it)] = msg;
 					message_it++;
 				}
-				if (message_it - 1 == SBUS_FRAME_SIZE)
+
+				if ((message_it) == SBUS_FRAME_SIZE)
 				{
-					if (raw_dma_data_t.buff[i] == SBUS_FOOTER)
-
+					message_it = 0;
+					if (msg == (uint8_t)SBUS_FOOTER)
 					{
-						message_it = 0;
-
-
 
 					//stop_bit_read = true;
 					// If the expected byte is stop byte, then we overwrite to the return value.
@@ -179,7 +189,9 @@ void sbus_read()
 						sbusChannelData.chan8 	= rx_truncate(sbusChannelData.chan8);
 					}
 					else
+					{
 						message_it = 0;
+					}
 
 				}
 			}
diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c
index 0e884c5..ce75b22 100644
--- a/UAV-ControlSystem/src/main.c
+++ b/UAV-ControlSystem/src/main.c
@@ -28,6 +28,7 @@
 #include "drivers/motormix.h"
 #include "drivers/motors.h"
 
+
 /**************************************************************************
 * BRIEF: Should contain all the initializations of the system, needs to
 * run before the scheduler.
@@ -57,6 +58,7 @@ void init_system()
 	//init motors to run with oneshot 125
 	pwmEnableAllMotors(Oneshot125);
 
+
 #ifdef USE_LEDS
 	//Initialize the on board leds
 	ledReavoEnable();