From 0df214b68af6b5e8cca1d958ca777d128e86004f Mon Sep 17 00:00:00 2001
From: philsson <philsson@gmail.com>
Date: Thu, 13 Oct 2016 14:04:29 +0200
Subject: [PATCH 1/7] Fixing RC problems on header in message

---
 UAV-ControlSystem/inc/drivers/motormix.h |   2 +-
 UAV-ControlSystem/inc/drivers/sbus.h     |   2 +-
 UAV-ControlSystem/src/drivers/motormix.c |  19 ++--
 UAV-ControlSystem/src/drivers/sbus.c     | 108 ++++++++++++++---------
 4 files changed, 78 insertions(+), 53 deletions(-)

diff --git a/UAV-ControlSystem/inc/drivers/motormix.h b/UAV-ControlSystem/inc/drivers/motormix.h
index 498bb8d..066a92a 100644
--- a/UAV-ControlSystem/inc/drivers/motormix.h
+++ b/UAV-ControlSystem/inc/drivers/motormix.h
@@ -25,7 +25,7 @@
 #define MOTOR_COUNT 10
 
 // TODO: These are only temporary before merge with PID part
-uint16_t PID_Out[3];
+int16_t PID_Out[3];
 
 // TODO: Temporary before PID
 typedef enum {
diff --git a/UAV-ControlSystem/inc/drivers/sbus.h b/UAV-ControlSystem/inc/drivers/sbus.h
index 8076b61..061600b 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  + 1 // sbus package is 176 bits (22 bytes)
+#define USART1_SBUS_DMA_SIZE SBUS_FRAME_SIZE * 5  + 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 433dcca..c6e7e64 100644
--- a/UAV-ControlSystem/src/drivers/motormix.c
+++ b/UAV-ControlSystem/src/drivers/motormix.c
@@ -97,9 +97,9 @@ void mix()
 	//uint16_t throttleIdle = mixerConfig.minThrottle + (throttleRange / 2);
 
 	int16_t RPY_Mix[MOTOR_COUNT];		// Roll Pitch and Yaw variables array
-	uint16_t RPY_Mix_Min = 0; 			// Stores the minimum desired command for any motor
-	uint16_t RPY_Mix_Max = 0;			// Maximum desired command for any motor
-	uint16_t throttle = sbusChannelData.chan3;	 // Import throttle value from remote
+	int16_t RPY_Mix_Min = 0; 			// Stores the minimum desired command for any motor
+	int16_t RPY_Mix_Max = 0;			// Maximum desired command for any motor
+	int16_t throttle = sbusChannelData.chan3;	 // Import throttle value from remote
 
 	// Might be used for some debug if necessary
 	static bool motorLimitReached;
@@ -122,10 +122,11 @@ void mix()
 		}
 
 
-		uint16_t throttleRange = mixerConfig.maxCommand - mixerConfig.minThrottle; // The throttle range we have with current defines
-		uint16_t RPY_MixRange = RPY_Mix_Max - RPY_Mix_Min; // Range of the desired mixer outputs
-		uint16_t throttleMin = mixerConfig.minThrottle;	// Import system variable
-		uint16_t throttleMax = mixerConfig.maxThrottle; // Import
+
+		int16_t RPY_MixRange = RPY_Mix_Max - RPY_Mix_Min; // Range of the desired mixer outputs
+		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
 
 
 		/* Check if we have enough interval for the adjustments */
@@ -156,8 +157,8 @@ void mix()
 		for (int i = 0; i < MOTOR_COUNT; i++)
 		{
 			// Constrain in within the regulation of the mix
-			motor_output[i] = RPY_Mix[i] +  constrain(throttle * mixerUAV[i].throttle, throttleMin, mixerConfig.maxCommand);
-			motor_output[i] = constrain(motor_output[i], mixerConfig.minCommand, mixerConfig.maxCommand);
+			motor_output[i] = RPY_Mix[i] + constrain(throttle * mixerUAV[i].throttle, throttleMin, throttleMax);
+			//motor_output[i] = constrain(motor_output[i], mixerConfig.minCommand, mixerConfig.maxCommand);
 		}
 	}
 	else // Airmode not active
diff --git a/UAV-ControlSystem/src/drivers/sbus.c b/UAV-ControlSystem/src/drivers/sbus.c
index e518afd..440f372 100644
--- a/UAV-ControlSystem/src/drivers/sbus.c
+++ b/UAV-ControlSystem/src/drivers/sbus.c
@@ -102,6 +102,7 @@ void sbus_read()
 {
 	// Holds what we've read so far
 	static uint8_t sbus_arr[SBUS_FRAME_SIZE];
+	static uint8_t message_it = 0;
 
 	static int sbus_arr_iterator = SBUS_FRAME_SIZE;
 	static bool stop_bit_read = false;
@@ -109,61 +110,84 @@ void sbus_read()
 	// 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++)
 		{
 			// Look for the beginning of a sbus frame
-			if (raw_dma_data_t.buff[i] == (uint8_t)SBUS_HEADER && stop_bit_read)
+			if ( message_it == 0 ) //&& stop_bit_read)
 			{
-				sbus_arr_iterator = 0;
-				stop_bit_read = false;
+				message_it = (raw_dma_data_t.buff[i] == ((uint8_t)SBUS_HEADER)) ? 1 : 0;
+//				sbus_arr_iterator = 0;
+//				stop_bit_read = false;
 			}
 			// Look for the end of sbus frame
-			else if(raw_dma_data_t.buff[i] == (uint8_t)SBUS_FOOTER)
+			//else if(raw_dma_data_t.buff[i] == (uint8_t)SBUS_FOOTER)
+			else
 			{
-				stop_bit_read = true;
-				// If the expected byte is stop byte, then we overwrite to the return value.
-				if (sbus_arr_iterator == SBUS_FRAME_SIZE - 1)
+
+				if (message_it -1 < SBUS_FRAME_SIZE)
 				{
-					sbusChannelData = *(sbusFrame_s*)sbus_arr;
+					sbus_arr[message_it - 1] = raw_dma_data_t.buff[i];
+					message_it++;
+				}
+				if (message_it - 1 == SBUS_FRAME_SIZE)
+				{
+					if (raw_dma_data_t.buff[i] == SBUS_FOOTER)
 
-					// Linear fitting
-
-					sbusChannelData.chan1 	= SBUS_UNIT_CONV(sbusChannelData.chan1);
-					sbusChannelData.chan2 	= SBUS_UNIT_CONV(sbusChannelData.chan2);
-					sbusChannelData.chan3 	= SBUS_UNIT_CONV(sbusChannelData.chan3);
-					sbusChannelData.chan4 	= SBUS_UNIT_CONV(sbusChannelData.chan4);
-					sbusChannelData.chan5 	= SBUS_UNIT_CONV(sbusChannelData.chan5);
-					sbusChannelData.chan6 	= SBUS_UNIT_CONV(sbusChannelData.chan6);
-					sbusChannelData.chan7 	= SBUS_UNIT_CONV(sbusChannelData.chan7);
-					sbusChannelData.chan8 	= SBUS_UNIT_CONV(sbusChannelData.chan8);
-
-					// TODO: Depending on defines don't process more than necessary
-					sbusChannelData.chan9 	= SBUS_UNIT_CONV(sbusChannelData.chan9);
-					sbusChannelData.chan10 	= SBUS_UNIT_CONV(sbusChannelData.chan10);
-					sbusChannelData.chan11 	= SBUS_UNIT_CONV(sbusChannelData.chan11);
-					sbusChannelData.chan12 	= SBUS_UNIT_CONV(sbusChannelData.chan12);
-					sbusChannelData.chan13 	= SBUS_UNIT_CONV(sbusChannelData.chan13);
-					sbusChannelData.chan14 	= SBUS_UNIT_CONV(sbusChannelData.chan14);
-					sbusChannelData.chan15 	= SBUS_UNIT_CONV(sbusChannelData.chan15);
-					sbusChannelData.chan16 	= SBUS_UNIT_CONV(sbusChannelData.chan16);
-
-					// TODO: Failsafe using defines checking if channels are in range BEFORE we truncate
+					{
+						message_it = 0;
 
 
-					sbusChannelData.chan1 	= rx_truncate(sbusChannelData.chan1);
-					sbusChannelData.chan2 	= rx_truncate(sbusChannelData.chan2);
-					sbusChannelData.chan3 	= rx_truncate(sbusChannelData.chan3);
-					sbusChannelData.chan4 	= rx_truncate(sbusChannelData.chan4);
-					sbusChannelData.chan5 	= rx_truncate(sbusChannelData.chan5);
-					sbusChannelData.chan6 	= rx_truncate(sbusChannelData.chan6);
-					sbusChannelData.chan7 	= rx_truncate(sbusChannelData.chan7);
-					sbusChannelData.chan8 	= rx_truncate(sbusChannelData.chan8);
+
+					//stop_bit_read = true;
+					// If the expected byte is stop byte, then we overwrite to the return value.
+					//if (sbus_arr_iterator == SBUS_FRAME_SIZE - 1)
+					//{
+						sbusChannelData = *(sbusFrame_s*)sbus_arr;
+
+						// Linear fitting
+
+						sbusChannelData.chan1 	= SBUS_UNIT_CONV(sbusChannelData.chan1);
+						sbusChannelData.chan2 	= SBUS_UNIT_CONV(sbusChannelData.chan2);
+						sbusChannelData.chan3 	= SBUS_UNIT_CONV(sbusChannelData.chan3);
+						sbusChannelData.chan4 	= SBUS_UNIT_CONV(sbusChannelData.chan4);
+						sbusChannelData.chan5 	= SBUS_UNIT_CONV(sbusChannelData.chan5);
+						sbusChannelData.chan6 	= SBUS_UNIT_CONV(sbusChannelData.chan6);
+						sbusChannelData.chan7 	= SBUS_UNIT_CONV(sbusChannelData.chan7);
+						sbusChannelData.chan8 	= SBUS_UNIT_CONV(sbusChannelData.chan8);
+
+						// TODO: Depending on defines don't process more than necessary
+						sbusChannelData.chan9 	= SBUS_UNIT_CONV(sbusChannelData.chan9);
+						sbusChannelData.chan10 	= SBUS_UNIT_CONV(sbusChannelData.chan10);
+						sbusChannelData.chan11 	= SBUS_UNIT_CONV(sbusChannelData.chan11);
+						sbusChannelData.chan12 	= SBUS_UNIT_CONV(sbusChannelData.chan12);
+						sbusChannelData.chan13 	= SBUS_UNIT_CONV(sbusChannelData.chan13);
+						sbusChannelData.chan14 	= SBUS_UNIT_CONV(sbusChannelData.chan14);
+						sbusChannelData.chan15 	= SBUS_UNIT_CONV(sbusChannelData.chan15);
+						sbusChannelData.chan16 	= SBUS_UNIT_CONV(sbusChannelData.chan16);
+
+						// TODO: Failsafe using defines checking if channels are in range BEFORE we truncate
+
+
+						sbusChannelData.chan1 	= rx_truncate(sbusChannelData.chan1);
+						sbusChannelData.chan2 	= rx_truncate(sbusChannelData.chan2);
+						sbusChannelData.chan3 	= rx_truncate(sbusChannelData.chan3);
+						sbusChannelData.chan4 	= rx_truncate(sbusChannelData.chan4);
+						sbusChannelData.chan5 	= rx_truncate(sbusChannelData.chan5);
+						sbusChannelData.chan6 	= rx_truncate(sbusChannelData.chan6);
+						sbusChannelData.chan7 	= rx_truncate(sbusChannelData.chan7);
+						sbusChannelData.chan8 	= rx_truncate(sbusChannelData.chan8);
+					}
+					else
+						message_it = 0;
+
 				}
 			}
-			// Copy next byte into the sbus_arr
-			if (sbus_arr_iterator < SBUS_FRAME_SIZE)
-				sbus_arr[sbus_arr_iterator] = raw_dma_data_t.buff[i];
-			sbus_arr_iterator++;
+
+//			// Copy next byte into the sbus_arr
+//			if (sbus_arr_iterator < SBUS_FRAME_SIZE)
+//				sbus_arr[sbus_arr_iterator] = raw_dma_data_t.buff[i];
+//			sbus_arr_iterator++;
 		}
 	}
 }

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 2/7] 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();

