Added calibration for accelerometer

UAV-ControlSystem/inc/Flight/pid.h

@@ -18,6 +18,7 @@
 #include<stdio.h>
 #include<stdint.h>
 #include "Flight/filter.h"
+#include "drivers/accel_gyro.h"
 
 #define XYZ_AXIS_COUNT 3					/*The maximum number of DOF that belongings to the PID*/
 
@@ -60,6 +61,12 @@ typedef struct pidProfile_s {
 /*Array of all pid profiles of the system*/
 extern pidProfile_t PidProfile[PID_COUNT];
 
+/*  */
+extern float accRollFineTune;
+extern float accPitchFineTune;
+
+extern accel_t accelProfile;												/*Struct profile for input data from sensor*/
+
 /*Is set in motor mix and used in pidUAVcore and mix */
 bool motorLimitReached;
 

UAV-ControlSystem/inc/config/eeprom.h

@@ -191,6 +191,15 @@ typedef enum {
 	EEPROM_FLAG_MIXERLOWSCALE,
 	EEPROM_FLAG_FLIGHTMODE_3,
 
+	/* accel calibration values */
+	EEPROM_FLAG_ACCELTUNE_OFFSET_X,
+	EEPROM_FLAG_ACCELTUNE_OFFSET_Y,
+	EEPROM_FLAG_ACCELTUNE_OFFSET_Z,
+
+	/* accel calibration fine tune values */
+	EEPROM_FLAG_ACCELTUNE_FINE_ROLL,
+	EEPROM_FLAG_ACCELTUNE_FINE_PITCH,
+
 	/* Counts the amount of system settings */
 	EEPROM_SYS_COUNT
 } EEPROM_SYS_ID_t;

UAV-ControlSystem/inc/drivers/accel_gyro.h

@@ -27,7 +27,7 @@
 #ifndef DRIVERS_ACCEL_GYRO_H_
 #define DRIVERS_ACCEL_GYRO_H_
 
-#include <stdbool.h>
+//#include <stdbool.h>
 #include "stm32f4xx.h"
 #include "stm32f4xx_revo.h"
 

UAV-ControlSystem/inc/drivers/failsafe_toggles.h

@@ -47,6 +47,15 @@
 #define systemFlags_flightMode_3_id					7	+ boolean_vals_offset
 #define systemFlags_barometerIsCalibrated_id		8	+ boolean_vals_offset
 #define systemFlags_AcceleromterIsCalibrated_id		9	+ boolean_vals_offset
+/* Stick booleans */
+#define systemFlags_throttleMax_id					10	+ boolean_vals_offset
+#define systemFlags_stickLeft_id					11	+ boolean_vals_offset
+#define systemFlags_stickRight_id					12	+ boolean_vals_offset
+#define systemFlags_stickUp_id						13	+ boolean_vals_offset
+#define systemFlags_stickDown_id					14	+ boolean_vals_offset
+#define systemFlags_stickCenterH_id					15	+ boolean_vals_offset
+#define systemFlags_stickCenterV_id					16	+ boolean_vals_offset
+#define systemFlags_throttleLeft_id					17	+ boolean_vals_offset
 
 
 /*Mask values for each of the flag values*/
@@ -66,6 +75,15 @@
 #define systemFlags_flightMode_3_mask					getFlagMaskValue(systemFlags_flightMode_3_id)
 #define systemFlags_barometerIsCalibrated_mask			getFlagMaskValue(systemFlags_barometerIsCalibrated_id)
 #define systemFlags_AcceleromterIsCalibrated_mask		getFlagMaskValue(systemFlags_AcceleromterIsCalibrated_id)
+/* Stick booleans */
+#define systemFlags_throttleMax_mask					getFlagMaskValue(systemFlags_throttleMax_id)
+#define systemFlags_stickLeft_mask						getFlagMaskValue(systemFlags_stickLeft_id)
+#define systemFlags_stickRight_mask						getFlagMaskValue(systemFlags_stickRight_id
+#define systemFlags_stickUp_mask						getFlagMaskValue(systemFlags_stickUp_id)
+#define systemFlags_stickDown_mask						getFlagMaskValue(systemFlags_stickDown_id)
+#define systemFlags_stickCenterH_mask					getFlagMaskValue(systemFlags_stickCenterH_id)
+#define systemFlags_stickCenterV_mask					getFlagMaskValue(systemFlags_stickCenterV_id)
+#define systemFlags_throttleLeft_mask					getFlagMaskValue(systemFlags_throttleLeft_id)
 
 
 
@@ -92,6 +110,16 @@ typedef union bitSetRegion
 		booleanValue_t flightMode_3				: 1;
 		booleanValue_t barometerIsCalibrated	: 1;
 		booleanValue_t AcceleromterIsCalibrated : 1;
+		/* Stick booleans */
+		booleanValue_t throttleMax				: 1;
+		booleanValue_t stickLeft				: 1;
+		booleanValue_t stickRight				: 1;
+		booleanValue_t stickUp					: 1;
+		booleanValue_t stickDown				: 1;
+		booleanValue_t stickCenterH				: 1;
+		booleanValue_t stickCenterV				: 1;
+		booleanValue_t throttleLeft				: 1;
+
 	}bitField;
 	uint64_t intRepresentation;
 }boolFlags_t;
