diff --git a/UAV-ControlSystem/inc/Flight/pid.h b/UAV-ControlSystem/inc/Flight/pid.h
index dd9f80f..7213f7d 100644
--- a/UAV-ControlSystem/inc/Flight/pid.h
+++ b/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,8 +61,13 @@ typedef struct pidProfile_s {
 /*Array of all pid profiles of the system*/
 extern pidProfile_t PidProfile[PID_COUNT];
 
-/*Is set in motor mix and used in pidUAVcore and mix */
-bool motorLimitReached;
+/*  */
+extern float accRollFineTune;
+extern float accPitchFineTune;
+
+extern accel_t accelProfile;												/*Struct profile for input data from sensor*/
+
+
 
 /**************************************************************************
 * BRIEF: Initializes PID profiles			 					          *
@@ -75,4 +81,6 @@ void pidInit();
 **************************************************************************/
 void pidRun(uint8_t ID);
 
+void pidEproom(void);
+
 #endif /* FLIGHT_PID_H_ */
diff --git a/UAV-ControlSystem/inc/config/eeprom.h b/UAV-ControlSystem/inc/config/eeprom.h
index 282761d..1774a35 100644
--- a/UAV-ControlSystem/inc/config/eeprom.h
+++ b/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;
diff --git a/UAV-ControlSystem/inc/drivers/I2C.h b/UAV-ControlSystem/inc/drivers/I2C.h
index 44fea89..0cd1530 100644
--- a/UAV-ControlSystem/inc/drivers/I2C.h
+++ b/UAV-ControlSystem/inc/drivers/I2C.h
@@ -54,6 +54,18 @@ bool i2c_send(I2C_HandleTypeDef* profile,
 			  uint32_t length);
 
 
+
+/******************************************************************************
+* BRIEF: Configure the I2C bus to be used                                     *
+* INFORMATION: This function only implements I2C1 or I2C2 DMA which are       *
+* available on the REVO board												  *
+******************************************************************************/
+bool i2c_configure_DMA(I2C_TypeDef *i2c,
+		   I2C_HandleTypeDef *out_profile,
+		   DMA_HandleTypeDef *out_rxDMA_profile,
+		   DMA_HandleTypeDef *out_txDMA_profile,
+		   uint32_t my_address);
+
 #endif /* DRIVERS_I2C_H_ */
 
 
diff --git a/UAV-ControlSystem/inc/drivers/accel_gyro.h b/UAV-ControlSystem/inc/drivers/accel_gyro.h
index fd42421..805964d 100644
--- a/UAV-ControlSystem/inc/drivers/accel_gyro.h
+++ b/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"
 
diff --git a/UAV-ControlSystem/inc/drivers/barometer.h b/UAV-ControlSystem/inc/drivers/barometer.h
new file mode 100644
index 0000000..2c47476
--- /dev/null
+++ b/UAV-ControlSystem/inc/drivers/barometer.h
@@ -0,0 +1,26 @@
+/*
+ * barometer.h
+ *
+ *  Created on: 18 okt. 2016
+ *      Author: holmis
+ */
+
+#ifndef DRIVERS_BAROMETER_H_
+#define DRIVERS_BAROMETER_H_
+
+
+bool barometer_init();
+
+bool barometer_reset();
+
+void barometer_CaclulateValues();
+
+double barometer_GetCurrentPreassure();
+
+double barometer_GetCurrentTemperature();
+
+float barometer_GetCurrentAltitudeBasedOnSeaLevel();
+
+
+
+#endif /* DRIVERS_BAROMETER_H_ */
diff --git a/UAV-ControlSystem/inc/drivers/failsafe_toggles.h b/UAV-ControlSystem/inc/drivers/failsafe_toggles.h
index aa2fe29..8e1044f 100644
--- a/UAV-ControlSystem/inc/drivers/failsafe_toggles.h
+++ b/UAV-ControlSystem/inc/drivers/failsafe_toggles.h
@@ -28,7 +28,7 @@
 #define getFlagMaskValue(x)							(1 << x)
 
 #define failSafe_vals_offset 						0		//offset for the fail safe values in the bitfield
-#define boolean_vals_offset							3		//offset for the booleans values in the bitfield
+#define boolean_vals_offset							3		//offset for the booleans values in the bitfield. Equals the amount of failsafe ids
 
 /*If a new value is added to the bitfield these IDs must be reviewed and checkd so that they still are correct*/
 //failsafe values
@@ -45,6 +45,17 @@
 #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
+#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*/
@@ -62,6 +73,17 @@
 #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)
+#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)
 
 
 
@@ -74,18 +96,30 @@ typedef union bitSetRegion
 	struct
 	{
 		//fail-safe booleans
-		booleanValue_t rcChannelInRange 	: 1;
-		booleanValue_t noRcReceived			: 1;
+		booleanValue_t rcChannelInRange 		: 1;
+		booleanValue_t noRcReceived				: 1;
 
 		//Flag boleans that are not fail-safe
-		booleanValue_t armed 				: 1;
-		booleanValue_t acceleromter 		: 1;
-		booleanValue_t barometer 			: 1;
-		booleanValue_t compass 				: 1;
-		booleanValue_t gps	 				: 1;
-		booleanValue_t mixerfullscale		: 1;
-		booleanValue_t mixerlowscale		: 1;
-		booleanValue_t flightMode_3			: 1;
+		booleanValue_t armed 					: 1;
+		booleanValue_t acceleromter 			: 1;
+		booleanValue_t barometer 				: 1;
+		booleanValue_t compass 					: 1;
+		booleanValue_t gps	 					: 1;
+		booleanValue_t mixerfullscale			: 1;
+		booleanValue_t mixerlowscale			: 1;
+		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;
@@ -109,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
diff --git a/UAV-ControlSystem/inc/drivers/i2c_soft.h b/UAV-ControlSystem/inc/drivers/i2c_soft.h
new file mode 100644
index 0000000..7d4dcd3
--- /dev/null
+++ b/UAV-ControlSystem/inc/drivers/i2c_soft.h
@@ -0,0 +1,28 @@
+/*
+ * i2c_soft.h
+ *
+ *  Created on: 27 okt. 2016
+ *      Author: holmis
+ */
+
+#ifndef DRIVERS_I2C_SOFT_H_
+#define DRIVERS_I2C_SOFT_H_
+
+#include "stm32f4xx.h"
+
+typedef struct
+{
+	GPIO_TypeDef * i2c_Port;
+	uint16_t i2c_scl_pin;
+	uint16_t i2c_sda_pin;
+}I2C_SOFT_handle_t;
+
+
+void i2c_soft_Init(I2C_TypeDef *i2c, I2C_SOFT_handle_t *out_profile);
+
+bool i2c_soft_Write(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t data);
+
+bool i2c_soft_Read(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf);
+
+
+#endif /* DRIVERS_I2C_SOFT_H_ */
diff --git a/UAV-ControlSystem/inc/drivers/motormix.h b/UAV-ControlSystem/inc/drivers/motormix.h
index 85522b9..fc999e7 100644
--- a/UAV-ControlSystem/inc/drivers/motormix.h
+++ b/UAV-ControlSystem/inc/drivers/motormix.h
@@ -46,6 +46,9 @@ typedef struct  {
 /* Global mixerConfig to bee available to EEPROM */
 extern mixerConfig_s mixerConfig;
 
+/*Is set in motor mix and used in pidUAVcore and mix */
+extern bool motorLimitReached;
+
 /***********************************************************************
 * BRIEF: The motormixer                                                *
 * INFORMATION: Sums the output from all control loops and adapts the   *
diff --git a/UAV-ControlSystem/inc/stm32f4xx_revo.h b/UAV-ControlSystem/inc/stm32f4xx_revo.h
index 1d71484..90f7cf2 100644
--- a/UAV-ControlSystem/inc/stm32f4xx_revo.h
+++ b/UAV-ControlSystem/inc/stm32f4xx_revo.h
@@ -137,6 +137,8 @@
 
  /* Baro */
 #define BARO
+#define BARO_USE_I2C_SOFT
+//#define BARO_USE_I2C_HARD //Dont work with DMA right now if fixed should be faster. Otherwise software is faster than hardware I2C
 
 #define MPU6000_NSS_PIN					GPIO_PIN_4
 #define MPU6000_NSS_PORT				GPIOA
diff --git a/UAV-ControlSystem/inc/system_variables.h b/UAV-ControlSystem/inc/system_variables.h
index 607b8e1..0217fd9 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 102
+#define EEPROM_SYS_VERSION 107
 
 #define ADC_STATE
 #include "stm32f4xx.h"
diff --git a/UAV-ControlSystem/inc/utilities.h b/UAV-ControlSystem/inc/utilities.h
index 446ddea..b8fa784 100644
--- a/UAV-ControlSystem/inc/utilities.h
+++ b/UAV-ControlSystem/inc/utilities.h
@@ -25,6 +25,8 @@
 
 #define maxStringSize_CLI  		100		//Max sting size used for the messages in the CLI
 
+#define ABS_FLOAT(x)	(((x) < 0)? -(x): (x))
+
 typedef char typeString[maxStringSize_CLI];
 typedef struct typeStringArr { char val[maxStringSize_CLI]; } typeStringArr;
 
@@ -81,6 +83,8 @@ uint32_t accumulate(uint32_t list[], int length);
 ***********************************************************************/
 void Error_Handler(void);
 
+
+
 uint8_t reverse(uint8_t byte);
 
 int16_t constrain(int16_t value, int16_t min, int16_t max);
diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c
index 082ef17..c16c8ec 100644
--- a/UAV-ControlSystem/src/Flight/pid.c
+++ b/UAV-ControlSystem/src/Flight/pid.c
@@ -1,6 +1,6 @@
 /**************************************************************************
 * NAME: pid.c      		                                                  *
-* AUTHOR: Johan Gärtner                                                   *
+* AUTHOR: Johan G�rtner                                                   *
 *
 * PURPOSE: This file contains pid functions		                          *
 * INFORMATION: pidUAV is the final pid controller which only takes        *
@@ -14,19 +14,21 @@
 * **************************************************************************/
 
 #include "Flight/pid.h"
-#include "drivers/accel_gyro.h"
 #include "drivers/sbus.h"
 #include "scheduler/scheduler.h"
 #include <math.h>
 #include "drivers/failsafe_toggles.h"
+#include "drivers/motormix.h"
+#include "utilities.h"
+
 
 #define PTERM_SCALE 0.032029f										/*P-term used as a scale value to the PID controller*/
-#define ITERM_SCALE 0.244381f										/*I-term used as a scale value to the PID controller*/
+#define ITERM_SCALE 0.0012f											/*I-term used as a scale value to the PID controller*/
 #define DTERM_SCALE 0.000529f										/*D-term used as a scale value to the PID controller*/
 
 #define RADIO_RANGE 500												/*Radio range input*/
 #define BAROMETER_RANGE 2000										/*Determines the range of the maximum height (limits the rc input)*/
-#define ACCELEROMETER_RANGE 20										/*Determines the range of the maximum angle  (limits the rc input) & (Accelerometer takes int to max 16 G)*/
+#define ACCELEROMETER_RANGE 60										/*Determines the range of the maximum angle  (limits the rc input) & (Accelerometer takes int to max 16 G)*/
 #define GYRO_RANGE 720												/*Determines the maximum rotational limit (limits the rc input)*/
 #define COMPASS_RANGE 180											/*Determines the maximum compass limit (limits the rc input)*/
 
@@ -53,8 +55,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*/
-gyro_t gyroProfile;													/*Struct profile for input data from sensor*/
+accel_t accelProfile = {0};												/*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;
+
 
 
 /**************************************************************************
@@ -68,11 +76,16 @@ float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, cons
 	switch (axis)
 	{
 	case ROLL:
-		angle = atan2(z_axis, sqrt(x_axis*x_axis + y_axis*y_axis))*180/M_PI - 90;
-		angle = (x_axis < 0) ? -angle : angle;										/*CW (right, form behind) = pos angle*/
+
+		angle = atan2(x_axis, sqrt(y_axis*y_axis + z_axis*z_axis))*180/M_PI;
+		angle = -1*((angle > 0)? (z_axis < 0 )? 180 - angle: angle : (z_axis < 0 )? - 180 - angle: angle);
+
 		break;
 	case PITCH:
-		angle = atan2(sqrt(x_axis*x_axis + z_axis*z_axis), y_axis)*180/M_PI - 90; 	/*down (the front down against ground) = pos angle*/
+
+		angle = atan2( y_axis, sqrt(z_axis*z_axis + x_axis*x_axis))*180/M_PI; 	/*down (the front down against ground) = pos angle*/
+		angle = (angle > 0)? ((z_axis < 0))? 180 - angle: angle : (z_axis < 0 )? - 180 - angle: angle;
+
 		break;
 	default:
 		angle = 0;
@@ -82,6 +95,11 @@ float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, cons
 	return angle;
 }
 
+float calcGravity(accel_t profile ) //const float x_axis, const float y_axis, const float z_axis)
+{
+	return sqrt(profile.accelXconv*profile.accelXconv + profile.accelYconv*profile.accelYconv + profile.accelZconv*profile.accelZconv);
+}
+
 /**************************************************************************
 * BRIEF: Scales data from input range to output range                     *
 * INFORMATION:                                                            *
@@ -89,7 +107,6 @@ float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, cons
 float convertData(int inputRange, int outputRange, int offset, float value)
 {
 	return ((float)outputRange/(float)inputRange)*(value-(float)offset);
-	//return 1.0;
 }
 
 /**************************************************************************
@@ -98,20 +115,29 @@ float convertData(int inputRange, int outputRange, int offset, float value)
 **************************************************************************/
 float constrainf(float amt, int low, int high)
 {
-	if (amt < low)
-		return low;
-	else if (amt > high)
-		return high;
+	if (amt < (float)low)
+		return (float)low;
+	else if (amt > (float)high)
+		return (float)high;
 	else
 		return amt;
 }
 
+
+float oldSensorValue[2] = {0};
+float oldSensorValueRoll[12] = {0};
+float oldSensorValuePitch[12] = {0};
+
+int i = 0;
+
 /**************************************************************************
 * BRIEF: Update current sensor values				                      *
 * INFORMATION:                       									  *
 **************************************************************************/
 void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
 {
+
+
 	switch (ID_profile)
 	{
 	case PID_ID_GYRO:
@@ -127,9 +153,64 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
 
 		mpu6000_read_accel(&accelProfile);	/*Reads data from accelerometer*/
 
+//
+//		if (calcGravity(accelProfile) > 1.15)
+//		{
+//
+//			sensorValues[ROLL] = gyroProfile.gyroY*PidProfileBuff[ROLL].dT;
+//			sensorValues[PITCH]  = gyroProfile.gyroX*PidProfileBuff[PITCH].dT;
+//
+//		}
+//		else
+//		{
+
+
+		float alpha = 0.5;
 		/*May need Low pass filter since the accelerometer may drift*/
-		sensorValues[ROLL] = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
-		sensorValues[PITCH] = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
+
+		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;
+
+		float RollValue = 0;
+		float PitchValue = 0;
+
+		for (int ii = 0; ii < 12; ii++)
+		{
+			RollValue = RollValue + oldSensorValueRoll[ii];
+			PitchValue = PitchValue + oldSensorValuePitch[ii];
+
+		}
+
+
+		i = (i < 11)? i + 1:0;
+
+		sensorValues[ROLL] = RollValue/12;
+		sensorValues[PITCH]  = PitchValue/12;
+
+		sensorValues[ROLL] = alpha*RollValue/12 + (1-alpha)*oldSensorValue[0];
+		sensorValues[PITCH] = alpha*PitchValue/12 + (1-alpha)*oldSensorValue[1];
+//
+		oldSensorValue[0] = sensorValues[ROLL];
+		oldSensorValue[1] = sensorValues[PITCH];
+
+
+
+//		sensorValues[ROLL] = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
+//		sensorValues[PITCH] = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
+
+		//float sensorRoll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
+		//sensorValues[ROLL] = pt1FilterApply4(&accelFilter[0], sensorRoll, 90, PidProfileBuff[ROLL].dT);
+
+		//float sensorPitch = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
+		//sensorValues[PITCH] = pt1FilterApply4(&accelFilter[1], sensorPitch, 90, PidProfileBuff[PITCH].dT);
 
 		break;
 	case PID_ID_COMPASS:
@@ -211,7 +292,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
 
 
 	/* -----calculate P component ---- */
-	float PTerm = PTERM_SCALE * rateError * pidProfile->P[axis] * pidProfile-> PIDweight[axis] / 100;
+	float PTerm = PTERM_SCALE * rateError * (float)pidProfile->P[axis] * (float)pidProfile-> PIDweight[axis] / 100.0;
 
 	// Constrain YAW by yaw_p_limit value
 	if (axis == YAW)
@@ -230,11 +311,11 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
 
 
 	/* -----calculate I component ---- */
-	float ITerm = pidProfileBuff->lastITerm[axis] + ITERM_SCALE * rateError * pidProfileBuff->dT * pidProfile->I[axis];
+	float ITerm = pidProfileBuff->lastITerm[axis] + ITERM_SCALE * rateError * pidProfileBuff->dT * (float)pidProfile->I[axis];
 
 	// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
 	// I coefficient (I) moved before integration to make limiting independent from PID settings
-	ITerm = constrainf(ITerm, -PID_MAX_I, PID_MAX_I);
+	ITerm = constrainf(ITerm, -(int)PID_MAX_I, (int)PID_MAX_I);
 
 	// Anti windup protection
 	if (motorLimitReached)
@@ -243,9 +324,15 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
 	}
 	else
 	{
-		pidProfileBuff->ITermLimit[axis] = abs(ITerm);
+		pidProfileBuff->ITermLimit[axis] = ABS_FLOAT(ITerm);
 	}
 
+//		if (motorLimitReached)
+//		{
+//			ITerm = pidProfileBuff->lastITerm[axis];
+//		}
+
+
 	pidProfileBuff->lastITerm[axis] = ITerm;
 
 
@@ -274,13 +361,19 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
 			delta = pt1FilterApply4(&pidProfileBuff->deltaFilter[axis], delta, pidProfile->dterm_lpf, pidProfileBuff->dT);
 		}
 
-		DTerm = DTERM_SCALE * delta * pidProfile->D[axis] * pidProfile->PIDweight[axis] / 100;
+		DTerm = DTERM_SCALE * delta * (float)pidProfile->D[axis] * (float)pidProfile->PIDweight[axis] / 100.0;
 		DTerm = constrainf(DTerm, -PID_MAX_D, PID_MAX_D);
 	}
 
 
 	/*----PID OUT----*/
-	pidProfile->PID_Out[axis] = constrainf(PTerm + ITerm + DTerm, -pidProfile->pid_out_limit, pidProfile->pid_out_limit);
+	if(!flags_IsSet_ID(systemFlags_armed_id) || (rc_input.Throttle < mixerConfig.minCheck))
+	{
+		ITerm = 0;
+		pidProfileBuff->lastITerm[axis] = 0;
+		pidProfileBuff->ITermLimit[axis] = 0;
+	}
+	pidProfile->PID_Out[axis] = constrainf(PTerm + ITerm + DTerm, -(int)pidProfile->pid_out_limit, (int)pidProfile->pid_out_limit);
 }
 
 /**************************************************************************
@@ -368,6 +461,8 @@ void pidRun(uint8_t ID)
 	}
 }
 
+/*--------------------------------------------Init Functions----------------------------------------------------------------------------------*/
+
 /**************************************************************************
 * BRIEF: Initializes a certain pidbuffer profile connected to             *
 * 		 a PID controller 												  *
@@ -383,23 +478,23 @@ void pidUAVInitBuff(pidProfileBuff_t *pidProfile, uint8_t ID)
 	case PID_ID_GYRO:
 
 		PidProfileBuff[ID].DOF = 3;
-		PidProfileBuff[ID].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000; //ÄNDRA TILL SEKUNDER inte ms
+		PidProfileBuff[ID].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000000; //�NDRA TILL SEKUNDER inte ms
 
 		break;
 	case PID_ID_ACCELEROMETER:
 
 		PidProfileBuff[ID].DOF = 2;
-		PidProfileBuff[ID].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000;
+		PidProfileBuff[ID].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000000;
 
 		break;
 	case PID_ID_COMPASS:
 
-		PidProfileBuff[ID].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000;
+		PidProfileBuff[ID].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000000;
 
 		break;
 	case PID_ID_BAROMETER:
 
-		PidProfileBuff[ID].dT = SystemTasks[TASK_BARO].desiredPeriod/1000;
+		PidProfileBuff[ID].dT = SystemTasks[TASK_BARO].desiredPeriod/1000000;
 
 		break;
 	default:
@@ -442,18 +537,25 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
 	PidProfile[ID].PID_Out[PITCH] = 0;
 	PidProfile[ID].PID_Out[YAW] = 0;
 
-	PidProfile[ID].PIDweight[ROLL] = 100;
-	PidProfile[ID].PIDweight[PITCH] = 100;
-	PidProfile[ID].PIDweight[YAW] = 100;
 
-	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;
+
 		PidProfile[ID].pidEnabled = true;
 		PidProfile[ID].dterm_lpf = 90;
 		PidProfile[ID].pid_out_limit = 3000;
@@ -461,6 +563,18 @@ 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;
+
 		PidProfile[ID].pidEnabled = true;
 		PidProfile[ID].dterm_lpf = 90;
 		PidProfile[ID].pid_out_limit = 1000;
@@ -468,11 +582,27 @@ 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;
+
 		PidProfile[ID].pidEnabled = false;
 
 		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;
+
 		PidProfile[ID].pidEnabled = false;
 
 		break;
@@ -490,7 +620,6 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
 void pidInit()
 {
 	mpu6000_init(&gyroProfile,&accelProfile); /*Init gyro and accelerometer*/