From 51ec5b41706482afae28f220f9fef93d8fc7e25f Mon Sep 17 00:00:00 2001
From: Jonas Holmberg <holmis.1993@gmail.com>
Date: Fri, 14 Oct 2016 15:32:49 +0200
Subject: [PATCH 3/7] Fixed some errors with the sbus RC command and other
 small things

Fixed an issue in the SBUS RC command read. Also fixed some things in
the motormix, not yet fixed totally. Also added a command in CLI to
calibrate the motors of the aircraft.
---
 UAV-ControlSystem/inc/config/eeprom.h    |  3 ++
 UAV-ControlSystem/inc/drivers/motors.h   | 10 ++++++
 UAV-ControlSystem/inc/system_variables.h |  2 +-
 UAV-ControlSystem/src/config/cli.c       | 38 +++++++++++++++++++++--
 UAV-ControlSystem/src/config/eeprom.c    |  9 ++++++
 UAV-ControlSystem/src/drivers/motors.c   | 33 ++++++++++++++++++++
 UAV-ControlSystem/src/drivers/sbus.c     | 39 ++++++++++++++++++++----
 UAV-ControlSystem/src/main.c             |  4 +--
 8 files changed, 127 insertions(+), 11 deletions(-)

diff --git a/UAV-ControlSystem/inc/config/eeprom.h b/UAV-ControlSystem/inc/config/eeprom.h
index 1af16b4..3b57398 100644
--- a/UAV-ControlSystem/inc/config/eeprom.h
+++ b/UAV-ControlSystem/inc/config/eeprom.h
@@ -147,6 +147,9 @@ typedef enum {
 	EEPROM_ADC_SCALES,
 	EEPROM_UART1_RX_INV,
 
+	/* Motor calibrate bool */
+	EEPROM_MOTORCALIBRATE,
+
 	/* Period values for tasks */
 	EEPROM_PERIOD_SYSTEM,
 	EEPROM_PERIOD_GYROPID,
diff --git a/UAV-ControlSystem/inc/drivers/motors.h b/UAV-ControlSystem/inc/drivers/motors.h
index ac89685..d7f32a1 100644
--- a/UAV-ControlSystem/inc/drivers/motors.h
+++ b/UAV-ControlSystem/inc/drivers/motors.h
@@ -15,6 +15,8 @@ typedef enum {
 	Oneshot125
 }motorOutput;
 
+extern bool perfromMotorCalibration;
+
 /**************************************************************************
 * BRIEF: Initializing a motor (maps a pwm signal to a certain pin)        										*
 * INFORMATION: The motor becomes active, Each motor configuration is changed in the "stm32f4xx_revo.h" file     *
@@ -70,5 +72,13 @@ void pwmAdjustSpeedOfMotor(uint8_t motor, uint16_t pulse);
 **************************************************************************/
 void pwmAdjustSpeedOfMotorDutyCycle(uint8_t motor, uint16_t DutyCycle);
 
+/**************************************************************************
+* BRIEF: 	   Calibrates the motors if it should                                             									 *
+* INFORMATION: Will perform a sequence that will calibrate the motors. The
+* 		       motors will only be calibrated if a boolean value that is ´
+* 		       checked inside have been set to true                                                        										 *
+**************************************************************************/
+void calibrateMotors();
+
 
 #endif /* DRIVERS_MOTORS_H_ */
diff --git a/UAV-ControlSystem/inc/system_variables.h b/UAV-ControlSystem/inc/system_variables.h
index 0cc221e..607b8e1 100644
--- a/UAV-ControlSystem/inc/system_variables.h
+++ b/UAV-ControlSystem/inc/system_variables.h
@@ -14,7 +14,7 @@
 #ifndef SYSTEM_VARIABLES_H_
 #define SYSTEM_VARIABLES_H_
 
-#define EEPROM_SYS_VERSION 101
+#define EEPROM_SYS_VERSION 102
 
 #define ADC_STATE
 #include "stm32f4xx.h"
diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c
index b6338fa..a1ab3fc 100644
--- a/UAV-ControlSystem/src/config/cli.c
+++ b/UAV-ControlSystem/src/config/cli.c
@@ -23,6 +23,7 @@
 #include <string.h>
 #include "utilities.h"
 #include "Scheduler/scheduler.h"
+#include "drivers/motors.h"
 
 #define cliActivateCharacter	35
 #define commandValueError		0xFFFFFFFFFFFFFFFF
@@ -71,6 +72,7 @@ typedef enum
 	ACTION_REBOOT,					//Reboots the entire system
 	ACTION_RESET,					//Resets the entire eeprom
 	ACTION_STATS,					//gives statistics on the system
+	ACTION_MOTORCALIBRATE,
 
 	//The number of actions
 	ACTION_COUNT,					//Count of the number of actions
@@ -91,7 +93,8 @@ const typeString commandAction_Strings[ACTION_COUNT] = {
 	"exit",
 	"reboot",
 	"reset",
-	"stats"
+	"stats",
+	"motorcalibrate"
 };
 
 /* String values descrbing information of a certain action */
@@ -108,6 +111,7 @@ const typeString commandActionInformation_Strings[ACTION_COUNT] = {
 	"| reboot - Exit CLI and reboots the system.\n\r",
 	"| reset - Restore all the values to its default values.\n\r",
 	"| stats - Gives some current stats of the system and tasks.\n\r"
+	"| motorcalibrate - Calibrates all motors."
 };
 
 /* String array containing all the signature examples for each action */
@@ -123,7 +127,8 @@ const typeString commandActionSignature_Strings[ACTION_COUNT] = {
 	" exit",
 	" reboot",
 	" reset",
-	" stats"
+	" stats",
+	" motorcalibrate"
 };
 
 /* Size values for the values a command will require */
@@ -148,6 +153,9 @@ typedef enum
 	COMMAND_ID_ADC_SCALES_RIGHT,
 	COMMAND_ID_PID_ROLL_KP,
 
+	/* calibrate motors command */
+	COMMAND_ID_MOTORCALIBRATE,
+
 	/* Period values for tasks */
 	COMMAND_ID_PERIOD_SYSTEM ,
 	COMMAND_ID_PERIOD_GYROPID,
@@ -269,6 +277,16 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = {
 		{
 			"pid_roll_kp", 			COMMAND_ID_PID_ROLL_KP, EEPROM_PID_ROLL_KP, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100}
 		},
+
+
+		/* Calibrate motors */
+		[COMMAND_ID_MOTORCALIBRATE] =
+		{
+			"motor_calibration", 	COMMAND_ID_MOTORCALIBRATE, EEPROM_MOTORCALIBRATE, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_BOOL, .valueRange = {0, 1}
+		},
+
+
+		/* set period commands */
 		[COMMAND_ID_PERIOD_SYSTEM] =
 		{
 			"task_period_system", 	COMMAND_ID_PERIOD_SYSTEM, EEPROM_PERIOD_SYSTEM, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_UINT_32, .valueRange = {0, 1000000000}
@@ -1259,6 +1277,22 @@ void cliRun()
 			case commandMask(commandSize_1, ACTION_STATS):
 					TransmitSystemStats();
 				break;
+			case commandMask(commandSize_1, ACTION_MOTORCALIBRATE):
+					TransmitBack("- Make sure PROPELLERS are REMOVED and be prepared that the aircraft my start the engines\n\n\r!");
+					TransmitBack("- REMOVE HANDS FROM THE AIRCRAFT\n\n\r");
+
+					TransmitBack("Do you really want to calibrate? (y/n)\n\n\r");
+					if (ReceiveBinaryDecision(121,110))
+					{
+						TransmitBack("Starting calibration, BE CAREFUL!!! \n\n\r");
+						calibrateMotors();
+					}
+					else
+					{
+						TransmitBack("Exiting the calibration, BE CAREFUL!!! \n\n\r");
+					}
+
+				break;
 			default:
 				if (actionId != ACTION_NOACTION)
 					TransmitCommandInstruction(actionId);
diff --git a/UAV-ControlSystem/src/config/eeprom.c b/UAV-ControlSystem/src/config/eeprom.c
index 15e85c1..81a4425 100644
--- a/UAV-ControlSystem/src/config/eeprom.c
+++ b/UAV-ControlSystem/src/config/eeprom.c
@@ -23,6 +23,7 @@
 #include "Scheduler/scheduler.h"
 #include "drivers/failsafe_toggles.h"
 #include "drivers/motormix.h"
+#include "drivers/motors.h"
 
 /* Reads the EEPROM version from EEPROM - Is compared to EEPROM_SYS_VERSION */
 uint8_t stored_eeprom_identifier;
@@ -95,6 +96,14 @@ EEPROM_DATA_t eeprom_sys_Arr[EEPROM_SYS_COUNT] = {
 		.dataPtr = &uart1_rx_inverter,
 	},
 
+	/* Motor calibrate */
+	[EEPROM_MOTORCALIBRATE] =
+	{
+		.size = sizeof(bool),
+		.dataPtr = &perfromMotorCalibration,
+	},
+
+
 	/* Task eeprom values */
 	[EEPROM_PERIOD_SYSTEM] =
 	{
diff --git a/UAV-ControlSystem/src/drivers/motors.c b/UAV-ControlSystem/src/drivers/motors.c
index 75a85b3..17902b6 100644
--- a/UAV-ControlSystem/src/drivers/motors.c
+++ b/UAV-ControlSystem/src/drivers/motors.c
@@ -12,6 +12,8 @@
 #include "stm32f4xx_revo.h"
 #include "drivers/pwm.h"
 #include "drivers/motors.h"
+#include "drivers/failsafe_toggles.h"
+#include "config/eeprom.h"
 
 const int MotorPWMPeriode = 2000; //Micro seconds
 const int MotorPWMInitPulse = 1000;
@@ -25,6 +27,8 @@ typedef struct
 	uint32_t channel;		//TIM_CHANNEL_1/TIM_CHANNEL_1/..
 }motorProfile;
 
+bool perfromMotorCalibration = false;
+
 /**************************************************************************
 * BRIEF: Returns the final pulse of a motor driver call
 * INFORMATION: The pulse is not allowed to be higher then 94 % of the total periode of the pwm signal, otherwise the pwm won't perform correctly
@@ -228,3 +232,32 @@ void pwmAdjustSpeedOfMotorDutyCycle(uint8_t motor, uint16_t DutyCycle)
 
 	setPwmPulse(profile.channel, profile.tim, checkPulse(pulse));
 }
+
+/**************************************************************************
+* BRIEF: 	   Calibrates the motors if it should                                             									 *
+* INFORMATION: Will perform a sequence that will calibrate the motors. The
+* 		       motors will only be calibrated if a boolean value that is ´
+* 		       checked inside have been set to true. This value will be ´
+* 		       set back to false after the function have been compleeted                                                        										 *
+**************************************************************************/
+void calibrateMotors()
+{
+
+		pwmDeactivateAllMotors();
+		HAL_Delay(1500);
+		pwmActivateAllMotors();
+
+		//First set the motor output to the maximum allowed throttle
+		for (uint8_t i = 1; i < 11; i++ ) pwmAdjustSpeedOfMotor(i,MotorPWMPeriode);
+
+		//wait for a little while
+		HAL_Delay(6200);
+
+		//Now set the speed of all motors to the lowest
+		for (uint8_t i = 1; i < 11; i++ ) pwmAdjustSpeedOfMotor(i,MotorPWMInitPulse);
+
+
+		perfromMotorCalibration = false;
+		saveEEPROM();
+
+}
diff --git a/UAV-ControlSystem/src/drivers/sbus.c b/UAV-ControlSystem/src/drivers/sbus.c
index 3fbc7e4..012e2c0 100644
--- a/UAV-ControlSystem/src/drivers/sbus.c
+++ b/UAV-ControlSystem/src/drivers/sbus.c
@@ -103,6 +103,9 @@ void sbus_read()
 	// Holds what we've read so far
 	static uint8_t sbus_arr[SBUS_FRAME_SIZE];
 	static uint8_t message_it = 0;
+	static uint32_t missedMsg = 0;
+	static uint8_t message_it_secondary_head = 0;
+	static bool new_header = false;
 
 	static int sbus_arr_iterator = SBUS_FRAME_SIZE;
 	static bool stop_bit_read = false;
@@ -112,6 +115,8 @@ void sbus_read()
 	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];
@@ -123,6 +128,7 @@ void sbus_read()
 				{
 					sbus_arr[(message_it)] = msg;
 					message_it++;
+					new_header = false;
 				}
 				else
 				{
@@ -136,6 +142,11 @@ void sbus_read()
 			//else if(raw_dma_data_t.buff[i] == (uint8_t)SBUS_FOOTER)
 			else
 			{
+				if (msg == (uint8_t)SBUS_HEADER && new_header == false)
+				{
+					new_header = true;
+					message_it_secondary_head = message_it;	//save the value of the position in The buffer array, not the dma array index
+				}
 
 				if ((message_it) < SBUS_FRAME_SIZE)
 				{
@@ -145,10 +156,11 @@ void sbus_read()
 
 				if ((message_it) == SBUS_FRAME_SIZE)
 				{
-					message_it = 0;
+					missedMsg++;
 					if (msg == (uint8_t)SBUS_FOOTER)
 					{
-
+						message_it = 0;
+						missedMsg--;
 					//stop_bit_read = true;
 					// If the expected byte is stop byte, then we overwrite to the return value.
 					//if (sbus_arr_iterator == SBUS_FRAME_SIZE - 1)
@@ -156,7 +168,6 @@ void sbus_read()
 						sbusChannelData = *(sbusFrame_s*)sbus_arr;
 
 						// Linear fitting
-
 						sbusChannelData.chan1 	= SBUS_UNIT_CONV(sbusChannelData.chan1);
 						sbusChannelData.chan2 	= SBUS_UNIT_CONV(sbusChannelData.chan2);
 						sbusChannelData.chan3 	= SBUS_UNIT_CONV(sbusChannelData.chan3);
@@ -177,8 +188,6 @@ void sbus_read()
 						sbusChannelData.chan16 	= SBUS_UNIT_CONV(sbusChannelData.chan16);
 
 						// TODO: Failsafe using defines checking if channels are in range BEFORE we truncate
-
-
 						sbusChannelData.chan1 	= rx_truncate(sbusChannelData.chan1);
 						sbusChannelData.chan2 	= rx_truncate(sbusChannelData.chan2);
 						sbusChannelData.chan3 	= rx_truncate(sbusChannelData.chan3);
@@ -190,7 +199,24 @@ void sbus_read()
 					}
 					else
 					{
-						message_it = 0;
+						int temp_secondaryHeader = message_it_secondary_head;
+						message_it = message_it - temp_secondaryHeader;	//update the counter to the empty part of the updated array
+						new_header = false;		//set new header to false, this is true if there is another header within the buffer
+
+						//Move all the remaning messages in the buffer to the start of the buffer
+						for (int i = temp_secondaryHeader; i < SBUS_FRAME_SIZE; i++)
+						{
+							int innerCount = i-temp_secondaryHeader;
+							sbus_arr[innerCount] = sbus_arr[i];
+
+							//check if we find another possible header inside the rest of the buffer and save that
+							if (sbus_arr[innerCount] == (uint8_t)SBUS_HEADER && innerCount > 0 && new_header == false )
+							{
+								new_header = true;
+								message_it_secondary_head = innerCount;
+							}
+						}
+
 					}
 
 				}
@@ -201,6 +227,7 @@ void sbus_read()
 //				sbus_arr[sbus_arr_iterator] = raw_dma_data_t.buff[i];
 //			sbus_arr_iterator++;
 		}
+
 	}
 }
 
diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c
index ce75b22..9d5d45d 100644
--- a/UAV-ControlSystem/src/main.c
+++ b/UAV-ControlSystem/src/main.c
@@ -55,10 +55,10 @@ void init_system()
 	//init sbus, using USART1
 	sbus_init();
 
-	//init motors to run with oneshot 125
+	//init motors to run with oneshot 125, small delay
+	HAL_Delay(1000);
 	pwmEnableAllMotors(Oneshot125);
 
-
 #ifdef USE_LEDS
 	//Initialize the on board leds
 	ledReavoEnable();

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 4/7] 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