@@ -115,6 +143,16 @@ typedef enum
 	FLAG_CONFIGURATION_MIXERFULLSCALE,
 	FLAG_CONFIGURATION_MIXERLOWSCALE,
 	FLAG_CONFIGURATION_FLIGHTMODE_3,
+	/* Stick booleans */
+	FLAG_CONFIGURATION_THROTTLEMAX,
+	FLAG_CONFIGURATION_STICKLEFT,
+	FLAG_CONFIGURATION_STICKRIGHT,
+	FLAG_CONFIGURATION_STICKUP,
+	FLAG_CONFIGURATION_STICKDOWN,
+	FLAG_CONFIGURATION_STICKCENTERH,
+	FLAG_CONFIGURATION_STICKCENTERV,
+	FLAG_CONFIGURATION_THROTTLELEFT,
+
 
 	//Counter
 	FLAG_CONFIGURATION_COUNT

UAV-ControlSystem/inc/system_variables.h

@@ -14,7 +14,7 @@
 #ifndef SYSTEM_VARIABLES_H_
 #define SYSTEM_VARIABLES_H_
 
-#define EEPROM_SYS_VERSION 102
+#define EEPROM_SYS_VERSION 107
 
 #define ADC_STATE
 #include "stm32f4xx.h"

UAV-ControlSystem/src/Flight/pid.c

@@ -14,7 +14,6 @@
 * **************************************************************************/
 
 #include "Flight/pid.h"
-#include "drivers/accel_gyro.h"
 #include "drivers/sbus.h"
 #include "scheduler/scheduler.h"
 #include <math.h>
