Merge pull request #17 from MDHSweden/PID

Pid fixed conflicts approved by Lennart

UAV-ControlSystem/inc/Flight/pid.h

@@ -31,9 +31,16 @@
 #define PITCH 1								/*Index terms to the PID*/
 #define YAW 2								/*Index terms to the PID*/
 
+#define PID_ID_GYRO 10
+#define PID_ID_ACCELEROMETER 11
+#define PID_ID_COMPASS 12
+#define PID_ID_BAROMETER 13
+
+
 /*Struct that belongs to a certain PID controller*/
 typedef struct pidProfile_s {
 
+	bool pidEnabled;
 	bool pidStabilisationEnabled;			/*Enables/Dissables PID controller*/
 
 	uint8_t ID_profile;						/*ID of a certain PID, shall be referenced to a certain sensor*/
@@ -63,6 +70,10 @@ typedef struct pidProfile_s {
 
 } pidProfile_t;
 
+
+extern pidProfile_t PidGyroProfile, PidAccelerometerProfile; /*Global variables to certain PID profiles*/
+extern pidProfile_t PidCompassProfile, PidBarometerProfile;
+
 /**************************************************************************
 * BRIEF: Constrain float values within a defined limit                    *
 * INFORMATION: Used in PID loop to limit values                           *
@@ -81,11 +92,17 @@ void getCurrentValues(float *sensorValues, uint8_t ID_profile);
 **************************************************************************/
 void getPointRate(float *desiredCommand, uint8_t ID_profile);
 
+/**************************************************************************
+* BRIEF: Initializes PID profiles			 					          *
+* INFORMATION: 															  *
+**************************************************************************/
+void pidInit();
+
 /**************************************************************************
 * BRIEF: Initializes the pid profile PID controller				          *
 * INFORMATION: Recommended to use if unexpected values occur of profile	  *
 **************************************************************************/
-void pidUAVInit(pidProfile_t *pidProfile);
+void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID);
 
 /**************************************************************************
 * BRIEF: Dynamic PID controller, able to handle several PID controller    *
@@ -94,4 +111,10 @@ void pidUAVInit(pidProfile_t *pidProfile);
 **************************************************************************/
 void pidUAV(pidProfile_t *pidProfile);
 
+/**************************************************************************
+* BRIEF: Runs a certain PID Controller									  *
+* INFORMATION:                       									  *
+**************************************************************************/
+void pidRun(uint8_t ID);
+
 #endif /* FLIGHT_PID_H_ */

UAV-ControlSystem/inc/config/eeprom.h