From d72e7e099b829a38975dbcde72a69c6d04f96bc8 Mon Sep 17 00:00:00 2001
From: philsson <philsson@gmail.com>
Date: Mon, 17 Oct 2016 14:33:00 +0200
Subject: [PATCH 5/7] New mixer mode implemented with low scaling. Renaming of
 Airmode into Full scale Implemented new modes in cli Fixed bugg in motors.c
 cutting out max value

---
 UAV-ControlSystem/inc/config/eeprom.h         |  4 +-
 .../inc/drivers/failsafe_toggles.h            | 16 ++---
 UAV-ControlSystem/inc/drivers/motors.h        |  2 +
 UAV-ControlSystem/src/config/cli.c            | 38 +++++-----
 UAV-ControlSystem/src/config/eeprom.c         |  8 +--
 .../src/drivers/failsafe_toggles.c            |  8 +--
 UAV-ControlSystem/src/drivers/motormix.c      | 69 +++++++++++--------
 UAV-ControlSystem/src/drivers/motors.c        |  2 +-
 8 files changed, 81 insertions(+), 66 deletions(-)

diff --git a/UAV-ControlSystem/inc/config/eeprom.h b/UAV-ControlSystem/inc/config/eeprom.h
index 3b57398..06536a4 100644
--- a/UAV-ControlSystem/inc/config/eeprom.h
+++ b/UAV-ControlSystem/inc/config/eeprom.h
@@ -187,8 +187,8 @@ typedef enum {
 	EEPROM_FLAG_FLIGHTMODE_BAROMETER,
 	EEPROM_FLAG_FLIGHTMODE_COMPASS,
 	EEPROM_FLAG_FLIGHTMODE_GPS,
-	EEPROM_FLAG_AIRMODE,
-	EEPROM_FLAG_FLIGHTMODE_2,
+	EEPROM_FLAG_MIXERFULLSCALE,
+	EEPROM_FLAG_MIXERLOWSCALE,
 	EEPROM_FLAG_FLIGHTMODE_3,
 
 	/* Counts the amount of system settings */
diff --git a/UAV-ControlSystem/inc/drivers/failsafe_toggles.h b/UAV-ControlSystem/inc/drivers/failsafe_toggles.h
index 3cbaf91..382a578 100644
--- a/UAV-ControlSystem/inc/drivers/failsafe_toggles.h
+++ b/UAV-ControlSystem/inc/drivers/failsafe_toggles.h
@@ -27,8 +27,8 @@
 #define systemFlags_flightmode_barometer_id	 		2	+ boolean_vals_offset
 #define systemFlags_flightmode_compass_id	 		3	+ boolean_vals_offset
 #define systemFlags_flightmode_gps_id		 		4	+ boolean_vals_offset
-#define systemFlags_airmode_id						5	+ boolean_vals_offset
-#define systemFlags_flightMode_2_id					6	+ boolean_vals_offset
+#define systemFlags_mixerfullscale_id				5	+ boolean_vals_offset
+#define systemFlags_mixerlowscale_id				6	+ boolean_vals_offset
 #define systemFlags_flightMode_3_id					7	+ boolean_vals_offset
 
 
@@ -43,8 +43,8 @@
 #define systemFlags_flightmode_barometer_mask	 	getFlagMaskValue(systemFlags_flightmode_barometer_id)
 #define systemFlags_flightmode_compass_mask	 		getFlagMaskValue(systemFlags_flightmode_compass_id)
 #define systemFlags_flightmode_gps_mask		 		getFlagMaskValue(systemFlags_flightmode_gps_id)
-#define systemFlags_airmode_mask					getFlagMaskValue(systemFlags_airmode_id)
-#define systemFlags_flightMode_2_mask				getFlagMaskValue(systemFlags_flightMode_2_id)
+#define systemFlags_mixerfullscale_mask				getFlagMaskValue(systemFlags_mixerfullscale_id)
+#define systemFlags_mixerlowscale_mask				getFlagMaskValue(systemFlags_mixerlowscale_id)
 #define systemFlags_flightMode_3_mask				getFlagMaskValue(systemFlags_flightMode_3_id)
 
 
@@ -67,8 +67,8 @@ typedef union bitSetRegion
 		booleanValue_t barometer 			: 1;
 		booleanValue_t compass 				: 1;
 		booleanValue_t gps	 				: 1;
-		booleanValue_t airmode				: 1;
-		booleanValue_t flightMode_2			: 1;
+		booleanValue_t mixerfullscale		: 1;
+		booleanValue_t mixerlowscale		: 1;
 		booleanValue_t flightMode_3			: 1;
 	}bitField;
 	uint64_t intRepresentation;