-	motorLimitReached = false;
 
 	pidUAVInitBuff(&PidProfileBuff[PID_ID_GYRO], PID_ID_GYRO);
 	pidUAVInitBuff(&PidProfileBuff[PID_ID_ACCELEROMETER], PID_ID_ACCELEROMETER);
@@ -502,3 +631,19 @@ void pidInit()
 	pidUAVInit(&PidProfile[PID_ID_BAROMETER], PID_ID_BAROMETER);
 	pidUAVInit(&PidProfile[PID_ID_COMPASS], PID_ID_COMPASS);
 }
+
+void pidEproom(void)
+{
+
+	PidProfile[PID_ID_ACCELEROMETER].PIDweight[ROLL] = 2;
+	PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 2;
+	PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100;
+
+	PidProfileBuff[PID_ID_GYRO].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000; //�NDRA TILL SEKUNDER inte ms
+	PidProfileBuff[PID_ID_ACCELEROMETER].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000;
+	PidProfileBuff[PID_ID_COMPASS].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000;
+	PidProfileBuff[PID_ID_BAROMETER].dT = SystemTasks[TASK_BARO].desiredPeriod/1000;
+
+	PidProfile[PID_ID_GYRO].I[YAW] = 40;
+
+}
diff --git a/UAV-ControlSystem/src/Scheduler/tasks.c b/UAV-ControlSystem/src/Scheduler/tasks.c
index 46c2a42..f06d65d 100644
--- a/UAV-ControlSystem/src/Scheduler/tasks.c
+++ b/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,
 		},
 