@@ -197,7 +197,22 @@ typedef enum {
 
 /* List of all profile EEPROM values */
 typedef enum {
-	EEPROM_PID_ROLL_KP = 0,
+	EEPROM_PID_GYRO_P_ROLL,
+	EEPROM_PID_GYRO_P_PITCH,
+	EEPROM_PID_GYRO_P_YAW,
+	EEPROM_PID_GYRO_I_ROLL,
+	EEPROM_PID_GYRO_I_PITCH,
+	EEPROM_PID_GYRO_I_YAW,
+	EEPROM_PID_GYRO_D_ROLL,
+	EEPROM_PID_GYRO_D_PITCH,
+	EEPROM_PID_GYRO_D_YAW,
+
+	EEPROM_PID_ACCEL_P_ROLL,
+	EEPROM_PID_ACCEL_P_PITCH,
+	EEPROM_PID_ACCEL_I_ROLL,
+	EEPROM_PID_ACCEL_I_PITCH,
+	EEPROM_PID_ACCEL_D_ROLL,
+	EEPROM_PID_ACCEL_D_PITCH,
 
 	/* Counts the amount of settings in profile */
 	EEPROM_PROFILE_COUNT

UAV-ControlSystem/inc/drivers/sbus.h

@@ -18,6 +18,7 @@
  **********************************************************************/
 #include <stdint.h>
 
+
 #ifndef DRIVERS_SBUS_H_
 #define DRIVERS_SBUS_H_
 
@@ -41,6 +42,7 @@
 #define PWM_PULSE_MIN 750	// minimum PWM pulse considered valid input
 #define PWM_PULSE_MAX 2250	// maximum PWM pulse considered valid input
 
+extern float rc_rate;
 
 /***********************************************************************
 * BRIEF: Failsafe scenarios of the RX                                  *
@@ -107,9 +109,22 @@ typedef struct sbusFrame_s {
 
 } __attribute__ ((__packed__)) sbusFrame_s;
 
+/* these are scaled RC Inputs centered around 0 */
+typedef struct rc_input_t {
+	int16_t Roll;
+	int16_t Pitch;
+	int16_t Yaw;
+	int16_t Throttle;
+} rc_input_t;
+
+
+
 /* This instance is read by the whole system and should contain actual RX data */
 extern sbusFrame_s sbusChannelData;
 
+/* these are scaled RC Inputs centered around 0 */
+extern rc_input_t rc_input;
+
 /***********************************************************************
 * BRIEF: Initializes the UART for sbus                                 *
 * INFORMATION: A DMA Buffer starts storing the bytes received from RX  *

UAV-ControlSystem/src/Flight/pid.c

@@ -14,6 +14,21 @@
 * **************************************************************************/
 
 #include "Flight/pid.h"
+#include "drivers/accel_gyro.h"
+#include "drivers/sbus.h"
+#include <math.h>
+
+#define RADIO_RANGE 1000											/*Radio range input*/
+#define BAROMETER_RANGE 2000										/*Determines the range of the maximum height (limits the rc input)*/
+#define ACCELEROMETER_RANGE 90										/*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 360											/*Determines the maximum compass limit (limits the rc input)*/
+
+pidProfile_t PidGyroProfile = {0}, PidAccelerometerProfile = {0};	/*Struct profile for each PID controller*/
+pidProfile_t PidCompassProfile = {0}, PidBarometerProfile = {0};	/*Struct profile for each PID controller*/
+
+accel_t accelProfile;												/*Struct profile for input data from sensor*/
+gyro_t gyroProfile;													/*Struct profile for input data from sensor*/
 
 /**************************************************************************
 * BRIEF: Constrain float values within a defined limit                    *
@@ -35,29 +50,36 @@ float constrainf(float amt, int low, int high)
 **************************************************************************/
 void getCurrentValues(float *sensorValues, uint8_t ID_profile)
 {
-	//*Do something smart*//
-
 	switch (ID_profile)
 	{
-	case 1:
+	case PID_ID_GYRO:
-		sensorValues[ROLL] = 0;
+		mpu6000_read_gyro(&gyroProfile);
-		sensorValues[PITCH] = 0;
+
-		sensorValues[YAW] = 0;
+		sensorValues[ROLL] = gyroProfile.gyroY;
+		sensorValues[PITCH] = gyroProfile.gyroX;
+		sensorValues[YAW] = gyroProfile.gyroZ;
 		break;
-	case 2:
+	case PID_ID_ACCELEROMETER:
+		mpu6000_read_accel(&accelProfile);
+
+		/*May need Low pass filter since the accelerometer may drift*/
+		sensorValues[ROLL] = atan2(accelProfile.accelXconv,accelProfile.accelZconv)*180*M_PI;//acos(accelProfile.accelXconv/R)*180/M_PI;
+		sensorValues[PITCH] = atan2(-accelProfile.accelXconv, sqrt(accelProfile.accelYconv*accelProfile.accelYconv + accelProfile.accelZconv*accelProfile.accelZconv))*180/M_PI;
 		break;
-	case 3:
+	case PID_ID_COMPASS:
 		break;
-	case 4:
+	case PID_ID_BAROMETER:
-		break;
-	case 5:
 		break;
 	default:
 		break;
 	}
-
 };
 
+float convertData(int inputRange, int outputRange, int offset, float value)
+{
+	return (outputRange/inputRange)*(value-offset);
+}
+
 /**************************************************************************
 * BRIEF: Update desired values from rc command		                      *
 * INFORMATION:                       									  *
@@ -65,35 +87,58 @@ void getCurrentValues(float *sensorValues, uint8_t ID_profile)
 void getPointRate(float *desiredCommand, uint8_t ID_profile)
 {
 	//*Do something smart*//
-	switch (ID_profile)
+	switch(ID_profile)
 	{
-	case 1:
+	case PID_ID_GYRO:
-		desiredCommand[ROLL] = 0;
+
-		desiredCommand[PITCH] = 0;
+		if (!PidAccelerometerProfile.pidEnabled)
-		desiredCommand[YAW] = 0;
+		{
+			desiredCommand[ROLL] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidAccelerometerProfile.PID_Out[ROLL]);
+			desiredCommand[PITCH] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidAccelerometerProfile.PID_Out[PITCH]);
+		}
+		else
+		{
+			desiredCommand[ROLL] = convertData(ACCELEROMETER_RANGE, GYRO_RANGE, 0, PidAccelerometerProfile.PID_Out[ROLL]);
+			desiredCommand[PITCH] = convertData(ACCELEROMETER_RANGE, GYRO_RANGE, 0, PidAccelerometerProfile.PID_Out[PITCH]);
+		}
+
+
+		if (!PidCompassProfile.pidEnabled)
+		{
+			desiredCommand[YAW] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidCompassProfile.PID_Out[0]);
+		}
+		else
+		{
+			desiredCommand[YAW] = convertData(ACCELEROMETER_RANGE, GYRO_RANGE, 0, PidCompassProfile.PID_Out[0]);
+		}
+
 		break;