@@ -90,8 +90,8 @@ typedef enum
 	FLAG_CONFIGURATION_FLIGHTMODE_BAROMETER,
 	FLAG_CONFIGURATION_FLIGHTMODE_COMPASS,
 	FLAG_CONFIGURATION_FLIGHTMODE_GPS,
-	FLAG_CONFIGURATION_AIRMODE,
-	FLAG_CONFIGURATION_FLIGHTMODE_2,
+	FLAG_CONFIGURATION_MIXERFULLSCALE,
+	FLAG_CONFIGURATION_MIXERLOWSCALE,
 	FLAG_CONFIGURATION_FLIGHTMODE_3,
 
 	//Counter
diff --git a/UAV-ControlSystem/inc/drivers/motors.h b/UAV-ControlSystem/inc/drivers/motors.h
index d7f32a1..de79654 100644
--- a/UAV-ControlSystem/inc/drivers/motors.h
+++ b/UAV-ControlSystem/inc/drivers/motors.h
@@ -8,6 +8,8 @@
 #ifndef DRIVERS_MOTORS_H_
 #define DRIVERS_MOTORS_H_
 
+#define MAX_PULSE 1950
+
 #include "stm32f4xx_revo.h"
 /* Struct of motor output protocol */
 typedef enum {
diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c
index a1ab3fc..dcf7d2f 100644
--- a/UAV-ControlSystem/src/config/cli.c
+++ b/UAV-ControlSystem/src/config/cli.c
@@ -217,13 +217,13 @@ typedef enum
 	COMMAND_ID_FLAG_FLIGHTMODE_GPS_MAXRANGE,
 	COMMAND_ID_FLAG_FLIGHTMODE_GPS_CHANNEL,
 
-	COMMAND_ID_FLAG_AIRMODE_MINRANGE,
-	COMMAND_ID_FLAG_AIRMODE_MAXRANGE,
-	COMMAND_ID_FLAG_AIRMODE_CHANNEL,
+	COMMAND_ID_FLAG_MIXERFULLSCALE_MINRANGE,
+	COMMAND_ID_FLAG_MIXERFULLSCALE_MAXRANGE,
+	COMMAND_ID_FLAG_MIXERFULLSCALE_CHANNEL,
 
-	COMMAND_ID_FLAG_FLIGHTMODE_2_MINRANGE,
-	COMMAND_ID_FLAG_FLIGHTMODE_2_MAXRANGE,
-	COMMAND_ID_FLAG_FLIGHTMODE_2_CHANNEL,
+	COMMAND_ID_FLAG_MIXERLOWSCALE_MINRANGE,
+	COMMAND_ID_FLAG_MIXERLOWSCALE_MAXRANGE,
+	COMMAND_ID_FLAG_MIXERLOWSCALE_CHANNEL,
 
 	COMMAND_ID_FLAG_FLIGHTMODE_3_MINRANGE,
 	COMMAND_ID_FLAG_FLIGHTMODE_3_MAXRANGE,
@@ -458,30 +458,30 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = {
 			"flag_flightmode_gps_channel", 	COMMAND_ID_FLAG_FLIGHTMODE_GPS_CHANNEL, EEPROM_FLAG_FLIGHTMODE_GPS, EEPROM_VALUE_TYPE_SYSTEM, 4, VAL_UINT_8, .valueRange = {0, 18}
 		},
 
-		[COMMAND_ID_FLAG_AIRMODE_MINRANGE] =
+		[COMMAND_ID_FLAG_MIXERFULLSCALE_MINRANGE] =
 		{
-			"flag_airmode_minrange", 	COMMAND_ID_FLAG_AIRMODE_MINRANGE, EEPROM_FLAG_AIRMODE, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_UINT_16, .valueRange = {0, 2500}
+			"flag_mixerfullscale_minrange", 	COMMAND_ID_FLAG_MIXERFULLSCALE_MINRANGE, EEPROM_FLAG_MIXERFULLSCALE, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_UINT_16, .valueRange = {0, 2500}
 		},
-		[COMMAND_ID_FLAG_AIRMODE_MAXRANGE] =
+		[COMMAND_ID_FLAG_MIXERFULLSCALE_MAXRANGE] =
 		{
-			"flag_airmode_maxrange", 	COMMAND_ID_FLAG_AIRMODE_MAXRANGE, EEPROM_FLAG_AIRMODE, EEPROM_VALUE_TYPE_SYSTEM, 2, VAL_UINT_16, .valueRange = {0, 2500}
+			"flag_mixerfullscale_maxrange", 	COMMAND_ID_FLAG_MIXERFULLSCALE_MAXRANGE, EEPROM_FLAG_MIXERFULLSCALE, EEPROM_VALUE_TYPE_SYSTEM, 2, VAL_UINT_16, .valueRange = {0, 2500}
 		},
-		[COMMAND_ID_FLAG_AIRMODE_CHANNEL] =
+		[COMMAND_ID_FLAG_MIXERFULLSCALE_CHANNEL] =
 		{
-			"flag_airmode_channel", 	COMMAND_ID_FLAG_AIRMODE_CHANNEL, EEPROM_FLAG_AIRMODE, EEPROM_VALUE_TYPE_SYSTEM, 4, VAL_UINT_8, .valueRange = {0, 18}
+			"flag_mixerfullscale_channel", 	COMMAND_ID_FLAG_MIXERFULLSCALE_CHANNEL, EEPROM_FLAG_MIXERFULLSCALE, EEPROM_VALUE_TYPE_SYSTEM, 4, VAL_UINT_8, .valueRange = {0, 18}
 		},
 
-		[COMMAND_ID_FLAG_FLIGHTMODE_2_MINRANGE] =
+		[COMMAND_ID_FLAG_MIXERLOWSCALE_MINRANGE] =
 		{
-			"flag_flightmode_2_minrange", 	COMMAND_ID_FLAG_FLIGHTMODE_2_MINRANGE, EEPROM_FLAG_FLIGHTMODE_2, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_UINT_16, .valueRange = {0, 2500}
+			"flag_mixerlowscale_minrange", 	COMMAND_ID_FLAG_MIXERLOWSCALE_MINRANGE, EEPROM_FLAG_MIXERLOWSCALE, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_UINT_16, .valueRange = {0, 2500}
 		},
-		[COMMAND_ID_FLAG_FLIGHTMODE_2_MAXRANGE] =
+		[COMMAND_ID_FLAG_MIXERLOWSCALE_MAXRANGE] =
 		{
-			"flag_flightmode_2_maxrange", 	COMMAND_ID_FLAG_FLIGHTMODE_2_MAXRANGE, EEPROM_FLAG_FLIGHTMODE_2, EEPROM_VALUE_TYPE_SYSTEM, 2, VAL_UINT_16, .valueRange = {0, 2500}
+			"flag_mixerlowscale_maxrange", 	COMMAND_ID_FLAG_MIXERLOWSCALE_MAXRANGE, EEPROM_FLAG_MIXERLOWSCALE, EEPROM_VALUE_TYPE_SYSTEM, 2, VAL_UINT_16, .valueRange = {0, 2500}
 		},
-		[COMMAND_ID_FLAG_FLIGHTMODE_2_CHANNEL] =
+		[COMMAND_ID_FLAG_MIXERLOWSCALE_CHANNEL] =
 		{
-			"flag_flightmode_2_channel", 	COMMAND_ID_FLAG_FLIGHTMODE_2_CHANNEL, EEPROM_FLAG_FLIGHTMODE_2, EEPROM_VALUE_TYPE_SYSTEM, 4, VAL_UINT_8, .valueRange = {0, 18}
+			"flag_mixerlowscale_channel", 	COMMAND_ID_FLAG_MIXERLOWSCALE_CHANNEL, EEPROM_FLAG_MIXERLOWSCALE, EEPROM_VALUE_TYPE_SYSTEM, 4, VAL_UINT_8, .valueRange = {0, 18}
 		},
 
 		[COMMAND_ID_FLAG_FLIGHTMODE_3_MINRANGE] =
@@ -498,8 +498,6 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = {
 		},
 
 
-
-
 };
 
 /***********************************************************************
diff --git a/UAV-ControlSystem/src/config/eeprom.c b/UAV-ControlSystem/src/config/eeprom.c
index 81a4425..45e73df 100644
--- a/UAV-ControlSystem/src/config/eeprom.c
+++ b/UAV-ControlSystem/src/config/eeprom.c
@@ -231,15 +231,15 @@ EEPROM_DATA_t eeprom_sys_Arr[EEPROM_SYS_COUNT] = {
 		.size = sizeof(flags_Configuration_t),
 		.dataPtr = &(flagConfigArr[FLAG_CONFIGURATION_FLIGHTMODE_GPS]),
 	},
-	[EEPROM_FLAG_AIRMODE] =
+	[EEPROM_FLAG_MIXERFULLSCALE] =
 	{
 		.size = sizeof(flags_Configuration_t),
-		.dataPtr = &(flagConfigArr[FLAG_CONFIGURATION_AIRMODE]),
+		.dataPtr = &(flagConfigArr[FLAG_CONFIGURATION_MIXERFULLSCALE]),
 	},
-	[EEPROM_FLAG_FLIGHTMODE_2] =
+	[EEPROM_FLAG_MIXERLOWSCALE] =
 	{
 		.size = sizeof(flags_Configuration_t),
-		.dataPtr = &(flagConfigArr[FLAG_CONFIGURATION_FLIGHTMODE_2]),
+		.dataPtr = &(flagConfigArr[FLAG_CONFIGURATION_MIXERLOWSCALE]),
 	},
 	[EEPROM_FLAG_FLIGHTMODE_3] =
 	{
diff --git a/UAV-ControlSystem/src/drivers/failsafe_toggles.c b/UAV-ControlSystem/src/drivers/failsafe_toggles.c
index d9056f5..ec7b139 100644
--- a/UAV-ControlSystem/src/drivers/failsafe_toggles.c
+++ b/UAV-ControlSystem/src/drivers/failsafe_toggles.c
@@ -42,17 +42,17 @@ flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = {
 			.channelNumber = 0,
 			.flagId = systemFlags_flightmode_gps_id,
 		},
-		[FLAG_CONFIGURATION_AIRMODE] = {
+		[FLAG_CONFIGURATION_MIXERFULLSCALE] = {
 			.minRange = 0,
 			.maxRange = 0,
 			.channelNumber = 0,
-			.flagId = systemFlags_airmode_id,
+			.flagId = systemFlags_mixerfullscale_id,
 		},
-		[FLAG_CONFIGURATION_FLIGHTMODE_2] = {
+		[FLAG_CONFIGURATION_MIXERLOWSCALE] = {
 			.minRange = 0,
 			.maxRange = 0,
 			.channelNumber = 0,
-			.flagId = systemFlags_flightMode_2_id,
+			.flagId = systemFlags_mixerlowscale_id,
 		},
 		[FLAG_CONFIGURATION_FLIGHTMODE_3] = {
 			.minRange = 0,
diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c
index b370f55..1eceb28 100644
--- a/UAV-ControlSystem/src/drivers/motormix.c
+++ b/UAV-ControlSystem/src/drivers/motormix.c
@@ -45,9 +45,9 @@ uint16_t motor_output[MOTOR_COUNT];
 // TODO: Implement in EEPROM
 mixerConfig_s mixerConfig = {
 	.minThrottle = 1050,
-	.maxThrottle = 1950,
+	.maxThrottle = MAX_PULSE - 100,
 	.minCommand = 1000,
-	.maxCommand = 2000,
+	.maxCommand = MAX_PULSE,
 	.minCheck = 1010,
 	.pid_at_min_throttle = true,
 	.motorstop = false,
@@ -104,7 +104,8 @@ void mix()
 	// Might be used for some debug if necessary
 	static bool motorLimitReached;
 
-	if (flags_IsSet_ID(systemFlags_airmode_id)) // TODO: replace with check for Airmode
+	/* Mixer Full Scale enabled */
+	if (flags_IsSet_ID(systemFlags_mixerfullscale_id))
 	{
 		for (int i = 0; i < MOTOR_COUNT; i++)
 		{
@@ -129,9 +130,9 @@ void mix()
 		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 */
-		// Check if we maxed out
+
+		/* Check if we maxed out */
 		if (RPY_MixRange > throttleRange)
 		{
 			motorLimitReached = true; // Yes, we maxed out
@@ -140,21 +141,13 @@ 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 - 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
 		{
@@ -184,15 +177,15 @@ void mix()
 
 		// Now we add desired throttle
 		for (int i = 0; i < MOTOR_COUNT; i++)
-		{
 			// 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);
-		}
 	}
-	else // Airmode not active
+	else // Mixer full scale NOT active
 	{
-		int16_t maxMotor = 0;
+		uint16_t throttleMid = (mixerConfig.minCommand + mixerConfig.maxThrottle) / 2;
+		/* This time we need to check against mid throttle */
+		RPY_Mix_Min = throttleMid;
+		RPY_Mix_Max = throttleMid;
 
 		for (int i = 0; i < MOTOR_COUNT; i++) // Without airmode this includes throttle
 		{
@@ -202,15 +195,37 @@ void mix()
 					PID_Out[ROLL] 	*	mixerUAV[i].roll 		+ 	\
 					PID_Out[YAW]	*	mixerUAV[i].yaw			*	((mixerConfig.yaw_reverse_direction) ? -1 : 1);
 
-			// Find the maximum motor output
-			if (RPY_Mix[i] > maxMotor) maxMotor = RPY_Mix[i];
+			// Find the minimum and maximum motor output
+			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];
 		}
 
-		int16_t maxThrottleDifference = 0;
-		if (maxMotor > mixerConfig.maxThrottle)
-			maxThrottleDifference = maxMotor - mixerConfig.maxThrottle;
 
-		// Approach at mixing
+		/* Mixer Low Scale - Scaling output around low throttle */
+		if (flags_IsSet_ID(systemFlags_mixerlowscale_id))
+		{
+			uint16_t RPY_Mix_Min_Overshoot = (RPY_Mix_Min < mixerConfig.minThrottle) ? mixerConfig.minThrottle - RPY_Mix_Min : 0;
+
+			if (RPY_Mix_Min_Overshoot > 0 ) //|| RPY_Mix_Max_Overshoot > 0)
+			{
+				/* How far away are we from throttle */
+				uint16_t RPY_Mix_Thr_Displacement = throttle - RPY_Mix_Min;
+
+				/* Scaling factor */
+				float mix_scale_reduction = ((float)RPY_Mix_Min_Overshoot / (float)RPY_Mix_Thr_Displacement);
+
+				/* Rescaling */
+				for (int i = 0; i < MOTOR_COUNT; i++)
+					RPY_Mix[i] = (((float)(RPY_Mix[i] - throttle)) * (1 - mix_scale_reduction)) + throttle;
+			}
+		}
+
+		/* Check how far over maxCommand we are (overflow) */
+		int16_t maxThrottleDifference = 0;
+		if (RPY_Mix_Max > mixerConfig.maxCommand)
+			maxThrottleDifference = RPY_Mix_Max - mixerConfig.maxCommand;
+
+		// Last flag checks and output reduction
 		for (int i = 0; i < MOTOR_COUNT; i++)
 		{
 			RPY_Mix[i] -= maxThrottleDifference; // We reduce the highest overflow on all motors
@@ -230,7 +245,7 @@ void mix()
 				// Though authority is very low with low throttle
 			}
 
-			// Constrain in within the valid motor outputs
+			/* Constrain in within the valid motor outputs */
 			motor_output[i] = constrain(RPY_Mix[i], mixerConfig.minCommand, mixerConfig.maxCommand);
 		}
 	}
diff --git a/UAV-ControlSystem/src/drivers/motors.c b/UAV-ControlSystem/src/drivers/motors.c
index 17902b6..b2a83d9 100644
--- a/UAV-ControlSystem/src/drivers/motors.c
+++ b/UAV-ControlSystem/src/drivers/motors.c
@@ -36,7 +36,7 @@ bool perfromMotorCalibration = false;
 **************************************************************************/
 uint16_t checkPulse(uint16_t pulse)
 {
-	return ((pulse/MotorPWMPeriode)*100 < 98)? pulse: MotorPWMPeriode*0.98;
+	return (pulse > MAX_PULSE) ? MAX_PULSE : pulse;
 }
 
 /**************************************************************************

From dfac730893ebcfa399bd449a2a93500ed72d086b Mon Sep 17 00:00:00 2001
From: Jonas Holmberg <holmis.1993@gmail.com>
Date: Tue, 18 Oct 2016 11:13:27 +0200
Subject: [PATCH 6/7] Added improvement to AUX command processing

Improved the speed of the processing of aux commands from the rc that
handles various bolean flags in the system. Also added some more
commenting to the failsafe and toggles fiels. Also updated the motor 7
and 8s information in the REVO.h
---
 .../inc/drivers/failsafe_toggles.h            | 54 +++++++-----
 UAV-ControlSystem/inc/stm32f4xx_revo.h        | 12 +--
 .../src/drivers/failsafe_toggles.c            | 83 ++++++++++++++-----
 UAV-ControlSystem/src/drivers/sbus.c          |  3 +
 UAV-ControlSystem/src/main.c                  |  2 +-
 UAV-ControlSystem/src/tasks_main.c            | 11 ++-
 6 files changed, 113 insertions(+), 52 deletions(-)

diff --git a/UAV-ControlSystem/inc/drivers/failsafe_toggles.h b/UAV-ControlSystem/inc/drivers/failsafe_toggles.h
index 382a578..4e43fe2 100644
--- a/UAV-ControlSystem/inc/drivers/failsafe_toggles.h
+++ b/UAV-ControlSystem/inc/drivers/failsafe_toggles.h
@@ -105,62 +105,76 @@ extern boolFlags_t systemFlags;
 extern flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT];
 
 /***********************************************************************
-* BRIEF:
-* INFORMATION:
+* BRIEF:	   Could be used to set start values for some values
+* INFORMATION: Possible to set the values for any of the failsafes
 ***********************************************************************/
 void initFlags();
 
 /***********************************************************************
-* BRIEF:
-* INFORMATION:
+* BRIEF:       Process RC channels that should operate on some flags
+* INFORMATION: Reads the value of the RC channels that and checks if
+* 			   any of the channels should handle some flag value in the
+* 			   system. If it should it will update its state.
 ***********************************************************************/
 void flags_ProcessRcChannel(int rcChannel_ID, int value);
 
 /***********************************************************************
-* BRIEF:
-* INFORMATION:
+* BRIEF:       Process RC channels that should operate on some flags
+* INFORMATION: Reads the value of the RC channels that and checks if
+* 			   any of the channels should handle some flag value in the
+* 			   system. If it should it will update its state.
+***********************************************************************/
+void flags_ProcessRcChannel_Improved(uint8_t minChannel, uint8_t maxChannel);
+
+/***********************************************************************
+* BRIEF:       Set flag to true(1)
+* INFORMATION: Given an id set that value to true
 ***********************************************************************/
 void flags_Set_ID(int id);
 
 /***********************************************************************
-* BRIEF:
-* INFORMATION:
+* BRIEF:       Set flag to false(0)
+* INFORMATION: Given an id set that value to false
 ***********************************************************************/
 void flags_Clear_ID(int id);
 
 /***********************************************************************
-* BRIEF:
-* INFORMATION:
+* BRIEF:       Toggle a flag
+* INFORMATION: Given an id changes the current value of a flag to its
+* 			   opposite.
 ***********************************************************************/
 void flags_Toggle_ID(int id);
 
 /***********************************************************************
-* BRIEF:
-* INFORMATION:
+* BRIEF:	   Checks if a flag is set
+* INFORMATION: Given an id value, check if that is set to true in the
+* 			   bitfield
 ***********************************************************************/
 bool flags_IsSet_ID(int id);
 
 /***********************************************************************
-* BRIEF:
-* INFORMATION:
+* BRIEF:       Set flag to true(1)
+* INFORMATION: Given a mask set that value to true
 ***********************************************************************/
 void flags_Set_MASK(int mask);
 
 /***********************************************************************
-* BRIEF:
-* INFORMATION:
+* BRIEF:       Set flag to false(0)
+* INFORMATION: Given a mask set that value to false
 ***********************************************************************/
 void flags_Clear_MASK(int mask);
 
 /***********************************************************************
-* BRIEF:
-* INFORMATION:
+* BRIEF:       Toggle a flag
+* INFORMATION: Given a mask changes the current value of a flag to its
+* 			   opposite.
 ***********************************************************************/
 void flags_Toggle_MASK(int mask);
 
 /***********************************************************************
-* BRIEF:
-* INFORMATION:
+* BRIEF:	   Checks if a flag is set
+* INFORMATION: Given a mask value, check if that is set to true in the
+* 			   bitfield
 ***********************************************************************/
 bool flags_IsSet_MASK(int mask);
 
diff --git a/UAV-ControlSystem/inc/stm32f4xx_revo.h b/UAV-ControlSystem/inc/stm32f4xx_revo.h
index afca423..1d71484 100644
--- a/UAV-ControlSystem/inc/stm32f4xx_revo.h
+++ b/UAV-ControlSystem/inc/stm32f4xx_revo.h
@@ -196,15 +196,15 @@
 #define MOTOR_6_CHANNEL				TIM_CHANNEL_1
 
 #define MOTOR_7						7
-#define MOTOR_7_PIN					GPIO_PIN_6
-#define MOTOR_7_PORT				GPIOC
-#define MOTOR_7_TIM					TIM8
+#define MOTOR_7_PIN					GPIO_PIN_14
+#define MOTOR_7_PORT				GPIOB
+#define MOTOR_7_TIM					TIM12
 #define MOTOR_7_CHANNEL				TIM_CHANNEL_1
 
 #define MOTOR_8						8
-#define MOTOR_8_PIN					GPIO_PIN_7
-#define MOTOR_8_PORT				GPIOC
-#define MOTOR_8_TIM					TIM8
+#define MOTOR_8_PIN					GPIO_PIN_15
+#define MOTOR_8_PORT				GPIOB
+#define MOTOR_8_TIM					TIM12
 #define MOTOR_8_CHANNEL				TIM_CHANNEL_2
 
 #define MOTOR_9						9
diff --git a/UAV-ControlSystem/src/drivers/failsafe_toggles.c b/UAV-ControlSystem/src/drivers/failsafe_toggles.c
index ec7b139..24bc083 100644
--- a/UAV-ControlSystem/src/drivers/failsafe_toggles.c
+++ b/UAV-ControlSystem/src/drivers/failsafe_toggles.c
@@ -6,6 +6,7 @@
  */
 
 #include "drivers/failsafe_toggles.h"
+#include "drivers/sbus.h"
 
 /* Stuct containing values for all the flags and failsafe booleans sin the system */
 boolFlags_t systemFlags = {{0}};
@@ -63,17 +64,19 @@ flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = {
 };
 
 /***********************************************************************
-* BRIEF:
-* INFORMATION:
+* BRIEF:	   Could be used to set start values for some values
+* INFORMATION: Possible to set the values for any of the failsafes
 ***********************************************************************/
 void initFlags()
 {
-	systemFlags.intRepresentation = 0;
+	//Could be used to set start values for failsafes.
 }
 
 /***********************************************************************
-* BRIEF:
-* INFORMATION:
+* BRIEF:       Process RC channels that should operate on some flags
+* INFORMATION: Reads the value of the RC channels that and checks if
+* 			   any of the channels should handle some flag value in the
+* 			   system. If it should it will update its state.
 ***********************************************************************/
 void flags_ProcessRcChannel(int rcChannel_ID, int value)
 {
@@ -94,8 +97,42 @@ void flags_ProcessRcChannel(int rcChannel_ID, int value)
 }
 
 /***********************************************************************
-* BRIEF:
-* INFORMATION:
+* BRIEF:       Process RC channels that should operate on some flags
+* INFORMATION: Reads the value of the RC channels that and checks if
+* 			   any of the channels should handle some flag value in the
+* 			   system. If it should it will update its state.
+***********************************************************************/
+void flags_ProcessRcChannel_Improved(uint8_t minChannel, uint8_t maxChannel)
+{
+	//Loop through all the active flag configuration values
+	for (int i = 0; i < FLAG_CONFIGURATION_COUNT; i++)
+	{
+		int currentChannelNumb = flagConfigArr[i].channelNumber;
+		//Check if the current Flag channel is within the set bounds
+		if(currentChannelNumb >= 5 && currentChannelNumb <= maxChannel)
+		{
+			//Get the value for the channel that the current flag is linked to
+			int value = getChannelValue(sbusChannelData, currentChannelNumb);
+
+			if(value >= flagConfigArr[i].minRange && value <= flagConfigArr[i].maxRange)
+			{
+				flags_Set_ID(flagConfigArr[i].flagId);
+			}
+			else
+			{
+				flags_Clear_ID(flagConfigArr[i].flagId);
+			}
+		}
+		else
+		{
+			continue;
+		}
+	}
+}
+
+/***********************************************************************
+* BRIEF:       Set flag to true(1)
+* INFORMATION: Given a mask set that value to true
 ***********************************************************************/
 void flags_Set_MASK(int mask)
 {
@@ -103,8 +140,8 @@ void flags_Set_MASK(int mask)
 }
 
 /***********************************************************************
-* BRIEF:
-* INFORMATION:
+* BRIEF:       Set flag to false(0)
+* INFORMATION: Given a mask set that value to false
 ***********************************************************************/
 void flags_Clear_MASK(int mask)
 {
@@ -112,8 +149,9 @@ void flags_Clear_MASK(int mask)
 }
 
 /***********************************************************************
-* BRIEF:
-* INFORMATION:
+* BRIEF:       Toggle a flag
+* INFORMATION: Given a mask changes the current value of a flag to its
+* 			   opposite.
 ***********************************************************************/
 void flags_Toggle_MASK(int mask)
 {
@@ -121,8 +159,9 @@ void flags_Toggle_MASK(int mask)
 }
 
 /***********************************************************************
-* BRIEF:
-* INFORMATION:
+* BRIEF:	   Checks if a flag is set
+* INFORMATION: Given a mask value, check if that is set to true in the
+* 			   bitfield
 ***********************************************************************/
 bool flags_IsSet_MASK(int mask)
 {
@@ -130,8 +169,8 @@ bool flags_IsSet_MASK(int mask)
 }
 
 /***********************************************************************
-* BRIEF:
-* INFORMATION:
+* BRIEF:       Set flag to true(1)
+* INFORMATION: Given an id set that value to true
 ***********************************************************************/
 void flags_Set_ID(int id)
 {
@@ -139,8 +178,8 @@ void flags_Set_ID(int id)
 }
 
 /***********************************************************************
-* BRIEF:
-* INFORMATION:
+* BRIEF:       Set flag to false(0)
+* INFORMATION: Given an id set that value to false
 ***********************************************************************/
 void flags_Clear_ID(int id)
 {
@@ -148,8 +187,9 @@ void flags_Clear_ID(int id)
 }
 
 /***********************************************************************
-* BRIEF:
-* INFORMATION:
+* BRIEF:       Toggle a flag
+* INFORMATION: Given an id changes the current value of a flag to its
+* 			   opposite.
 ***********************************************************************/
 void flags_Toggle_ID(int id)
 {
@@ -157,8 +197,9 @@ void flags_Toggle_ID(int id)
 }
 
 /***********************************************************************
-* BRIEF:
-* INFORMATION:
+* BRIEF:	   Checks if a flag is set
+* INFORMATION: Given an id value, check if that is set to true in the
+* 			   bitfield
 ***********************************************************************/
 bool flags_IsSet_ID(int id)
 {
diff --git a/UAV-ControlSystem/src/drivers/sbus.c b/UAV-ControlSystem/src/drivers/sbus.c
index 012e2c0..8df2742 100644
--- a/UAV-ControlSystem/src/drivers/sbus.c
+++ b/UAV-ControlSystem/src/drivers/sbus.c
@@ -298,6 +298,9 @@ int getChannelValue(sbusFrame_s frame, int id)
 			toReturn = frame.flag_DChannel_18;
 			break;
 #endif
+		default:
+			toReturn = 0;
+			break;
 	}
 	return toReturn;
 }
diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c
index 9d5d45d..73b8a41 100644
--- a/UAV-ControlSystem/src/main.c
+++ b/UAV-ControlSystem/src/main.c
@@ -50,7 +50,7 @@ void init_system()
 
 
 	//initialize the CLI NOTE: Cant use the same usart as anything else or there will be some big trouble
-	cliInit(USART3);
+	cliInit(USART6);
 
 	//init sbus, using USART1
 	sbus_init();
diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c
index 2e6b1cc..beffa23 100644
--- a/UAV-ControlSystem/src/tasks_main.c
+++ b/UAV-ControlSystem/src/tasks_main.c
@@ -72,10 +72,13 @@ void systemTaskRx(void)
 	sbusFrame_s frame = sbusChannelData;
 
 	/* Process channel data for switches and toggles on the controller, starts after "stick" channels */
-	for (int i = STICK_CHANNEL_COUNT; i < STICK_CHANNEL_COUNT + AUX_CHANNEL_COUNT; i++)	//ToDo: add define for the number of channels to process, /if not work change i to start at numb of stick channels
-	{
-		flags_ProcessRcChannel(i+1, getChannelValue(frame, i+1));
-	}
+//	for (int i = STICK_CHANNEL_COUNT; i < STICK_CHANNEL_COUNT + AUX_CHANNEL_COUNT; i++)	//ToDo: add define for the number of channels to process, /if not work change i to start at numb of stick channels
+//	{
+//		flags_ProcessRcChannel(i+1, getChannelValue(frame, i+1));
+//	}
+
+	/*Updated flag processRcChannel function, not yet tested. Should work as the entire loop above*/
+	flags_ProcessRcChannel_Improved(STICK_CHANNEL_COUNT+1, STICK_CHANNEL_COUNT + AUX_CHANNEL_COUNT);
 
 	//temporary send data from the RC directly form the RC
 	PID_Out[0] = frame.chan1 - 1500;