@@ -53,11 +52,14 @@ typedef struct pidProfileBuff_s {
 pidProfile_t PidProfile[PID_COUNT] = {0};							/*Array of all pid profiles of the system*/
 pidProfileBuff_t PidProfileBuff[PID_COUNT] = {0};
 
-accel_t accelProfile;												/*Struct profile for input data from sensor*/
+accel_t accelProfile = {0};												/*Struct profile for input data from sensor*/
-gyro_t gyroProfile;													/*Struct profile for input data from sensor*/
+gyro_t gyroProfile = {0};													/*Struct profile for input data from sensor*/
 
 pt1Filter_t accelFilter[2] = {0};
 
+float accRollFineTune = 0;
+float accPitchFineTune = 0;
+
 /**************************************************************************
 * BRIEF: Calculates angle from accelerometer   		                      *
 * INFORMATION:                                                            *
@@ -151,6 +153,11 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
 		float X_roll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
 		float X_pitch = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
 
+		/*TODO add finetune for roll and pitch*/
+		X_roll += accRollFineTune;
+		X_pitch += accPitchFineTune;
+
+
 		oldSensorValueRoll[i] = X_roll;
 		oldSensorValuePitch[i] = X_pitch;
 
@@ -499,14 +506,20 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
 	PidProfile[ID].PID_Out[YAW] = 0;
 
 
-	PidProfile[ID].P[ROLL] = 10;
+
-	PidProfile[ID].P[PITCH] = 10;
-	PidProfile[ID].P[YAW] = 10;
 
 	switch (ID)
 	{
 	case PID_ID_GYRO:
 
+		PidProfile[ID].P[ROLL] = 150;
+		PidProfile[ID].P[PITCH] = 135;
+		PidProfile[ID].P[YAW] = 150;
+
+		PidProfile[ID].D[ROLL] = 75;
+		PidProfile[ID].D[PITCH] = 95;
+		PidProfile[ID].D[YAW] = 50;
+
 		PidProfile[ID].PIDweight[ROLL] = 100;
 		PidProfile[ID].PIDweight[PITCH] = 100;
 		PidProfile[ID].PIDweight[YAW] = 100;
@@ -518,6 +531,14 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
 		break;
 	case PID_ID_ACCELEROMETER:
 
+		PidProfile[ID].P[ROLL] = 90;
+		PidProfile[ID].P[PITCH] = 90;
+		PidProfile[ID].P[YAW] = 0;
+
+		PidProfile[ID].D[ROLL] = 40;
+		PidProfile[ID].D[PITCH] = 40;
+		PidProfile[ID].D[YAW] = 0;
+
 		PidProfile[ID].PIDweight[ROLL] = 2;
 		PidProfile[ID].PIDweight[PITCH] = 2;
 		PidProfile[ID].PIDweight[YAW] = 100;
@@ -529,6 +550,10 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
 		break;
 	case PID_ID_COMPASS:
 
+		PidProfile[ID].P[ROLL] = 10;
+		PidProfile[ID].P[PITCH] = 10;
+		PidProfile[ID].P[YAW] = 10;
+
 		PidProfile[ID].PIDweight[ROLL] = 100;
 		PidProfile[ID].PIDweight[PITCH] = 100;
 		PidProfile[ID].PIDweight[YAW] = 100;
@@ -538,6 +563,10 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
 		break;
 	case PID_ID_BAROMETER:
 
+		PidProfile[ID].P[ROLL] = 10;
+		PidProfile[ID].P[PITCH] = 10;
+		PidProfile[ID].P[YAW] = 10;
+
 		PidProfile[ID].PIDweight[ROLL] = 100;
 		PidProfile[ID].PIDweight[PITCH] = 100;
 		PidProfile[ID].PIDweight[YAW] = 100;

UAV-ControlSystem/src/Scheduler/tasks.c

@@ -88,7 +88,7 @@ task_t SystemTasks[TASK_COUNT] =
 		{
 			.taskName = "SERIAL",
 			.taskFunction = systemTaskSerial,
-			.desiredPeriod = GetUpdateRateHz(100),			//100 hz update rate (10 ms)
+			.desiredPeriod = GetUpdateRateHz(2),			//100 hz update rate (10 ms)
 			.staticPriority = PRIORITY_LOW,
 		},
 

UAV-ControlSystem/src/config/cli.c

@@ -24,6 +24,8 @@
 #include "utilities.h"
 #include "Scheduler/scheduler.h"
 #include "drivers/motors.h"
+#include "drivers/accel_gyro.h"
+#include "Flight/pid.h"
 
 #define cliActivateCharacter	35
 #define commandValueError		0xFFFFFFFFFFFFFFFF
@@ -76,6 +78,7 @@ typedef enum
 	ACTION_GYROCALIBRATE,
 	ACTION_ACCELEROMTETERCALIBRATE,
 	ACTION_COMPASSCALIBRATE,
+	ACTION_CALIBRATIONINFOACCEL,
 
 	//The number of actions
 	ACTION_COUNT,					//Count of the number of actions
@@ -100,7 +103,8 @@ const typeString commandAction_Strings[ACTION_COUNT] = {
 	"calibrate_motors",
 	"calibrate_gyro",
 	"calibrate_accelerometer",
-	"calibrate_compass"
+	"calibrate_compass",
+	"calibration_info_accelerometer"
 };
 
 /* String values descrbing information of a certain action */
@@ -120,7 +124,8 @@ const typeString commandActionInformation_Strings[ACTION_COUNT] = {
 	"| calibrate_motors - Calibrates all motors.",
 	"| calibrate_gyro - Calibrates the gyro.",
 	"| calibrate_accelerometer - Calibrates the accelerometer.",
-	"| calibrate_compass - Calibrates the compass."
+	"| calibrate_compass - Calibrates the compass.",
+	"| calibration_info_accelerometer - Provides info on the accelerometer calibration."
 };
 
 /* String array containing all the signature examples for each action */
@@ -140,7 +145,8 @@ const typeString commandActionSignature_Strings[ACTION_COUNT] = {
 	" calibrate_motors",
 	" calibrate_gyro",
 	" calibrate_accelerometer",
-	" calibrate_compass"
+	" calibrate_compass",
+	" calibration_info_accelerometer"
 };
 
 /* Size values for the values a command will require */
@@ -1477,7 +1483,6 @@ void cliRun()
 					if (ReceiveBinaryDecision(121,110))
 					{
 						TransmitBack("Starting calibration! \n\n\r");
-						TransmitBack("NOT IMPLEMENTED! \n\n\r");
 						TransmitBack("Calibration complete! \n\n\r");
 					}
 					else
@@ -1500,6 +1505,15 @@ void cliRun()
 					}
 
 					break;
+			case commandMask(commandSize_1, ACTION_CALIBRATIONINFOACCEL):
+					{
+					char tempBuffer[100];
+					TransmitBack("- Accelerometer calibration information: \n\n\r");
+					TransmitBack(tempBuffer);
+					sprintf(tempBuffer, "- BaseTune: \n\r-OffsetX: %d \n\r-OffsetY: %d \n\r-OffsetZ: %d \n\n\r", accelProfile.offsetX, accelProfile.offsetY, accelProfile.offsetZ);
+
+					break;
+					}
 			default:
 				if (actionId != ACTION_NOACTION)
 					TransmitCommandInstruction(actionId);

UAV-ControlSystem/src/config/eeprom.c

@@ -248,9 +248,34 @@ EEPROM_DATA_t eeprom_sys_Arr[EEPROM_SYS_COUNT] = {
 		.dataPtr = &(flagConfigArr[FLAG_CONFIGURATION_FLIGHTMODE_3]),
 	},
 
+	/* accel calibration values */
+	[EEPROM_FLAG_ACCELTUNE_OFFSET_X] =
+	{
+		.size = sizeof(int16_t),
+		.dataPtr = &(accelProfile.offsetX),
+	},
+	[EEPROM_FLAG_ACCELTUNE_OFFSET_Y] =
+	{
+		.size = sizeof(int16_t),
+		.dataPtr = &(accelProfile.offsetY),
+	},
+	[EEPROM_FLAG_ACCELTUNE_OFFSET_Z] =
+	{
+		.size = sizeof(int16_t),
+		.dataPtr = &(accelProfile.offsetZ),
+	},
 
-
+	/* accel fine tune */
-
+	[EEPROM_FLAG_ACCELTUNE_FINE_ROLL] =
+	{
+		.size = sizeof(float),
+		.dataPtr = &(accRollFineTune),
+	},
+	[EEPROM_FLAG_ACCELTUNE_FINE_PITCH] =
+	{
+		.size = sizeof(float),
+		.dataPtr = &(accPitchFineTune),
+	},
 };
 
 /* Data pointers and sizes for profile content */