-	case 2:
+	case PID_ID_ACCELEROMETER:
+		desiredCommand[ROLL] = convertData(RADIO_RANGE, ACCELEROMETER_RANGE, 0, rc_input.Roll);
+		desiredCommand[PITCH] = convertData(RADIO_RANGE, ACCELEROMETER_RANGE, 0, rc_input.Pitch);
 		break;
-	case 3:
+	case PID_ID_COMPASS:
+		desiredCommand[0] = convertData(RADIO_RANGE, COMPASS_RANGE, 0, rc_input.Yaw);	/*desiredCommand must have an index of 0, since its only loop once*/
 		break;
-	case 4:
+	case PID_ID_BAROMETER:
-		break;
+		desiredCommand[THROTTLE] = convertData(RADIO_RANGE,BAROMETER_RANGE,0, rc_input.Throttle);
-	case 5:
 		break;
 	default:
 		break;
 	}
 }
 
+
 /**************************************************************************
 * BRIEF: Initializes the pid profile PID controller				          *
 * INFORMATION: Recommended to use if unexpected values occur of profile	  *
 **************************************************************************/
-void pidUAVInit(pidProfile_t *pidProfile)
+void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
 {
 	/*This is recommended init settings*/
-	pidProfile->ID_profile = 0;
+	pidProfile->ID_profile = ID;
 
+	pidProfile->pidEnabled = true;
 	pidProfile-> pidStabilisationEnabled = true;
 
 	pidProfile->DOF = 0;
@@ -153,6 +198,21 @@ void pidUAVInit(pidProfile_t *pidProfile)
 
 }
 
+/**************************************************************************
+* BRIEF: Initializes PID profiles			 					          *
+* INFORMATION: 															  *
+**************************************************************************/
+void pidInit()
+{
+
+	mpu6000_init(&gyroProfile,&accelProfile);								/*Init gyro and accelerometer*/
+
+	pidUAVInit(&PidGyroProfile, PID_ID_GYRO);
+	pidUAVInit(&PidAccelerometerProfile, PID_ID_ACCELEROMETER);
+	pidUAVInit(&PidCompassProfile, PID_ID_COMPASS);
+	pidUAVInit(&PidBarometerProfile, PID_ID_BAROMETER);
+}
+
 /**************************************************************************
 * BRIEF: Dynamic PID controller, able to handle several PID controller    *
 * 		 connected to different profiles.
@@ -249,3 +309,68 @@ void pidUAV(pidProfile_t *pidProfile)
 		pidProfile->PID_Out[axis] = PID_Out[axis];
 	}
 }
+
+/**************************************************************************
+* BRIEF: Runs a certain PID Controller									  *
+* INFORMATION:                       									  *
+**************************************************************************/
+void pidRun(uint8_t ID)
+{
+	switch(ID)
+	{
+
+	case PID_ID_GYRO:
+
+		if (!PidGyroProfile.pidEnabled)
+		{
+			PidGyroProfile.PID_Out[ROLL] = rc_input.Roll;
+			PidGyroProfile.PID_Out[PITCH] = rc_input.Pitch;
+			PidGyroProfile.PID_Out[YAW] = rc_input.Yaw;
+		}
+		else
+		{
+			pidUAV(&PidGyroProfile);
+		}
+
+		break;
+	case PID_ID_ACCELEROMETER:
+
+		if (!PidAccelerometerProfile.pidEnabled)
+		{
+			PidAccelerometerProfile.PID_Out[ROLL] = rc_input.Roll;
+			PidAccelerometerProfile.PID_Out[PITCH] = rc_input.Pitch;
+		}
+		else
+		{
+			pidUAV(&PidAccelerometerProfile);
+		}
+
+		break;
+	case PID_ID_COMPASS:
+
+		if (!PidCompassProfile.pidEnabled)
+		{
+			PidCompassProfile.PID_Out[0] = rc_input.Yaw;
+		}
+		else
+		{
+			pidUAV(&PidCompassProfile);
+		}
+
+		break;
+	case PID_ID_BAROMETER:
+
+		if (!PidBarometerProfile.pidEnabled)
+		{
+			PidBarometerProfile.PID_Out[0] = rc_input.Throttle;
+		}
+		else
+		{
+			pidUAV(&PidBarometerProfile);
+		}
+
+		break;
+	default:
+		break;
+	}
+}