From 238b71b8bc70ed607cbcb9249e3fc9bd5f6b5ada Mon Sep 17 00:00:00 2001
From: Jonas Holmberg <holmis.1993@gmail.com>
Date: Tue, 18 Oct 2016 11:51:47 +0200
Subject: [PATCH 7/7] Added some more comments to Failsafe and CLI

General comments added for the failsafe and toggles. Also added
description in CLI.h on the necessary steps to add a new command to the
CLI and how to also add the value it should operate on to the EEPROM.
---
 UAV-ControlSystem/inc/config/cli.h            | 47 ++++++++++++++++++-
 .../inc/drivers/failsafe_toggles.h            | 28 ++++++++---
 .../src/drivers/failsafe_toggles.c            | 26 +++++++---
 3 files changed, 87 insertions(+), 14 deletions(-)

diff --git a/UAV-ControlSystem/inc/config/cli.h b/UAV-ControlSystem/inc/config/cli.h
index c686d3b..8492df7 100644
--- a/UAV-ControlSystem/inc/config/cli.h
+++ b/UAV-ControlSystem/inc/config/cli.h
@@ -55,5 +55,50 @@ bool cliHasMessage();
 bool cliShouldRun();
 
 
-
 #endif /* CONFIG_CLI_H_ */
+
+/* CHECKLIST - How to add new command in CLI                          */
+/* A: First add the value that should be changed to the EEPROM
+ *
+ * 	  1: Create a EEPROM enum id for the value in eeprom.h. Make sure
+ * 	     to place within the correct enum container system or profile.
+ *
+ * 	  2: In eeprom.c add two values to the correct array for the value
+ * 	     profile or system. THe two values should be the pointer to the
+ * 	     value to store in eeprom and the size of said value.
+ *
+ * 	  3: Now the value should be storable int the EEPROM.
+ *
+ * B: Create the actual command in the CLI.
+ *
+ * 	  1: First create an enum id for the CLI command in the command_Ids_t
+ * 	     contained in the cli.c.
+ *
+ * 	  2: When the id is created it represents an array value in the
+ * 	     commandTable, an array of cliCommandConfig_t.
+ *
+ * 	  3: Initialize the values of the cliCommandConfig_t for the newly
+ * 	     created command id. Provide in this order:
+ * 	     - The name of the command when typing it in CLI.
+ * 	     - The created Command id.
+ * 	     - The EEPROM id to the value the command shoud operate on.
+ * 	     - The type of EEPROM value syetem, profile, etc...
+ * 	     - The offset of the value at the given eeprom address, usable if
+ * 	       the value stored in the EEPROM is for example a struct and
+ * 	       the value to be accessed is for example 4 bytes from the start
+ * 	       of the struct. In that case this value would be set to 4.
+ * 	     - The type of the value the command should operate on. Uint8,
+ * 	       uint16, uint 32 etc...
+ * 	     - The min and maximum value that can be given using the command.
+ *
+ * C: When both these things have been done the command should have been
+ * 	  added to the CLI and it should operate on the correct value in the
+ * 	  system that is also stored in the EEPROM.
+ *
+ * D: NOTE, be careful when setting up the CLI command, any small mistake
+ *    in the command can cause strange errors that can be hard to track
+ *    down at a later stage. Therefore make sure that the command works
+ *    as intended and double check all the values for the command and see
+ *    that they are what they should be.
+ *  */
+
diff --git a/UAV-ControlSystem/inc/drivers/failsafe_toggles.h b/UAV-ControlSystem/inc/drivers/failsafe_toggles.h
index 4e43fe2..2c6e72f 100644
--- a/UAV-ControlSystem/inc/drivers/failsafe_toggles.h
+++ b/UAV-ControlSystem/inc/drivers/failsafe_toggles.h
@@ -1,9 +1,23 @@
-/*
- * failsafe_toggles.h
- *
- *  Created on: 10 okt. 2016
- *      Author: holmis
- */
+ /**********************************************************************
+ * NAME:        failsafe_toggles.h                                     *
+ * AUTHOR:      Jonas Holmberg                                         *
+ * PURPOSE:     Give the ability to create boolean flags and failsafe  *
+ * 				variables.                                	    	   *
+ * INFORMATION: Create any boolean variable that should be able to be  *
+ * 				used throughout the system. These can be boolean vales *
+ * 				for standard checks, failsafes etc. These can also be  *
+ * 				linked to a RC channel and value range wherein the     *
+ * 				value will be set to one.							   *
+ *	                                                                   *
+ * GLOBAL VARIABLES:                                                   *
+ * Variable		  Type		     Description                           *
+ * --------		  ----		     -----------                           *
+ * systemFlags	  boolFlags_t    Represents a bitfield of all the      *
+ * 					 			 boolean values in the system.	       *
+ * flagConfigArr  flags_Configuration_t Configuration values for any   *
+ * 									    flag that should be changeable *
+ * 									    from the RC controller.		   *
+ **********************************************************************/
 
 #ifndef DRIVERS_FAILSAFE_TOGGLES_H_
 #define DRIVERS_FAILSAFE_TOGGLES_H_