@@ -105,7 +105,7 @@ task_t SystemTasks[TASK_COUNT] =
 		{
 			.taskName = "BAROMETER",
 			.taskFunction = systemTaskBaro,
-			.desiredPeriod = GetUpdateRateHz(20),			//20 hz update rate (50 ms)
+			.desiredPeriod = GetUpdateRateHz(200),			//200 hz update rate (5 ms)
 			.staticPriority = PRIORITY_LOW,
 		},
 #endif
diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c
index acc5305..1d7cb04 100644
--- a/UAV-ControlSystem/src/config/cli.c
+++ b/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
@@ -73,6 +75,10 @@ typedef enum
 	ACTION_RESET,					//Resets the entire eeprom
 	ACTION_STATS,					//gives statistics on the system
 	ACTION_MOTORCALIBRATE,
+	ACTION_GYROCALIBRATE,
+	ACTION_ACCELEROMTETERCALIBRATE,
+	ACTION_COMPASSCALIBRATE,
+	ACTION_CALIBRATIONINFOACCEL,
 
 	//The number of actions
 	ACTION_COUNT,					//Count of the number of actions
@@ -94,7 +100,11 @@ const typeString commandAction_Strings[ACTION_COUNT] = {
 	"reboot",
 	"reset",
 	"stats",
-	"motorcalibrate"
+	"calibrate_motors",
+	"calibrate_gyro",
+	"calibrate_accelerometer",
+	"calibrate_compass",
+	"calibration_info_accelerometer"
 };
 
 /* String values descrbing information of a certain action */
@@ -110,8 +120,12 @@ const typeString commandActionInformation_Strings[ACTION_COUNT] = {
 	"| exit - Exits the CLI mode.\n\r",
 	"| 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."
+	"| stats - Gives some current stats of the system and tasks.\n\r",
+	"| calibrate_motors - Calibrates all motors.",
+	"| calibrate_gyro - Calibrates the gyro.",
+	"| calibrate_accelerometer - Calibrates the accelerometer.",
+	"| calibrate_compass - Calibrates the compass.",
+	"| calibration_info_accelerometer - Provides info on the accelerometer calibration."
 };
 
 /* String array containing all the signature examples for each action */
@@ -128,7 +142,11 @@ const typeString commandActionSignature_Strings[ACTION_COUNT] = {
 	" reboot",
 	" reset",
 	" stats",
-	" motorcalibrate"
+	" calibrate_motors",
+	" calibrate_gyro",
+	" calibrate_accelerometer",
+	" calibrate_compass",
+	" calibration_info_accelerometer"
 };
 
 /* Size values for the values a command will require */
@@ -1414,17 +1432,17 @@ void cliRun()
 				break;
 			case commandMask(commandSize_1, ACTION_RESET):
 				//resets all the values in the eeprom to the default values
-					TransmitBack("- All values will be deleted and system rebooted. Continue? (y/n)... \n\n\r");
+				TransmitBack("- All values will be deleted and system rebooted. Continue? (y/n)... \n\n\r");
 
-					//read until a y or n is found
-					if (ReceiveBinaryDecision(121, 110))
-					{
-						defaultEEPROM();
-					}
-					else
-					{
-						TransmitBack("- Values unchanged...\n\n\r");
-					}
+				//read until a y or n is found
+				if (ReceiveBinaryDecision(121, 110))
+				{
+					defaultEEPROM();
+				}
+				else
+				{
+					TransmitBack("- Values unchanged...\n\n\r");
+				}
 				break;
 			case commandMask(commandSize_1, ACTION_STATS):
 					TransmitSystemStats();
@@ -1438,13 +1456,67 @@ void cliRun()
 					{
 						TransmitBack("Starting calibration, BE CAREFUL!!! \n\n\r");
 						calibrateMotors();
+						TransmitBack("Wait until the beeping have ceased... \n\n\r");
 					}
 					else
 					{
 						TransmitBack("Exiting the calibration, BE CAREFUL!!! \n\n\r");
 					}
 