UAV-ControlSystem/src/config/cli.c

@@ -230,6 +230,32 @@ typedef enum
 	COMMAND_ID_FLAG_FLIGHTMODE_3_CHANNEL,
 
 
+	/* Profile values */
+
+	/* PID value */
+	//GYRO
+	COMMAND_ID_PID_GYRO_P_ROLL,
+	COMMAND_ID_PID_GYRO_P_PITCH,
+	COMMAND_ID_PID_GYRO_P_YAW,
+
+	COMMAND_ID_PID_GYRO_I_ROLL,
+	COMMAND_ID_PID_GYRO_I_PITCH,
+	COMMAND_ID_PID_GYRO_I_YAW,
+
+	COMMAND_ID_PID_GYRO_D_ROLL,
+	COMMAND_ID_PID_GYRO_D_PITCH,
+	COMMAND_ID_PID_GYRO_D_YAW,
+
+	//ACCEL
+	COMMAND_ID_PID_ACCEL_P_ROLL,
+	COMMAND_ID_PID_ACCEL_P_PITCH,
+
+	COMMAND_ID_PID_ACCEL_I_ROLL,
+	COMMAND_ID_PID_ACCEL_I_PITCH,
+
+	COMMAND_ID_PID_ACCEL_D_ROLL,
+	COMMAND_ID_PID_ACCEL_D_PITCH,
+
 	/* Counter for the amount of commands */
 	COMMAND_ID_COUNT,
 
@@ -273,11 +299,6 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = {
 		{
 			"adc_scales_right", 	COMMAND_ID_ADC_SCALES_RIGHT, EEPROM_ADC_SCALES, EEPROM_VALUE_TYPE_SYSTEM, 8, VAL_UINT_32, .valueRange = {0, 200}
 		},
-		[COMMAND_ID_PID_ROLL_KP] =
-		{
-			"pid_roll_kp", 			COMMAND_ID_PID_ROLL_KP, EEPROM_PID_ROLL_KP, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100}
-		},
-
 
 		/* Calibrate motors */
 		[COMMAND_ID_MOTORCALIBRATE] =
@@ -285,7 +306,6 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = {
 			"motor_calibration", 	COMMAND_ID_MOTORCALIBRATE, EEPROM_MOTORCALIBRATE, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_BOOL, .valueRange = {0, 1}
 		},
 