@@ -98,7 +112,7 @@ typedef enum
 	FLAG_CONFIGURATION_COUNT
 }flags_configuration_IDs_t;
 
-/* Stuct containing values for all the flags and failsafe booleans sin the system */
+/* Bitfield containing values for all the flags and failsafe booleans sin the system */
 extern boolFlags_t systemFlags;
 
 /* Array of flag configurations. These are values that can be set by RC. */
diff --git a/UAV-ControlSystem/src/drivers/failsafe_toggles.c b/UAV-ControlSystem/src/drivers/failsafe_toggles.c
index 24bc083..3101c34 100644
--- a/UAV-ControlSystem/src/drivers/failsafe_toggles.c
+++ b/UAV-ControlSystem/src/drivers/failsafe_toggles.c
@@ -1,9 +1,23 @@
-/*
- * failsafe_toggles.c
- *
- *  Created on: 10 okt. 2016
- *      Author: holmis
- */
+ /**********************************************************************
+ * NAME:        failsafe_toggles.c                                     *
+ * AUTHOR:      Jonas Holmberg                                         *
+ * PURPOSE:     Give the ability to create boolean flags and failsafe  *
+ * 				variables.                                	    	   *
+ * INFORMATION: Create any boolean variable that should be able to be  *
+ * 				used throughout the system. These can be boolean vales *
+ * 				for standard checks, failsafes etc. These can also be  *
+ * 				linked to a RC channel and value range wherein the     *
+ * 				value will be set to one.							   *
+ *	                                                                   *
+ * GLOBAL VARIABLES:                                                   *
+ * Variable		  Type		     Description                           *
+ * --------		  ----		     -----------                           *
+ * systemFlags	  boolFlags_t    Represents a bitfield of all the      *
+ * 					 			 boolean values in the system.	       *
+ * flagConfigArr  flags_Configuration_t Configuration values for any   *
+ * 									    flag that should be changeable *
+ * 									    from the RC controller.		   *
+ **********************************************************************/
 
 #include "drivers/failsafe_toggles.h"
 #include "drivers/sbus.h"