UAV-ControlSystem/src/drivers/accel_gyro.c

@@ -239,7 +239,7 @@ bool mpu6000_init(gyro_t* gyro, accel_t* accel)
 		return false;
 
 	mpu6000_read_gyro_offset(gyro);
-	mpu6000_read_acc_offset(accel);
+	//mpu6000_read_acc_offset(accel);
 
 	HAL_Delay(60);
 

UAV-ControlSystem/src/drivers/failsafe_toggles.c

@@ -29,14 +29,14 @@ boolFlags_t systemFlags = {{0}};
 flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = {
 		[FLAG_CONFIGURATION_ARM] = {
 			.minRange = 1500,
-			.maxRange = 2000,
+			.maxRange = 2100,
 			.channelNumber = 8,
 			.flagId = systemFlags_armed_id,
 		},
 		[FLAG_CONFIGURATION_FLIGHTMODE_ACCELEROMETER] = {
-			.minRange = 1600,
+			.minRange = 1400,
-			.maxRange = 2000,
+			.maxRange = 2100,
-			.channelNumber = 5,
+			.channelNumber = 6,
 			.flagId = systemFlags_flightmode_acceleromter_id,
 		},
 		[FLAG_CONFIGURATION_FLIGHTMODE_BAROMETER] = {
@@ -58,15 +58,15 @@ flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = {
 			.flagId = systemFlags_flightmode_gps_id,
 		},
 		[FLAG_CONFIGURATION_MIXERFULLSCALE] = {
-			.minRange = 0,
+			.minRange = 1900,
-			.maxRange = 0,
+			.maxRange = 2100,
-			.channelNumber = 0,
+			.channelNumber = 5,
 			.flagId = systemFlags_mixerfullscale_id,
 		},
 		[FLAG_CONFIGURATION_MIXERLOWSCALE] = {
-			.minRange = 0,
+			.minRange = 900,
-			.maxRange = 0,
+			.maxRange = 1100,
-			.channelNumber = 0,
+			.channelNumber = 5,
 			.flagId = systemFlags_mixerlowscale_id,
 		},
 		[FLAG_CONFIGURATION_FLIGHTMODE_3] = {
@@ -74,7 +74,59 @@ flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = {
 			.maxRange = 0,
 			.channelNumber = 0,
 			.flagId = systemFlags_flightMode_3_id,
+		},
+
+		/*Stick controls*/
+		[FLAG_CONFIGURATION_THROTTLEMAX] = {
+			.minRange = 1950,
+			.maxRange = 2000,
+			.channelNumber = 3,
+			.flagId = systemFlags_throttleMax_id,
+		},
+		[FLAG_CONFIGURATION_STICKLEFT] = { //negative
+			.minRange = 1000,
+			.maxRange = 1100,
+			.channelNumber = 1,
+			.flagId = systemFlags_stickLeft_id,
+		},
+		[FLAG_CONFIGURATION_STICKRIGHT] = {	//positive
+			.minRange = 1900,
+			.maxRange = 2000,
+			.channelNumber = 1,
+			.flagId = systemFlags_stickRight_id,
+		},
+		[FLAG_CONFIGURATION_STICKUP] = { //negative
+			.minRange = 1000,
+			.maxRange = 1100,
+			.channelNumber = 2,
+			.flagId = systemFlags_stickUp_id,
+		},
+		[FLAG_CONFIGURATION_STICKDOWN] = { //positive
+			.minRange = 1900,
+			.maxRange = 2000,
+			.channelNumber = 2,
+			.flagId = systemFlags_stickDown_id,
+		},
+		[FLAG_CONFIGURATION_STICKCENTERH] = {
+			.minRange = 1400,
+			.maxRange = 1600,
+			.channelNumber = 1,
+			.flagId = systemFlags_stickCenterH_id,
+		},
+		[FLAG_CONFIGURATION_STICKCENTERV] = {
+			.minRange = 1400,
+			.maxRange = 1600,
+			.channelNumber = 2,
+			.flagId = systemFlags_stickCenterV_id,
+		},
+		[FLAG_CONFIGURATION_THROTTLELEFT] = {
+			.minRange = 2000,
+			.maxRange = 1900,
+			.channelNumber = 4,
+			.flagId = systemFlags_throttleLeft_id,
 		}
+
+
 };
 
 /***********************************************************************
@@ -123,7 +175,7 @@ void flags_ProcessRcChannel_Improved(uint8_t minChannel, uint8_t maxChannel)
 	{
 		int currentChannelNumb = flagConfigArr[i].channelNumber;
 		//Check if the current Flag channel is within the set bounds
-		if(currentChannelNumb >= 5 && currentChannelNumb <= maxChannel)
+		if(currentChannelNumb >= minChannel && currentChannelNumb <= maxChannel)
 		{
 			//Get the value for the channel that the current flag is linked to
 			int value = getChannelValue(sbusChannelData, currentChannelNumb);

UAV-ControlSystem/src/drivers/motormix.c

@@ -45,14 +45,14 @@ uint16_t motor_output[MOTOR_COUNT];
 /* Default values for the mixerConfig */
 // TODO: Implement in EEPROM
 mixerConfig_s mixerConfig = {
-	.minThrottle = 1050,
+	.minThrottle = 1040,
 	.maxThrottle = MAX_PULSE - 100,
 	.minCommand = 990,
 	.maxCommand = MAX_PULSE,
 	.minCheck = 1010,
 	.pid_at_min_throttle = true,
 	.motorstop = false,
-	.yaw_reverse_direction = false
+	.yaw_reverse_direction = true
 };
 
 /* Used in "mixerUAV" to create the dynamic model of the UAV */

UAV-ControlSystem/src/main.c

@@ -61,9 +61,7 @@ void init_system()
 	//init sbus, using USART1
 	sbus_init();
 
-	//init motors to run with oneshot 125, small delay
+
-	HAL_Delay(1000);
-	pwmEnableAllMotors(Oneshot125);
 
 
 #ifdef USE_LEDS
@@ -99,6 +97,10 @@ void init_system()
 
 #endif
 
+	//init motors to run with oneshot 125, small delay
+	HAL_Delay(1000);
+	pwmEnableAllMotors(Oneshot125);
+
 }
 
 /**************************************************************************
@@ -114,6 +116,9 @@ int main(void)
 	//Init the system
 	init_system();
 
+	//Light the yellow led
+	ledOnInverted(Led1, Led1_GPIO_PORT);
+
 	//Initialize the scheduler, add all the tasks that should run to the ready queue of the scheduler
 	initScheduler();
 

UAV-ControlSystem/src/tasks_main.c

@@ -92,7 +92,10 @@ void systemTaskRx(void)
 //	}
 
 	/*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);
+	//flags_ProcessRcChannel_Improved(STICK_CHANNEL_COUNT+1, STICK_CHANNEL_COUNT + AUX_CHANNEL_COUNT);
+
+	/* Includes the stick channel in the toggles checks */
+	flags_ProcessRcChannel_Improved(1, STICK_CHANNEL_COUNT + AUX_CHANNEL_COUNT);
 
 	//temporary send data from the RC directly form the RC
 //	RawRcCommand.Roll = frame.chan1;
@@ -162,24 +165,63 @@ bool systemTaskRxCliCheck(uint32_t currentDeltaTime)
 
 void systemTaskSerial(void)
 {
-//	uint8_t c = 118;
+	static bool readyToCalibrate = true;
-//	usart_transmit(&cliUsart, &c, 1, 1000000000);
+	const float calibrationAmount = 0.5;
-	if (flags_IsSet_ID(systemFlags_armed_id))
+
-		ledOnInverted(Led0_PIN, Led0_GPIO_PORT);
+	//Only run if the system is not armed
-	else
+	if (!flags_IsSet_ID(systemFlags_armed_id))
-		ledOffInverted(Led0_PIN, Led0_GPIO_PORT);
+	{
+		if (flags_IsSet_ID(systemFlags_throttleMax_id))
+		{
+//			if(flags_IsSet_ID(systemFlags_throttleLeft_id))
+//			{
+//				if(flags_IsSet_ID(systemFlags_stickDown_id))
+//				{
+//					mpu6000_read_acc_offset(&accelProfile);
+//				}
+//			}
+			if(readyToCalibrate)
+			{
+				if (flags_IsSet_ID(systemFlags_stickLeft_id))
+				{
+					accRollFineTune -= calibrationAmount;
+				}
+				else if (flags_IsSet_ID(systemFlags_stickRight_id))
+				{
+					accRollFineTune += calibrationAmount;
+				}
+				else if (flags_IsSet_ID(systemFlags_stickUp_id))
+				{
+					accPitchFineTune -= calibrationAmount;
+				}
+				else if (flags_IsSet_ID(systemFlags_stickDown_id))
+				{
+					accPitchFineTune += calibrationAmount;
+				}
+
+			}
+			else
+			{
+				//if the stick is centered set ready to calibrate to true
+				if(flags_IsSet_MASK(systemFlags_stickCenterH_mask | systemFlags_stickCenterV_mask))
+				{
+					readyToCalibrate = true;
+				}
+			}
+
+		}
+	}
 
 }
 
 void systemTaskBattery(void)
 {
-	//Keep track of the battery level of the system
+	//	uint8_t c = 118;
-//	uint8_t c = 98;
+	//	usart_transmit(&cliUsart, &c, 1, 1000000000);
-//	usart_transmit(&cliUsart, &c, 1, 1000000000);
+	if (flags_IsSet_ID(systemFlags_armed_id))
-	if (flags_IsSet_MASK((systemFlags_flightmode_acceleromter_mask | systemFlags_armed_mask)))
+		ledOnInverted(Led0_PIN, Led0_GPIO_PORT);
-		ledOnInverted(Led1, Led1_GPIO_PORT);
 	else
-		ledOffInverted(Led1, Led1_GPIO_PORT);
+		ledOffInverted(Led0_PIN, Led0_GPIO_PORT);
 }
 
 void systemTaskBaro(void)