-
 		/* set period commands */
 		[COMMAND_ID_PERIOD_SYSTEM] =
 		{
@@ -497,6 +517,80 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = {
 			"flag_flightmode_3_channel", 	COMMAND_ID_FLAG_FLIGHTMODE_3_CHANNEL, EEPROM_FLAG_FLIGHTMODE_3, EEPROM_VALUE_TYPE_SYSTEM, 4, VAL_UINT_8, .valueRange = {0, 18}
 		},
 
+		//PROFILE VALUES
+		//PID VALUES
+		//GYRO P
+		[COMMAND_ID_PID_GYRO_P_ROLL] =
+		{
+				"pid_gyro_p_roll", 	COMMAND_ID_PID_GYRO_P_ROLL, EEPROM_PID_GYRO_P_ROLL, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100}
+		},
+		[COMMAND_ID_PID_GYRO_P_PITCH] =
+		{
+				"pid_gyro_p_pitch", 	COMMAND_ID_PID_GYRO_P_PITCH, EEPROM_PID_GYRO_P_PITCH, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100}
+		},
+		[COMMAND_ID_PID_GYRO_P_YAW] =
+		{
+				"pid_gyro_p_yaw", 	COMMAND_ID_PID_GYRO_P_YAW, EEPROM_PID_GYRO_P_YAW, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100}
+		},
+
+		//GYRO I
+		[COMMAND_ID_PID_GYRO_I_ROLL] =
+		{
+				"pid_gyro_i_roll", 	COMMAND_ID_PID_GYRO_I_ROLL, EEPROM_PID_GYRO_I_ROLL, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100}
+		},
+		[COMMAND_ID_PID_GYRO_I_PITCH] =
+		{
+				"pid_gyro_i_pitch", 	COMMAND_ID_PID_GYRO_I_PITCH, EEPROM_PID_GYRO_I_PITCH, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100}
+		},
+		[COMMAND_ID_PID_GYRO_I_YAW] =
+		{
+				"pid_gyro_i_yaw", 	COMMAND_ID_PID_GYRO_I_YAW, EEPROM_PID_GYRO_I_YAW, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100}
+		},
+
+		//GYRO D
+		[COMMAND_ID_PID_GYRO_D_ROLL] =
+		{
+				"pid_gyro_d_roll", 	COMMAND_ID_PID_GYRO_D_ROLL, EEPROM_PID_GYRO_D_ROLL, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100}
+		},
+		[COMMAND_ID_PID_GYRO_D_PITCH] =
+		{
+				"pid_gyro_d_pitch", 	COMMAND_ID_PID_GYRO_D_PITCH, EEPROM_PID_GYRO_D_PITCH, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100}
+		},
+		[COMMAND_ID_PID_GYRO_D_YAW] =
+		{
+				"pid_gyro_d_yaw", 	COMMAND_ID_PID_GYRO_D_YAW, EEPROM_PID_GYRO_D_YAW, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100}
+		},
+
+		//ACCEL P
+		[COMMAND_ID_PID_ACCEL_P_ROLL] =
+		{
+				"pid_accel_p_roll", 	COMMAND_ID_PID_ACCEL_P_ROLL, EEPROM_PID_ACCEL_P_ROLL, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100}
+		},
+		[COMMAND_ID_PID_ACCEL_P_PITCH] =
+		{
+				"pid_accel_p_pitch", 	COMMAND_ID_PID_ACCEL_P_PITCH, EEPROM_PID_ACCEL_P_PITCH, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100}
+		},
+
+		//ACCEL I
+		[COMMAND_ID_PID_ACCEL_I_ROLL] =
+		{
+				"pid_accel_i_roll", 	COMMAND_ID_PID_ACCEL_I_ROLL, EEPROM_PID_ACCEL_I_ROLL, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100}
+		},
+		[COMMAND_ID_PID_ACCEL_I_PITCH] =
+		{
+				"pid_accel_i_pitch", 	COMMAND_ID_PID_ACCEL_I_PITCH, EEPROM_PID_ACCEL_I_PITCH, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100}
+		},
+
+		//ACCEL D
+		[COMMAND_ID_PID_ACCEL_D_ROLL] =
+		{
+				"pid_accel_d_roll", 	COMMAND_ID_PID_ACCEL_D_ROLL, EEPROM_PID_ACCEL_D_ROLL, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100}
+		},
+		[COMMAND_ID_PID_ACCEL_D_PITCH] =
+		{
+				"pid_accel_d_pitch", 	COMMAND_ID_PID_ACCEL_D_PITCH, EEPROM_PID_ACCEL_D_PITCH, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100}
+		},
+
 
 };
 

UAV-ControlSystem/src/config/eeprom.c

@@ -24,6 +24,7 @@
 #include "drivers/failsafe_toggles.h"
 #include "drivers/motormix.h"
 #include "drivers/motors.h"
+#include "Flight/pid.h"
 
 /* Reads the EEPROM version from EEPROM - Is compared to EEPROM_SYS_VERSION */
 uint8_t stored_eeprom_identifier;