-				break;
+					break;
+			case commandMask(commandSize_1, ACTION_GYROCALIBRATE):
+					TransmitBack("Do you really want to calibrate the gyro? (y/n)\n\n\r");
+					if (ReceiveBinaryDecision(121,110))
+					{
+						TransmitBack("Starting calibration! \n\n\r");
+						TransmitBack("NOT IMPLEMENTED! \n\n\r");
+						TransmitBack("Calibration complete! \n\n\r");
+					}
+					else
+					{
+						TransmitBack("Exiting the calibration! \n\n\r");
+					}
+
+					break;
+			case commandMask(commandSize_1, ACTION_ACCELEROMTETERCALIBRATE):
+					TransmitBack("Do you really want to calibrate the accelerometer? (y/n)\n\n\r");
+					if (ReceiveBinaryDecision(121,110))
+					{
+						TransmitBack("Starting calibration! \n\n\r");
+						mpu6000_read_acc_offset(&accelProfile);
+						TransmitBack("Calibration complete! \n\n\r");
+					}
+					else
+					{
+						TransmitBack("Exiting the calibration! \n\n\r");
+					}
+
+					break;
+			case commandMask(commandSize_1, ACTION_COMPASSCALIBRATE):
+					TransmitBack("Do you really want to calibrate the compass? (y/n)\n\n\r");
+					if (ReceiveBinaryDecision(121,110))
+					{
+						TransmitBack("Starting calibration! \n\n\r");
+						TransmitBack("NOT IMPLEMENTED! \n\n\r");
+						TransmitBack("Calibration complete! \n\n\r");
+					}
+					else
+					{
+						TransmitBack("Exiting the calibration! \n\n\r");
+					}
+
+					break;
+			case commandMask(commandSize_1, ACTION_CALIBRATIONINFOACCEL):
+					{
+					char tempBuffer[100];
+					TransmitBack("- Accelerometer calibration information: \n\n\r");
+					sprintf(tempBuffer, "- Finetune values: \n\r- Pitch: %0.2f \n\r- Roll: %0.2f \n\n\r", accPitchFineTune, accRollFineTune);
+					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);
+					TransmitBack(tempBuffer);
+
+					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 c58f327..7852cc1 100644
--- a/UAV-ControlSystem/src/config/eeprom.c
+++ b/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 */
diff --git a/UAV-ControlSystem/src/drivers/I2C.c b/UAV-ControlSystem/src/drivers/I2C.c
index ed643b0..f7d204f 100644
--- a/UAV-ControlSystem/src/drivers/I2C.c
+++ b/UAV-ControlSystem/src/drivers/I2C.c
@@ -34,8 +34,8 @@ bool i2c_configure(I2C_TypeDef *i2c,
 		i2c_port = I2C1_PORT;
 		sda_pin = I2C1_SDA_PIN;
 		scl_pin = I2C1_SCL_PIN;
-		if(__HAL_RCC_I2C1_IS_CLK_DISABLED())
-			__HAL_RCC_I2C1_CLK_ENABLE();
+//		if(__HAL_RCC_I2C1_IS_CLK_DISABLED())
+//			__HAL_RCC_I2C1_CLK_ENABLE();
 	}
 	else if(i2c == I2C2)
 	{
@@ -55,22 +55,26 @@ bool i2c_configure(I2C_TypeDef *i2c,
 	//Initialize pins for SCL and SDA
 	GPIO_InitTypeDef GPIO_InitStruct;
 	GPIO_InitStruct.Pin = sda_pin | scl_pin;
-	GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
-	GPIO_InitStruct.Pull = GPIO_PULLUP;
+	GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
+	GPIO_InitStruct.Pull = GPIO_NOPULL;
 	GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
 	GPIO_InitStruct.Alternate = i2c_af;
 	HAL_GPIO_Init(i2c_port, &GPIO_InitStruct);
 
+
+	HAL_Delay(10);
+	if(__HAL_RCC_I2C1_IS_CLK_DISABLED())
+				__HAL_RCC_I2C1_CLK_ENABLE();
 	//Initialize I2C communication
 	out_profile->Instance = i2c;
-	out_profile->Init.ClockSpeed = 100000;
-	out_profile->Init.DutyCycle = I2C_DUTYCYCLE_2;
+	out_profile->Init.ClockSpeed = 400000;
+	out_profile->Init.DutyCycle = I2C_DUTYCYCLE_2/*I2C_DUTYCYCLE_2*/;
 	out_profile->Init.OwnAddress1 = my_address;
 	out_profile->Init.OwnAddress2 = 0;
 	out_profile->Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
 	out_profile->Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
 	out_profile->Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
-	out_profile->Init.NoStretchMode = I2C_NOSTRETCH_ENABLE;
+	out_profile->Init.NoStretchMode = I2C_NOSTRETCH_DISABLED;
 	if(HAL_I2C_Init(out_profile) != HAL_OK)
 		return false;
 
@@ -115,14 +119,155 @@ bool i2c_send(I2C_HandleTypeDef* profile,
 			  uint8_t* data,
 			  uint32_t length)
 {
+	//TODO: Fix this function
 	uint8_t i = 0;
 	// try to send the data 10 times and if no acknowledge is received during these 10 messages we stop trying so that
 	// the system don't gets stuck forever because a slave is unreachable
 	while(HAL_I2C_Master_Transmit(profile,(slave_address << 1), (uint8_t*)data, length, 1000) != HAL_OK && i++ < 10)
-	{}
+	{I2C1->CR1 |= (1 << 9);}
+//	while(HAL_I2C_Master_Transmit(profile, slave_address, (uint8_t*)data, length, 5000) != HAL_OK && i++ < 10)
+//		{}
 
 	//Wait til the I2C bus is done with all sending
     while (HAL_I2C_GetState(profile) != HAL_I2C_STATE_READY){}
 
 	return (i < 10);
 }
+
+uint8_t dma_baro_rx_buffer[3];
+uint8_t dma_baro_tx_buffer[1];
+
+bool i2c_configure_DMA(I2C_TypeDef *i2c,
+				   I2C_HandleTypeDef *out_profile,
+				   DMA_HandleTypeDef *out_rxDMA_profile,
+				   DMA_HandleTypeDef *out_txDMA_profile,
+				   uint32_t my_address)
+{
+	uint8_t i2c_af;
+	uint16_t sda_pin, scl_pin;
+	GPIO_TypeDef *i2c_port;
+	DMA_Stream_TypeDef *dma_rx_instance, *dma_tx_instance;
+	uint32_t channel;
+
+	// get the correct alternate function and pins for the selected I2C
+	// Only I2C1 and I2C2 is available on the REVO board
+	if(i2c == I2C1)
+	{
+		i2c_af = GPIO_AF4_I2C1;
+		i2c_port = I2C1_PORT;
+		sda_pin = I2C1_SDA_PIN;
+		scl_pin = I2C1_SCL_PIN;
+		dma_rx_instance = DMA1_Stream5;
+		dma_tx_instance = DMA1_Stream6;
+		channel = DMA_CHANNEL_1;
+//		if(__HAL_RCC_I2C1_IS_CLK_DISABLED())
+//			__HAL_RCC_I2C1_CLK_ENABLE();
+	}
+	else if(i2c == I2C2)
+	{
+		i2c_af = GPIO_AF4_I2C2;
+		i2c_port = I2C2_PORT;
+		sda_pin = I2C2_SDA_PIN;
+		scl_pin = I2C2_SCL_PIN;
+		if(__HAL_RCC_I2C2_IS_CLK_DISABLED())
+			__HAL_RCC_I2C2_CLK_ENABLE();
+	}
+	else
+		return false; // The provided I2C is not correct
+
+	if(__HAL_RCC_GPIOB_IS_CLK_DISABLED())
+		__HAL_RCC_GPIOB_CLK_ENABLE();
+
+
+	//Initialize pins for SCL and SDA
+	GPIO_InitTypeDef GPIO_InitStruct;
+	GPIO_InitStruct.Pin = sda_pin | scl_pin;
+	GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
+	GPIO_InitStruct.Pull = GPIO_NOPULL;
+	GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
+	GPIO_InitStruct.Alternate = i2c_af;
+	HAL_GPIO_Init(i2c_port, &GPIO_InitStruct);
+
+	HAL_Delay(10);
+	if(__HAL_RCC_I2C1_IS_CLK_DISABLED())
+				__HAL_RCC_I2C1_CLK_ENABLE();
+	if(__HAL_RCC_DMA1_IS_CLK_DISABLED())
+				__HAL_RCC_DMA1_CLK_ENABLE();
+	//Initialize I2C communication
+	out_profile->Instance = i2c;
+	out_profile->Init.ClockSpeed = 400000;
+	out_profile->Init.DutyCycle = I2C_DUTYCYCLE_2/*I2C_DUTYCYCLE_2*/;
+	out_profile->Init.OwnAddress1 = my_address;
+	out_profile->Init.OwnAddress2 = 0;
+	out_profile->Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
+	out_profile->Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
+	out_profile->Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
+	out_profile->Init.NoStretchMode = I2C_NOSTRETCH_DISABLED;
+
+
+	if(HAL_I2C_Init(out_profile) != HAL_OK)
+		return false;
+
+	// Enable the DMA on the i2c register level
+	out_profile->Instance->CR2 |= (1 << 11);
+
+	/* Create the DMAs */
+	DMA_HandleTypeDef out_rxDMA_profile2;
+	out_rxDMA_profile2.Instance = dma_rx_instance;
+	out_rxDMA_profile2.Init.Channel = channel;
+	out_rxDMA_profile2.Init.Direction = DMA_PERIPH_TO_MEMORY;
+	out_rxDMA_profile2.Init.PeriphInc = DMA_PINC_DISABLE;
+	out_rxDMA_profile2.Init.MemInc = DMA_MINC_ENABLE;
+	out_rxDMA_profile2.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
+	out_rxDMA_profile2.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
+	out_rxDMA_profile2.Init.Mode = DMA_CIRCULAR;
+	out_rxDMA_profile2.Init.Priority = DMA_PRIORITY_VERY_HIGH;
+	if(HAL_DMA_Init(&out_rxDMA_profile2) != HAL_OK)
+		return false;
+
+	__HAL_LINKDMA(out_profile ,hdmarx, out_rxDMA_profile2);
+
+	DMA_HandleTypeDef out_txDMA_profile2;
+	out_txDMA_profile2.Instance = dma_tx_instance;
+	out_txDMA_profile2.Init.Channel = channel;
+	out_txDMA_profile2.Init.Direction = DMA_MEMORY_TO_PERIPH;
+	out_txDMA_profile2.Init.PeriphInc = DMA_PINC_DISABLE;
+	out_txDMA_profile2.Init.MemInc = DMA_MINC_ENABLE;
+	out_txDMA_profile2.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
+	out_txDMA_profile2.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
+	out_txDMA_profile2.Init.Mode = DMA_CIRCULAR;
+	out_txDMA_profile2.Init.Priority = DMA_PRIORITY_VERY_HIGH;
+	if(HAL_DMA_Init(&out_txDMA_profile2) != HAL_OK)
+			return false;
+
+	__HAL_LINKDMA(out_profile ,hdmatx, out_txDMA_profile2);
+
+
+	//Setup DMA buffers
+
+	// Disable the DMA, must be done before writing to the addresses below
+	dma_rx_instance->CR &= ~(DMA_SxCR_EN);
+
+	dma_rx_instance->NDTR = 3;                                    // Set the buffer size
+	dma_rx_instance->PAR =  (uint32_t)&i2c->DR; // Set the address to the USART data register
+	dma_rx_instance->M0AR = (uint32_t)dma_baro_rx_buffer;                // Set the address to the first DMA buffer
+	dma_rx_instance->CR &=  ~(0xF << 11);                                         // Set the data size to 8 bit values
+
+	//Enable the DMA again to start receiving data from the USART
+	dma_rx_instance->CR |= DMA_SxCR_EN;
+
+
+	dma_tx_instance->CR &= ~(DMA_SxCR_EN);
+
+	dma_tx_instance->NDTR = 1;
+	dma_tx_instance->PAR =  (uint32_t)&i2c->DR;
+	dma_tx_instance->M0AR = (uint32_t)dma_baro_tx_buffer;
+	dma_tx_instance->CR &=  ~(0xF << 11);
+
+
+	dma_tx_instance->CR |= DMA_SxCR_EN;
+
+
+	return true;
+}
+
diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c
index 45851c8..759dcc5 100644
--- a/UAV-ControlSystem/src/drivers/accel_gyro.c
+++ b/UAV-ControlSystem/src/drivers/accel_gyro.c
@@ -44,7 +44,7 @@ bool mpu6000_transmit_receive(uint8_t reg, uint8_t* data, uint8_t length, uint32
 
 /***********************************************************************
  * BRIEF: mpu6000_read_offset reads and returns the offset of the       *
- * gyroscope and accelerometer                                          *
+ * gyroscope                                                            *
  * INFORMATION:                                                         *
  * Is automatically called when mpu6000_init is called                  *
  * The flight controller needs to be stationary when this function is   *
@@ -52,46 +52,104 @@ bool mpu6000_transmit_receive(uint8_t reg, uint8_t* data, uint8_t length, uint32
  * When the UAV is finished this data could be saved so that the        *
  * offset doesn't need to be read every time                            *
  ***********************************************************************/
-HAL_StatusTypeDef mpu6000_read_offset(gyro_t* gyro, accel_t* accel)
+HAL_StatusTypeDef mpu6000_read_gyro_offset(gyro_t* gyro)
 {
 	uint8_t dataG[6]; /* Temporary data variable used to receive gyroscope data */
-	uint8_t dataA[6]; /* Temporary data variable used to receive accelerometer data */
+	int16_t dataGCheck[6] = {0};	/* checkval */
+	uint8_t maxDrift = 5;
+	bool calibrate_go = true;
+	bool calibrate_ok = false;
+	int countCalibrate = 0;
 
-	if(!mpu6000_transmit_receive(MPU_RA_ACCEL_XOUT_H, dataA, 6, 10))
-		return HAL_ERROR;
+	//small delay so that we know the gyro should give some values
+	HAL_Delay(100);
+	for (int i = 0; i < 100; i++)
+	{
+		//assume that we should calibrate at the start of each loop
+		calibrate_go = true;
+
+		//get new values
+		if(!mpu6000_transmit_receive(MPU_RA_GYRO_XOUT_H, dataG, 6, 10))
+				return HAL_ERROR;
+
+		//cheack the new values and see if they are within acceptable range compared to the previous values, so that it is realativelly still
+		for(int j = 0; j < 6; j ++)
+		{
+			int16_t currentDrift = (int16_t)abs(dataGCheck[j] - dataG[j]);
+			if (!(currentDrift < maxDrift))
+				calibrate_go = false;
+
+			dataGCheck[j] = dataG[j];
+		}
+
+		//if true, we have good values exit loop
+		if (calibrate_go)
+		{
+			countCalibrate++;
+		}
+		if (countCalibrate > 4)
+		{
+			calibrate_ok = true;
+			break;
+		}
+		//wait for a little bit so the checks are not to fast
+		HAL_Delay(200);
+	}
 
-	if(!mpu6000_transmit_receive(MPU_RA_GYRO_XOUT_H, dataG, 6, 10))
-		return HAL_ERROR;
 
 #ifdef YAW_ROT_0
 	gyro->offsetX = -(((int16_t)dataG[0] << 8) | dataG[1]);
 	gyro->offsetY = -(((int16_t)dataG[2] << 8) | dataG[3]);
 	gyro->offsetZ =  (((int16_t)dataG[4] << 8) | dataG[5]);
 
-	accel->offsetX = -((int16_t)dataA[0] << 8) | dataA[1];
-	accel->offsetY = -((int16_t)dataA[2] << 8) | dataA[3];
-	accel->offsetZ =  accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]);
 #elif defined(YAW_ROT_90)
 	gyro->offsetX = -(((int16_t)dataG[2] << 8) | dataG[3]);
 	gyro->offsetY =  (((int16_t)dataG[0] << 8) | dataG[1]);
 	gyro->offsetZ =  (((int16_t)dataG[4] << 8) | dataG[5]);
-
-	accel->offsetX = -((int16_t)dataA[2] << 8) | dataA[3];
-	accel->offsetY =  ((int16_t)dataA[0] << 8) | dataA[1];
-	accel->offsetZ =  accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]);
 #elif defined(YAW_ROT_180)
 	gyro->offsetX = (((int16_t)dataG[0] << 8) | dataG[1]);
 	gyro->offsetY = (((int16_t)dataG[2] << 8) | dataG[3]);
 	gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]);
