From bce8d66ea0dd61cc1690be64e85195589f452a5f Mon Sep 17 00:00:00 2001
From: Jonas Holmberg <holmis.1993@gmail.com>
Date: Thu, 13 Oct 2016 17:10:13 +0200
Subject: [PATCH] May have fixed the problem

May have solved the issue with the throttle not wokring when maxing some
of the stick controlls. Could still not be correct but the issue seems
to have been located.
---
 UAV-ControlSystem/inc/drivers/sbus.h     |  2 +-
 UAV-ControlSystem/src/drivers/motormix.c | 13 +++++++++-
 UAV-ControlSystem/src/drivers/sbus.c     | 32 ++++++++++++++++--------
 UAV-ControlSystem/src/main.c             |  2 ++
 4 files changed, 37 insertions(+), 12 deletions(-)

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