@@ -254,11 +255,93 @@ EEPROM_DATA_t eeprom_sys_Arr[EEPROM_SYS_COUNT] = {
 
 /* Data pointers and sizes for profile content */
 EEPROM_DATA_t eeprom_profile_Arr[EEPROM_PROFILE_COUNT] = {
-	[EEPROM_PID_ROLL_KP] =
+	//GYRO P VAL
+	[EEPROM_PID_GYRO_P_ROLL] =
 	{
-		.size = sizeof(pid_pitch_pk),
+		.size = sizeof(PidGyroProfile.P[0]),
-		.dataPtr = &pid_pitch_pk,
+		.dataPtr = &PidGyroProfile.P[0],
-	}
+	},
+	[EEPROM_PID_GYRO_P_PITCH] =
+	{
+		.size = sizeof(PidGyroProfile.P[1]),
+		.dataPtr = &PidGyroProfile.P[1],
+	},
+	[EEPROM_PID_GYRO_P_YAW] =
+	{
+		.size = sizeof(PidGyroProfile.P[2]),
+		.dataPtr = &PidGyroProfile.P[2],
+	},
+
+	//GYRO I VAL
+	[EEPROM_PID_GYRO_I_ROLL] =
+	{
+		.size = sizeof(PidGyroProfile.I[0]),
+		.dataPtr = &PidGyroProfile.I[0],
+	},
+	[EEPROM_PID_GYRO_I_PITCH] =
+	{
+		.size = sizeof(PidGyroProfile.I[1]),
+		.dataPtr = &PidGyroProfile.I[1],
+	},
+	[EEPROM_PID_GYRO_I_YAW] =
+	{
+		.size = sizeof(PidGyroProfile.I[2]),
+		.dataPtr = &PidGyroProfile.I[2],
+	},
+
+	//GYRO D VAL
+	[EEPROM_PID_GYRO_D_ROLL] =
+	{
+		.size = sizeof(PidGyroProfile.D[0]),
+		.dataPtr = &PidGyroProfile.D[0],
+	},
+	[EEPROM_PID_GYRO_D_PITCH] =
+	{
+		.size = sizeof(PidGyroProfile.D[1]),
+		.dataPtr = &PidGyroProfile.D[1],
+	},
+	[EEPROM_PID_GYRO_D_YAW] =
+	{
+		.size = sizeof(PidGyroProfile.D[2]),
+		.dataPtr = &PidGyroProfile.D[2],
+	},
+
+	//ACCEL P VAL
+	[EEPROM_PID_ACCEL_P_ROLL] =
+	{
+		.size = sizeof(PidAccelerometerProfile.P[0]),
+		.dataPtr = &PidAccelerometerProfile.P[0],
+	},
+	[EEPROM_PID_ACCEL_P_PITCH] =
+	{
+		.size = sizeof(PidAccelerometerProfile.P[1]),
+		.dataPtr = &PidAccelerometerProfile.P[1],
+	},
+
+	//ACCEL I VAL
+	[EEPROM_PID_ACCEL_I_ROLL] =
+	{
+		.size = sizeof(PidAccelerometerProfile.I[0]),
+		.dataPtr = &PidAccelerometerProfile.I[0],
+	},
+	[EEPROM_PID_ACCEL_I_PITCH] =
+	{
+		.size = sizeof(PidAccelerometerProfile.I[1]),
+		.dataPtr = &PidAccelerometerProfile.I[1],
+	},
+
+	//ACCEL_D_VAL
+	[EEPROM_PID_ACCEL_D_ROLL] =
+	{
+		.size = sizeof(PidAccelerometerProfile.D[0]),
+		.dataPtr = &PidAccelerometerProfile.D[0],
+	},
+	[EEPROM_PID_ACCEL_D_PITCH] =
+	{
+		.size = sizeof(PidAccelerometerProfile.D[1]),
+		.dataPtr = &PidAccelerometerProfile.D[1],
+	},
+
 };
 
 /* Data pointers and sizes for footer content */

UAV-ControlSystem/src/drivers/sbus.c

@@ -29,6 +29,9 @@
 /* This instance is read by the whole system and should contain actual RX data */
 sbusFrame_s sbusChannelData = {0};
 dma_usart_return raw_dma_data_t;