-
-	accel->offsetX = ((int16_t)dataA[0] << 8) | dataA[1];
-	accel->offsetY = ((int16_t)dataA[2] << 8) | dataA[3];
-	accel->offsetZ =  accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]);
 #elif defined(YAW_ROT_270)
 	gyro->offsetX =  (((int16_t)dataG[2] << 8) | dataG[3]);
 	gyro->offsetY = -(((int16_t)dataG[0] << 8) | dataG[1]);
 	gyro->offsetZ =  (((int16_t)dataG[4] << 8) | dataG[5]);
+#endif
 
+	return HAL_OK;
+}
+
+/***********************************************************************
+ * BRIEF: mpu6000_read_offset reads and returns the offset of the       *
+ * accelerometer                                                        *
+ * INFORMATION:                                                         *
+ * Is automatically called when mpu6000_init is called                  *
+ * The flight controller needs to be stationary when this function is   *
+ * called                                                               *
+ * When the UAV is finished this data could be saved so that the        *
+ * offset doesn't need to be read every time                            *
+ ***********************************************************************/
+HAL_StatusTypeDef mpu6000_read_acc_offset(accel_t* accel)
+{
+	uint8_t dataA[6]; /* Temporary data variable used to receive accelerometer data */
+
+	if(!mpu6000_transmit_receive(MPU_RA_ACCEL_XOUT_H, dataA, 6, 10))
+		return HAL_ERROR;
+
+
+#ifdef YAW_ROT_0
+	accel->offsetX = -((int16_t)dataA[0] << 8) | dataA[1];
+	accel->offsetY = -((int16_t)dataA[2] << 8) | dataA[3];
+	accel->offsetZ =  accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]);
+#elif defined(YAW_ROT_90)
+	accel->offsetX = -((int16_t)dataA[2] << 8) | dataA[3];
+	accel->offsetY =  ((int16_t)dataA[0] << 8) | dataA[1];
+	accel->offsetZ =  accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]);
+#elif defined(YAW_ROT_180)
+	accel->offsetX = ((int16_t)dataA[0] << 8) | dataA[1];
+	accel->offsetY = ((int16_t)dataA[2] << 8) | dataA[3];
+	accel->offsetZ =  accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]);
+#elif defined(YAW_ROT_270)
 	accel->offsetX =  ((int16_t)dataA[2] << 8) | dataA[3];
 	accel->offsetY = -((int16_t)dataA[0] << 8) | dataA[1];
 	accel->offsetZ =  accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]);
@@ -180,9 +238,11 @@ bool mpu6000_init(gyro_t* gyro, accel_t* accel)
 	if(!mpu6000_transmit(reg, 2))
 		return false;
 
+	mpu6000_read_gyro_offset(gyro);
+	//mpu6000_read_acc_offset(accel);
+
 	HAL_Delay(60);
 
-	mpu6000_read_offset(gyro, accel);
 
 	return true;
 }
diff --git a/UAV-ControlSystem/src/drivers/barometer.c b/UAV-ControlSystem/src/drivers/barometer.c
new file mode 100644
index 0000000..b3fa766
--- /dev/null
+++ b/UAV-ControlSystem/src/drivers/barometer.c
@@ -0,0 +1,383 @@
+/*
+ * barometer.c
+ *
+ *  Created on: 18 okt. 2016
+ *      Author: holmis
+ */
+
+#include "drivers/barometer.h"
+#include "drivers/I2C.h"
+#include "stm32f4xx_revo.h"
+#include "drivers/system_clock.h"
+#include "math.h"
+#include "drivers/i2c_soft.h"
+#include "drivers/failsafe_toggles.h"
+
+#define Device_address_1		0x56
+
+#define ADDR_WRITE 				0xEE // Module address write mode
+#define ADDR_READ 				0xEF // Module address read mode
+#define ADDRESS_BARO			0x77 //0x77
+
+#define CMD_RESET 				0x1E // ADC reset command
+#define CMD_ADC_READ 			0x00 // ADC read command
+#define CMD_ADC_CONV 			0x40 // ADC conversion command
+#define CMD_ADC_D1 				0x00 // ADC D1 conversion
+#define CMD_ADC_D2 				0x10 // ADC D2 conversion
+#define CMD_ADC_256 			0x00 // ADC OSR=256
+#define CMD_ADC_512 			0x02 // ADC OSR=512
+#define CMD_ADC_1024 			0x04 // ADC OSR=1024
+#define CMD_ADC_2048 			0x06 // ADC OSR=2048
+#define CMD_ADC_4096 			0x08 // ADC OSR=4096
+#define CMD_PROM_RD 			0xA0 // Prom read command
+
+#define SEA_PRESS   1013.25                 //default sea level pressure level in mb
+#define FTMETERS    0.3048                  //convert feet to meters
+
+#define CALIBRATION_VAL_AMOUNT	30
+
+I2C_HandleTypeDef baroI2C_handle;
+DMA_HandleTypeDef baroI2C_Rx_DMA_handle;
+DMA_HandleTypeDef baroI2C_Tx_DMA_handle;
+I2C_SOFT_handle_t baroI2C_soft_handle;
+
+uint8_t sampleAmount;
+
+double baro_Preassure;                       // compensated pressure value (mB)
+double baro_Temperature;                       // compensated temperature value (degC)
+double baro_Altitude;                       // altitude (ft)
+double baro_S;                       		// sea level barometer (mB)
+
+float altitudeCalibrationValue = 0;			//Value used as calibration value
+float calibrationSamples[CALIBRATION_VAL_AMOUNT]; //array of stored values to be used for calibration, only samples calibration values when machine is not armed
+int calibrationSamplesCount = 0;
+int calibrationSamplesIterator = 0;
+
+//TODO: remove when not used for testing any more
+uint32_t tempTestCounterStart = 0;
+
+uint8_t cobuf[3] = {0};
+
+/* address: 0 = factory data and the setup
+ * address: 1-6 = calibration coefficients
+ * address: 7 = serial code and CRC */
+uint32_t coefficients_arr[8];                  //coefficient storage
+
+void barometer_addCalibrationSample()
+{
+	//fisrt check if the amount of samples is greater than the array
+	if (!(calibrationSamplesCount >= CALIBRATION_VAL_AMOUNT))
+		calibrationSamplesCount++;	//if not increase the counter
+
+	//Check if the iterator should restart from the beginning because of overflow
+	if (calibrationSamplesIterator >= CALIBRATION_VAL_AMOUNT)
+		calibrationSamplesIterator = 0;	//if it is set it to zero
+
+	//Add the lates calculated altitude value to the samples
+	calibrationSamples[calibrationSamplesIterator] = baro_Altitude;
+
+	//increase the iterator
+	calibrationSamplesIterator ++;
+}
+
+bool barometer_Calibrate()
+{
+	//Check if any calibration values exist
+	if (calibrationSamplesCount <= 0)
+		return false;
+
+	float sampled = 0;
+
+	//loop through all the calibration samples
+	for (int i = 0; i < calibrationSamplesCount; i++)
+	{
+		sampled += calibrationSamples[i];
+	}
+
+	//calculate the new calibration value based on the average of all the samples
+	altitudeCalibrationValue = sampled / calibrationSamplesCount;
+
+	//rest all the values associated with the calibration samples
+	calibrationSamplesCount = 0;
+	calibrationSamplesIterator = 0;
+
+	//Set the calibration flag to true
+	flags_Set_ID(systemFlags_barometerIsCalibrated_id);
+
+	//Calibration went ok
+	return true;
+}
+
+bool barometer_init()
+{
+	//Set the sample rate of the data that will be calculated on the barometer peripheral
+	sampleAmount = CMD_ADC_2048;
+
+	//Initialize pins for SCL and SDA
+#ifdef BARO_USE_I2C_SOFT
+	i2c_soft_Init(I2C1, &baroI2C_soft_handle);
+#endif
+#ifdef BARO_USE_I2C_HARD
+
+#endif
+
+	return true;
+}
+
+bool barometer_reset()
+{
+	/* Send a reset command to the baromter
+	 * Afterwards read in all the coefficient values that are stored on the PROM of the barometer */
+
+#ifdef BARO_USE_I2C_SOFT
+	i2c_soft_Write(&baroI2C_soft_handle, ADDRESS_BARO, CMD_RESET, 1);
+	HAL_Delay(2800);
+
+	for (int i = 0; i < 8; i++)
+	{
+		uint8_t rxbuf[2] = { 0, 0 };
+		i2c_soft_Read(&baroI2C_soft_handle, ADDRESS_BARO, CMD_PROM_RD + i * 2, 2, rxbuf); // send PROM READ command
+		coefficients_arr[i] = rxbuf[0] << 8 | rxbuf[1];
+	}
+#endif
+
+#ifdef BARO_USE_I2C_HARD
+	uint8_t cobuf2[3] = {0};
+		/* Change to hardware polling mode */
+	cobuf2[0] = CMD_ADC_CONV + (CMD_ADC_D2 + sampleAmount);
+	HAL_GPIO_DeInit(I2C1_PORT, I2C1_SCL_PIN);
+	HAL_GPIO_DeInit(I2C1_PORT, I2C1_SDA_PIN);
+	i2c_configure(I2C1, &baroI2C_handle, 0x0);
+	bool isSent = i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf2, 1);
+	HAL_Delay(20);
+
+	cobuf2[0] = CMD_ADC_READ;
+	i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf2, 1);
+	i2c_receive(&baroI2C_handle, ADDRESS_BARO, cobuf2, 3);
+
+
+
+		/* Hardware DMA test */
+	//	cobuf2[0] = CMD_ADC_CONV + (CMD_ADC_D2 + sampleAmount);
+	//	HAL_GPIO_DeInit(I2C1_PORT, I2C1_SCL_PIN);
+	//	HAL_GPIO_DeInit(I2C1_PORT, I2C1_SDA_PIN);
+	//	i2c_configure_DMA(I2C1, &baroI2C_handle, &baroI2C_Rx_DMA_handle, &baroI2C_Tx_DMA_handle, 0x0);
+	//	bool isSent2 = HAL_I2C_Master_Transmit_DMA(&baroI2C_handle, ADDRESS_BARO, cobuf2, 1);
+	//	//bool isSent2 = i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf2, 1);
+	//	HAL_Delay(20);
+	//
+	//
+	//	cobuf2[0] = CMD_ADC_READ;
+	//	isSent2 = HAL_I2C_Master_Transmit_DMA(&baroI2C_handle, ADDRESS_BARO, cobuf2, 1);
+	//	//isSent2 = i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf2, 1);
+	//
+	//	HAL_Delay(20);
+	//	HAL_I2C_Master_Receive_DMA(&baroI2C_handle, ADDRESS_BARO, cobuf2, 3);
+	//	//i2c_receive(&baroI2C_handle, ADDRESS_BARO, cobuf2, 3);
+	//	while(HAL_DMA_GetState(&baroI2C_handle) != HAL_DMA_STATE_READY )
+	//	{
+	//
+	//	}
+	//	HAL_Delay(20);
+
+#endif
+
+
+	/* Produce an initial calibration value */
+	/* Run loop 5 times since there are 5 state, also need delay to ensure values will be read */
+	for (int i = 0; i < 5; i ++)
+	{
+		barometer_CaclulateValues();
+		HAL_Delay(10);
+	}
+
+	/* Set the inital calibration value */
+	barometer_Calibrate();
+	//force bakc the iscalibrated status to false
+	flags_Clear_ID(systemFlags_barometerIsCalibrated_id);
+
+	tempTestCounterStart = clock_get_ms();
+	return true;
+}
+
+typedef enum {
+	CALCSTATE_D2_CALCULATION = 0,	//Tell the sensor that we want to read D2
+	CALCSTATE_D2_READ,				//Read D2 from the sensor
+	CALCSTATE_D1_CALCULATION,		//Tell the sensor that we want to read D1
+	CALCSTATE_D1_READ,				//Read D1 from the sensor
+	CALCSTATE_CALCULATE_PTA			//preassure, temp, altidute calc
+}calculationState;
+
+void barometer_CalculatePTA(uint32_t D1, uint32_t D2)
+{
+	/* calculate dT, difference between actual and reference temp: (D2 - C5 * 2^8) */
+	int64_t dT = D2 - ((uint64_t)coefficients_arr[5] << 8);
+
+	/* Calculate OFF, offset at actual temp: (C2 * 2^16 + (C4 * dT)/2^7) */
+	int64_t OFF = (((uint32_t)coefficients_arr[2]) << 16) + ((coefficients_arr[4] * dT) >> 7);
+
+	/* Calculate SENS, sensitivity at actual temperature: (C1 * 2^15 + (C3 * dT)/2^8) */
+	int64_t SENS = ((uint32_t)coefficients_arr[1] << 15) + ((coefficients_arr[3] * dT) >> 8 );
+
+	/* Calculate TEMP: Actual temperature -40 to 85 C: (2000 + dT * C6/2^23) */
+	int32_t TEMP = 2000 + (int64_t)dT * (int64_t)coefficients_arr[6] / (int64_t)(1 << 23);
+	baro_Temperature = (double)TEMP / 100.0;	//Assign the calculated temp to the holding variable
+
+	/* Improvements for different temperatures */
+	if (TEMP < 2000) //if temp is lower than 20 Celsius
+	{
+		int64_t T1 = ((int64_t)TEMP - 2000) * ((int64_t)TEMP - 2000);
+		int64_t OFF1  = (5 * T1) >> 1;
+		int64_t SENS1 = (5 * T1) >> 2;
+		if(TEMP < -1500) //if temp is lower than -15
+		{
+			T1 = ((int64_t)TEMP + 1500) * ((int64_t)TEMP + 1500);
+			OFF1  +=  7 * T1;
+			SENS1 += 11 * T1 >> 1;
+		}
+		OFF -= OFF1;
+		SENS -= SENS1;
+	}
+	/* Calculate pressure, temperature compensated pressure 10..1200mbar: ( (D1 * SENS/2^21 - OFF)2^15 ) */
+	baro_Preassure = (double)(((((int64_t)D1 * SENS ) >> 21) - OFF) / (double) (1 << 15)) / 100.0;
+
+	/* Calculate the altitude */
+	float feet = ((float)1 - (pow(((float)baro_Preassure / (float)SEA_PRESS), (float)0.190284))) * (float)145366.45;
+
+	baro_Altitude = (flags_IsSet_ID(systemFlags_barometerIsCalibrated_id)) ? (feet * FTMETERS) - altitudeCalibrationValue : (feet * FTMETERS);
+}
+
+void barometer_CaclulateValues()
+{
+	/*the calculation is in need of different states. This is because the
+	 * a wait time is needed when talking to the sensor. Because we cant
+	 * use a delay wait we need to do parts of the calculation every time
+	 * the function is called. The "delay" is handled by the period of
+	 * the task that handles the calculation. It cant have a period faster
+	 * that 10 ms, or the wait will not be enough in some cases according
+	 * to the datasheet of the sensor http://www.amsys.info/sheets/amsys.en.ms5611_01ba03.pdf*/
+	static uint8_t currentCalculationState = CALCSTATE_D2_CALCULATION;
+	static uint32_t D1 = 0;
+	static uint32_t D2 = 0;
+	uint8_t cobuf[3] = {0};
+	uint32_t startTime;
+	uint32_t endTime;
+
+
+	//If the machine is armed and not calibrated we perform a calibraton
+	if (!flags_IsSet_ID(systemFlags_barometerIsCalibrated_id))
+	{
+		if (flags_IsSet_ID(systemFlags_armed_id))
+		{
+			barometer_Calibrate();
+		}
+	}
+
+
+
+	switch (currentCalculationState)
+	{
+		case CALCSTATE_D2_CALCULATION:
+			//Set the message to be sent to the barometer
+			cobuf[0] = CMD_ADC_CONV + (CMD_ADC_D2 + sampleAmount);
+
+			//Send the message to the barometer
+			#ifdef BARO_USE_I2C_SOFT
+			i2c_soft_Write(&baroI2C_soft_handle, ADDRESS_BARO, CMD_ADC_CONV + (CMD_ADC_D2 + sampleAmount),1); // send conversion command
+			#endif
+			#ifdef BARO_USE_I2C_HARD
+			i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf, 1);
+			#endif
+
+			//change the state so we will go to D2 read next time function is called
+			currentCalculationState = CALCSTATE_D2_READ;
+			break;
+		case CALCSTATE_D2_READ:
+			//Set the message to be sent to the barometer
+			cobuf[0] = CMD_ADC_READ;
+
+			//Read the message from the barometer
+			#ifdef BARO_USE_I2C_SOFT
+			i2c_soft_Read(&baroI2C_soft_handle, ADDRESS_BARO, CMD_ADC_READ, 3, cobuf); // send PROM READ command
+			#endif
+			#ifdef BARO_USE_I2C_HARD
+			i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf, 1);
+			i2c_receive(&baroI2C_handle, ADDRESS_BARO, cobuf, 3);
+			#endif
+
+			//Shift the values to the correct position for the 24 bit D2 value
+			D2 = (cobuf[0] << 16) + (cobuf[1] << 8) + cobuf[2];
+
+			//change the state so we will go to D2 read next time function is called
+			currentCalculationState = CALCSTATE_D1_CALCULATION;
+			break;
+		case CALCSTATE_D1_CALCULATION:
+			//Set the message to be sent to the barometer
+			cobuf[0] = CMD_ADC_CONV + (CMD_ADC_D1 + sampleAmount);
+
+			//Send the message to the barometer
+			#ifdef BARO_USE_I2C_SOFT
+			i2c_soft_Write(&baroI2C_soft_handle, ADDRESS_BARO, CMD_ADC_CONV + (CMD_ADC_D1 + sampleAmount),1); // send conversion command
+			#endif
+			#ifdef BARO_USE_I2C_HARD
+			i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf, 1);
+			#endif
+
+			//change the state so we will go to D1 read next time function is called
+			currentCalculationState = CALCSTATE_D1_READ;
+			break;
+		case CALCSTATE_D1_READ:
+			//Set the message to be sent to the barometer
+			cobuf[0] = CMD_ADC_READ;
+
+			//Read the message from the baromter
+			#ifdef BARO_USE_I2C_SOFT
+			i2c_soft_Read(&baroI2C_soft_handle, ADDRESS_BARO, CMD_ADC_READ, 3, cobuf); // send PROM READ command
+			#endif
+			#ifdef BARO_USE_I2C_HARD
+			i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf, 1);
+			i2c_receive(&baroI2C_handle, ADDRESS_BARO, cobuf, 3);
+			#endif
+
+			//Shift the values to the correct position for the 24 bit D2 value
+			D1 = (cobuf[0] << 16) + (cobuf[1] << 8) + cobuf[2];
+
+			//Change the state for the next time the function is called
+			currentCalculationState = CALCSTATE_CALCULATE_PTA;
+			break;
+		case CALCSTATE_CALCULATE_PTA:
+			startTime = clock_get_us();
+			//Calculate the Pressure, temperature and altitude
+			barometer_CalculatePTA(D1, D2);
+
+			//only calculate new calibration values if we are not armed
+			if (!flags_IsSet_ID(systemFlags_armed_id))
+			{
+				barometer_addCalibrationSample();	//add new calibration value
+				flags_Clear_ID(systemFlags_barometerIsCalibrated_id);	//Clear the flag for barometer calibration
+			}
+
+			endTime = clock_get_us() - startTime;
+			//Change the state
+			currentCalculationState = CALCSTATE_D2_CALCULATION;
+			break;
+	}
+
+}
+
+double barometer_GetCurrentPreassure()
+{
+	return baro_Preassure;
+}
+
+double barometer_GetCurrentTemperature()
+{
+	return baro_Temperature;
+}
+
+float barometer_GetCurrentAltitudeBasedOnSeaLevel()
+{
+	return baro_Altitude;
+}
+
+
diff --git a/UAV-ControlSystem/src/drivers/failsafe_toggles.c b/UAV-ControlSystem/src/drivers/failsafe_toggles.c
index 3101c34..58b00ce 100644
--- a/UAV-ControlSystem/src/drivers/failsafe_toggles.c
+++ b/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,
-			.maxRange = 2000,
-			.channelNumber = 5,
+			.minRange = 1400,
+			.maxRange = 2100,
+			.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,
-			.maxRange = 0,
-			.channelNumber = 0,
+			.minRange = 1900,
+			.maxRange = 2100,
+			.channelNumber = 5,
 			.flagId = systemFlags_mixerfullscale_id,
 		},
 		[FLAG_CONFIGURATION_MIXERLOWSCALE] = {
-			.minRange = 0,
-			.maxRange = 0,
-			.channelNumber = 0,
+			.minRange = 900,
+			.maxRange = 1100,
+			.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);
diff --git a/UAV-ControlSystem/src/drivers/i2c_soft.c b/UAV-ControlSystem/src/drivers/i2c_soft.c
new file mode 100644
index 0000000..34bf083
--- /dev/null
+++ b/UAV-ControlSystem/src/drivers/i2c_soft.c
@@ -0,0 +1,241 @@
+/*
+ * i2c_soft.c
+ *
+ *  Created on: 27 okt. 2016
+ *      Author: holmis
+ */
+
+#include "drivers/i2c_soft.h"
+#include "stm32f4xx_revo.h"
+
+#define WRITE_INDICATOR					0
+#define READ_INDICATOR					1
+
+static void IOHi(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
+{
+	HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_SET);
+}
+
+static void IOLo(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
+{
+	HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_RESET);
+}
+
+static bool IORead(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
+{
+	return !! (GPIOx->IDR & GPIO_Pin);
+}
+
+static void i2c_soft_delay(void)
+{
+    volatile int i = 1;
+    while (i) {
+        i--;
+    }
+}
+
+void i2c_soft_Init(I2C_TypeDef *i2c, I2C_SOFT_handle_t *out_profile)
+{
+	uint16_t sda_pin, scl_pin;
+	GPIO_TypeDef *i2c_port;
+
+	//Check what i2c should be used
+	if(i2c == I2C1)
+	{
+		i2c_port = I2C1_PORT;
+		sda_pin = I2C1_SDA_PIN;
+		scl_pin = I2C1_SCL_PIN;
+	}
+	else if(i2c == I2C2)
+	{
+		i2c_port = I2C2_PORT;
+		sda_pin = I2C2_SDA_PIN;
+		scl_pin = I2C2_SCL_PIN;
+	}
+
+	//Init the GPIO pins
+	GPIO_InitTypeDef GPIO_InitStruct;
+	GPIO_InitStruct.Pin = scl_pin | sda_pin;
+	GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;
+	GPIO_InitStruct.Pull = GPIO_NOPULL;
+	GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
+	HAL_GPIO_Init(i2c_port, &GPIO_InitStruct);
+
+	//Assign the values to the out struct
+	out_profile->i2c_Port = i2c_port;
+	out_profile->i2c_scl_pin = scl_pin;
+	out_profile->i2c_sda_pin = sda_pin;
+}
+
+static bool i2c_soft_Start(I2C_SOFT_handle_t *handle)
+{
+	IOHi(handle->i2c_Port, handle->i2c_sda_pin);
+	IOHi(handle->i2c_Port, handle->i2c_scl_pin);
+    asm ("nop"); // i2c_soft_delay();
+    if (!IORead(handle->i2c_Port, handle->i2c_sda_pin)) {
+        return false;
+    }
+    IOLo(handle->i2c_Port, handle->i2c_sda_pin);
+    asm ("nop"); // i2c_soft_delay();
+    if (IORead(handle->i2c_Port, handle->i2c_sda_pin)) {
+        return false;
+    }
+    IOLo(handle->i2c_Port, handle->i2c_sda_pin);
+    asm ("nop"); // i2c_soft_delay();
+    return true;
+}
+
+static void i2c_soft_Stop(I2C_SOFT_handle_t *handle)
+{
+	IOLo(handle->i2c_Port, handle->i2c_scl_pin);
+	asm ("nop"); // i2c_soft_delay();
+    IOLo(handle->i2c_Port, handle->i2c_sda_pin);
+    asm ("nop"); // i2c_soft_delay();
+    IOHi(handle->i2c_Port, handle->i2c_scl_pin);
+    asm ("nop"); // i2c_soft_delay();
+    IOHi(handle->i2c_Port, handle->i2c_sda_pin);
+    asm ("nop"); // i2c_soft_delay();
+}
+
+static void i2c_soft_Ack(I2C_SOFT_handle_t *handle)
+{
+	IOLo(handle->i2c_Port, handle->i2c_scl_pin);
+	asm ("nop"); // i2c_soft_delay();
+    IOLo(handle->i2c_Port, handle->i2c_sda_pin);
+    asm ("nop"); // i2c_soft_delay();
+    IOHi(handle->i2c_Port, handle->i2c_scl_pin);
+    asm ("nop"); // i2c_soft_delay();
+    IOLo(handle->i2c_Port, handle->i2c_scl_pin);
+    asm ("nop"); // i2c_soft_delay();
+}
+
+static void i2c_soft_NoAck(I2C_SOFT_handle_t *handle)
+{
+	IOLo(handle->i2c_Port, handle->i2c_scl_pin);
+    asm ("nop"); // i2c_soft_delay();
+    IOHi(handle->i2c_Port, handle->i2c_sda_pin);
+    asm ("nop"); // i2c_soft_delay();
+    IOHi(handle->i2c_Port, handle->i2c_scl_pin);
+    asm ("nop"); // i2c_soft_delay();
+    IOLo(handle->i2c_Port, handle->i2c_scl_pin);
+    asm ("nop"); // i2c_soft_delay();
+}
+
+static bool i2c_soft_WaitAck(I2C_SOFT_handle_t *handle)
+{
+	IOLo(handle->i2c_Port, handle->i2c_scl_pin);
+	asm ("nop"); // i2c_soft_delay();
+    IOHi(handle->i2c_Port, handle->i2c_sda_pin);
+    asm ("nop"); // i2c_soft_delay();
+    IOHi(handle->i2c_Port, handle->i2c_scl_pin);
+    asm ("nop"); // i2c_soft_delay();
+    if (IORead(handle->i2c_Port, handle->i2c_sda_pin)) {
+    	IOLo(handle->i2c_Port, handle->i2c_scl_pin);
+        return false;
+    }
+    IOLo(handle->i2c_Port, handle->i2c_scl_pin);
+    return true;
+}
+
+static void i2c_soft_SendByte(I2C_SOFT_handle_t *handle, uint8_t byte)
+{
+    uint8_t i = 8;
+    while (i--) {
+    	IOLo(handle->i2c_Port, handle->i2c_scl_pin);
+    	asm ("nop"); // i2c_soft_delay();
+        if (byte & 0x80) {
+        	IOHi(handle->i2c_Port, handle->i2c_sda_pin);
+        }
+        else {
+        	IOLo(handle->i2c_Port, handle->i2c_sda_pin);
+        }
+        byte <<= 1;
+        asm ("nop"); // i2c_soft_delay();
+        IOHi(handle->i2c_Port, handle->i2c_scl_pin);
+        asm ("nop"); // i2c_soft_delay();
+    }
+    IOLo(handle->i2c_Port, handle->i2c_scl_pin);
+}
+
+static uint8_t i2c_soft_ReceiveByte(I2C_SOFT_handle_t *handle)
+{
+    uint8_t i = 8;
+    uint8_t byte = 0;
+
+    IOHi(handle->i2c_Port, handle->i2c_sda_pin);
+    while (i--) {
+        byte <<= 1;
+        IOLo(handle->i2c_Port, handle->i2c_scl_pin);
+        asm ("nop"); // i2c_soft_delay();
+        IOHi(handle->i2c_Port, handle->i2c_scl_pin);
+        asm ("nop"); // i2c_soft_delay();
+        if (IORead(handle->i2c_Port, handle->i2c_sda_pin)) {
+            byte |= 0x01;
+        }
+    }
+    IOLo(handle->i2c_Port, handle->i2c_scl_pin);
+    return byte;
+}
+
+bool i2c_soft_Read(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf)
+{
+	//just send the addres 0x77
+		//write = 0, read = 1
+
+    if (!i2c_soft_Start(handle)) {
+        return false;
+    }
+
+    i2c_soft_SendByte(handle, addr << 1 | 0);
+    if (!i2c_soft_WaitAck(handle)) {
+        i2c_soft_Stop(handle);
+        //i2cErrorCount++;
+        return false;
+    }
+    i2c_soft_SendByte(handle, reg);
+    i2c_soft_WaitAck(handle);
+    i2c_soft_Start(handle);
+    i2c_soft_SendByte(handle, addr << 1 | 1);
+    i2c_soft_WaitAck(handle);
+    while (len) {
+        *buf = i2c_soft_ReceiveByte(handle);
+        if (len == 1) {
+        	i2c_soft_NoAck(handle);
+        }
+        else {
+        	i2c_soft_Ack(handle);
+        }
+        buf++;
+        len--;
+    }
+    i2c_soft_Stop(handle);
+    return true;
+}
+
+bool i2c_soft_Write(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t data)
+{
+	//just send the addres 0x77
+	//write = 0, read = 1
+
+	//Start the i2c, if it cant return
+	if (!i2c_soft_Start(handle)) {
+	        return false;
+	    }
+
+	//Send the address
+	i2c_soft_SendByte(handle, addr << 1 | WRITE_INDICATOR);
+    if (!i2c_soft_WaitAck(handle)) {
+        i2c_soft_Stop(handle);
+       // i2cErrorCount++;
+        return false;
+    }
+
+    //send the data
+    i2c_soft_SendByte(handle, reg);
+    i2c_soft_WaitAck(handle);
+    i2c_soft_SendByte(handle, data);
+    i2c_soft_WaitAck(handle);
+    i2c_soft_Stop(handle);
+    return true;
+}
+
diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c
index 22624cf..0def06d 100644
--- a/UAV-ControlSystem/src/drivers/motormix.c
+++ b/UAV-ControlSystem/src/drivers/motormix.c
@@ -42,17 +42,20 @@
 /* An array containing the calculated motor outputs */
 uint16_t motor_output[MOTOR_COUNT];
 
+/* Bool to see if motors are maxed out. Stops windup in PID implementation */
+bool motorLimitReached = false;
+
 /* 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 */
@@ -102,8 +105,6 @@ void mix()
 	int16_t RPY_Mix_Max = 0;			// Maximum desired command for any motor
 	int16_t throttle = PidProfile[PID_ID_BAROMETER].PID_Out[THROTTLE];	 // Import throttle value from remote
 
-	// Might be used for some debug if necessary
-	//static bool motorLimitReached;
 
 	/* Mixer Full Scale enabled */
 	if (flags_IsSet_ID(systemFlags_mixerfullscale_id))
@@ -114,8 +115,8 @@ void mix()
 			 * Calculation is: Output from control system * weight from model for each motor
 			 */
 			RPY_Mix[i] = \
-					PidProfile[PID_ID_GYRO].PID_Out[ROLL] 	*	mixerUAV[i].roll 	+ 	\
-					PidProfile[PID_ID_GYRO].PID_Out[PITCH] 	* 	mixerUAV[i].pitch	+ 	\
+					(int)PidProfile[PID_ID_GYRO].PID_Out[ROLL] 	*	mixerUAV[i].roll 	+ 	\
+					(int)PidProfile[PID_ID_GYRO].PID_Out[PITCH] 	* 	mixerUAV[i].pitch	+ 	\
 					PidProfile[PID_ID_GYRO].PID_Out[YAW]	*	mixerUAV[i].yaw		*	((mixerConfig.yaw_reverse_direction) ? -1 : 1);
 
 
@@ -194,8 +195,8 @@ void mix()
 		{
 			RPY_Mix[i] = \
 					throttle 								*	mixerUAV[i].throttle	+	\
-					PidProfile[PID_ID_GYRO].PID_Out[ROLL] 	*	mixerUAV[i].roll 	+ 	\
-					PidProfile[PID_ID_GYRO].PID_Out[PITCH] 	* 	mixerUAV[i].pitch	+ 	\
+					(int)PidProfile[PID_ID_GYRO].PID_Out[ROLL] 	*	mixerUAV[i].roll 	+ 	\
+					(int)PidProfile[PID_ID_GYRO].PID_Out[PITCH] 	* 	mixerUAV[i].pitch	+ 	\
 					PidProfile[PID_ID_GYRO].PID_Out[YAW]	*	mixerUAV[i].yaw		*	((mixerConfig.yaw_reverse_direction) ? -1 : 1);
 		}
 
diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c
index 7b5e199..18fbba9 100644
--- a/UAV-ControlSystem/src/main.c
+++ b/UAV-ControlSystem/src/main.c
@@ -28,8 +28,7 @@
 #include "drivers/motormix.h"
 #include "drivers/motors.h"
 #include "Flight/pid.h"
-#include "drivers/arduino_com.h"
-
+#include "drivers/barometer.h"#include "drivers/arduino_com.h"
 
 /**************************************************************************
 * BRIEF: Should contain all the initializations of the system, needs to
@@ -47,20 +46,25 @@ void init_system()
 	//Configure the clock
 	system_clock_config();
 
+	//init motors to run with oneshot 125, small delay
+	HAL_Delay(1000);
+	pwmEnableAllMotors(Oneshot125);
+
+	//Initializes all the pids that are used in the system. This part will also init the gyro and accelerometer.
 	pidInit();
 
 	/* read saved variables from eeprom, in most cases eeprom should be read after a lot of the initializes  */
 	readEEPROM();
 
+	pidEproom();
+
 	//initialize the CLI NOTE: Cant use the same usart as anything else or there will be some big trouble
 	cliInit(USART3);
 
 	//init sbus, using USART1
 	sbus_init();
 
-	//init motors to run with oneshot 125, small delay
-	HAL_Delay(1000);
-	pwmEnableAllMotors(Oneshot125);
+
 
 
 #ifdef USE_LEDS
@@ -72,7 +76,8 @@ void init_system()
 #endif
 
 #ifdef BARO
-
+	//barometer_init();
+	//barometer_reset();
 #endif
 
 #ifdef COMPASS
@@ -95,6 +100,8 @@ void init_system()
 
 #endif
 
+
+
 }
 
 /**************************************************************************
@@ -110,6 +117,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();
 
diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c
index 64b57ad..4392405 100644
--- a/UAV-ControlSystem/src/tasks_main.c
+++ b/UAV-ControlSystem/src/tasks_main.c
@@ -41,6 +41,7 @@
 #include "drivers/accel_gyro.h"
 #include "drivers/motormix.h"
 #include "Flight/pid.h"
+#include "drivers/barometer.h"
 
 void systemTaskGyroPid(void)
 {
@@ -91,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;
@@ -161,29 +165,68 @@ bool systemTaskRxCliCheck(uint32_t currentDeltaTime)
 
 void systemTaskSerial(void)
 {
-//	uint8_t c = 118;
-//	usart_transmit(&cliUsart, &c, 1, 1000000000);
-	if (flags_IsSet_ID(systemFlags_armed_id))
-		ledOnInverted(Led0_PIN, Led0_GPIO_PORT);
-	else
-		ledOffInverted(Led0_PIN, Led0_GPIO_PORT);
+	static bool readyToCalibrate = true;
+	const float calibrationAmount = 0.5;
+
+	//Only run if the system is not armed
+	if (!flags_IsSet_ID(systemFlags_armed_id))
+	{
+		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 = 98;
-//	usart_transmit(&cliUsart, &c, 1, 1000000000);
-	if (flags_IsSet_MASK((systemFlags_flightmode_acceleromter_mask | systemFlags_armed_mask)))
-		ledOnInverted(Led1, Led1_GPIO_PORT);
+	//	uint8_t c = 118;
+	//	usart_transmit(&cliUsart, &c, 1, 1000000000);
+	if (flags_IsSet_ID(systemFlags_armed_id))
+		ledOnInverted(Led0_PIN, Led0_GPIO_PORT);
 	else
-		ledOffInverted(Led1, Led1_GPIO_PORT);
+		ledOffInverted(Led0_PIN, Led0_GPIO_PORT);
 }
 
 void systemTaskBaro(void)
 {
-	pidRun(PID_ID_BAROMETER);
+	//barometer_CaclulateValues();
 }
 
 void systemTaskCompass(void)
@@ -204,6 +247,13 @@ void systemTaskSonar(void)
 void systemTaskAltitude(void)
 {
 	//Keep track of the vehicles current altitude, based on some sensor. In this case either barometer or sonar
+
+	//double temperature = barometer_GetCurrentTemperature();
+	//double pressure = barometer_GetCurrentPreassure();
+	//float altitute = barometer_GetCurrentAltitudeBasedOnSeaLevel();
+
+	//pid run, should probably be moved to systemTaskAltitude
+	pidRun(PID_ID_BAROMETER);
 }
 
 void systemTaskBeeper(void)
diff --git a/revolution_hal_lib/HAL_Driver/Src/stm32f4xx_hal_i2c.c b/revolution_hal_lib/HAL_Driver/Src/stm32f4xx_hal_i2c.c
index 008158e..477c3e6 100644
--- a/revolution_hal_lib/HAL_Driver/Src/stm32f4xx_hal_i2c.c
+++ b/revolution_hal_lib/HAL_Driver/Src/stm32f4xx_hal_i2c.c
@@ -376,6 +376,8 @@ HAL_StatusTypeDef HAL_I2C_Init(I2C_HandleTypeDef *hi2c)
 
   /* Get PCLK1 frequency */
   pclk1 = HAL_RCC_GetPCLK1Freq();
+  //this is temp test
+ // pclk1 = 50000000;
 
   /* Calculate frequency range */
   freqrange = I2C_FREQRANGE(pclk1);