+dma_usart_return raw_dma_data_t = {0};
+rc_input_t rc_input = {0};
+float rc_rate = 1.0;
 
 /* Create a DMA Handler */
 usart_dma_profile dmaHandler;
@@ -94,6 +97,11 @@ uint16_t rx_truncate(uint16_t rx_channel)
 		return rx_channel;
 }
 
+int16_t rc_input_mapping(float channel)
+{
+	return (int16_t)((channel-1500)*rc_rate);
+}
+
 /***********************************************************************
 * BRIEF: Updates "sbusChannelData"                                     *
 * INFORMATION: Is called by the scheduler                              *
@@ -196,6 +204,11 @@ void sbus_read()
 						sbusChannelData.chan6 	= rx_truncate(sbusChannelData.chan6);
 						sbusChannelData.chan7 	= rx_truncate(sbusChannelData.chan7);
 						sbusChannelData.chan8 	= rx_truncate(sbusChannelData.chan8);
+						
+						rc_input.Roll = rc_input_mapping((float)sbusChannelData.chan1);
+						rc_input.Pitch = rc_input_mapping((float)sbusChannelData.chan2);
+						rc_input.Yaw = rc_input_mapping((float)sbusChannelData.chan4);
+						rc_input.Throttle = (int16_t)sbusChannelData.chan3;
 					}
 					else
 					{
@@ -218,7 +231,6 @@ void sbus_read()
 						}
 
 					}
-
 				}
 			}
 

UAV-ControlSystem/src/main.c

@@ -27,6 +27,7 @@
 #include "drivers/sbus.h"
 #include "drivers/motormix.h"
 #include "drivers/motors.h"
+#include "Flight/pid.h"
 
 
 /**************************************************************************
@@ -45,10 +46,11 @@ void init_system()
 	//Configure the clock
 	system_clock_config();
 
+	pidInit();
+
 	/* read saved variables from eeprom, in most cases eeprom should be read after a lot of the initializes  */
 	readEEPROM();
 
-
 	//initialize the CLI NOTE: Cant use the same usart as anything else or there will be some big trouble
 	cliInit(USART6);
 
@@ -59,6 +61,7 @@ void init_system()
 	HAL_Delay(1000);
 	pwmEnableAllMotors(Oneshot125);
 
+
 #ifdef USE_LEDS
 	//Initialize the on board leds
 	ledReavoEnable();

UAV-ControlSystem/src/tasks_main.c

@@ -40,17 +40,30 @@
 #include "drivers/I2C.h"
 #include "drivers/accel_gyro.h"
 #include "drivers/motormix.h"
+#include "Flight/pid.h"
 
 void systemTaskGyroPid(void)
 {
 	//Read gyro and update PID and finally update the motors. The most important task in the system
 
+	//Update Gyro
+
+	//Convert?
+
+	//PID Gyro
+	pidRun(PID_ID_GYRO);
+
+	//MIX GO
+
+
 	//call the motor mix
 	mix();
 }
 
 void systemTaskAccelerometer(void)
 {
+
+	pidRun(PID_ID_ACCELEROMETER);
 	//update the accelerometer data
 //	uint8_t c = 97;
 //	usart_transmit(&cliUsart, &c, 1, 1000000000);
@@ -81,9 +94,10 @@ void systemTaskRx(void)
 	flags_ProcessRcChannel_Improved(STICK_CHANNEL_COUNT+1, STICK_CHANNEL_COUNT + AUX_CHANNEL_COUNT);
 
 	//temporary send data from the RC directly form the RC
-	PID_Out[0] = frame.chan1 - 1500;
+//	RawRcCommand.Roll = frame.chan1;
-	PID_Out[1] = frame.chan2 - 1500;
+//	RawRcCommand.Pitch = frame.chan2;
-	PID_Out[2] = frame.chan4 - 1500;
+//	RawRcCommand.Yaw = frame.chan4;
+//	RawRcCommand.Throttle = frame.chan4;
 
 }
 
@@ -158,12 +172,12 @@ void systemTaskBattery(void)
 
 void systemTaskBaro(void)
 {
-	//Obtain the barometer data
+	pidRun(PID_ID_BAROMETER);
 }
 
 void systemTaskCompass(void)
 {
-	//Obtain compass data
+	pidRun(PID_ID_COMPASS);
 }
 
 void systemTaskGps(void)