PID functions

Added new global variables and functions to the PID controller which is
supposed to sync with the main program

UAV-ControlSystem/inc/Flight/pid.h

@@ -31,9 +31,24 @@
 #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
+
+
+typedef struct
+{
+	int Roll;
+	int Pitch;
+	int Yaw;
+	int Throttle;
+} control_data_t;
+
 /*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 +78,11 @@ typedef struct pidProfile_s {
 
 } pidProfile_t;
 
+
+extern control_data_t RawRcCommand;
+extern pidProfile_t PidGyroProfile, PidAccelerometerProfile;
+extern pidProfile_t PidCompassProfile, PidBarometerProfile;
+
 /**************************************************************************
 * BRIEF: Constrain float values within a defined limit                    *
 * INFORMATION: Used in PID loop to limit values                           *
@@ -81,6 +101,12 @@ 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	  *
@@ -94,4 +120,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/src/Flight/pid.c

@@ -15,6 +15,17 @@
 
 #include "Flight/pid.h"
 
+#define BAROMETER_RANGE 2000
+#define RADIO_RANGE 1000
+#define ACCELEROMETER_RANGE 4000
+#define GYRO_RANGE 32
+#define COMPASS_RANGE 360
+
+control_data_t RawRcCommand = {0};
+pidProfile_t PidGyroProfile = {0}, PidAccelerometerProfile = {0};
+pidProfile_t PidCompassProfile = {0}, PidBarometerProfile = {0};
+
+
 /**************************************************************************
 * BRIEF: Constrain float values within a defined limit                    *
 * INFORMATION: Used in PID loop to limit values                           *
@@ -58,6 +69,11 @@ void getCurrentValues(float *sensorValues, uint8_t ID_profile)
 
 };
 
+float convertData(int inputRange, int outputRange, int offset, float value)
+{
+	return (outputRange/inputRange)*(value-offset);
+}
+
 /**************************************************************************
 * BRIEF: Update desired values from rc command		                      *
 * INFORMATION:                       									  *
@@ -65,26 +81,29 @@ 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[ROLL] = PidAccelerometerProfile.PID_Out[ROLL];
-		desiredCommand[PITCH] = 0;
+		desiredCommand[PITCH] = PidAccelerometerProfile.PID_Out[PITCH];
-		desiredCommand[YAW] = 0;
+		desiredCommand[YAW] = PidCompassProfile.PID_Out[0];
 		break;
-	case 2:
+	case PID_ID_ACCELEROMETER:
+		desiredCommand[ROLL] = convertData(RADIO_RANGE, ACCELEROMETER_RANGE, 0, RawRcCommand.Roll);
+		desiredCommand[PITCH] = convertData(RADIO_RANGE, ACCELEROMETER_RANGE, 0, RawRcCommand.Pitch);
 		break;
-	case 3:
+	case PID_ID_COMPASS:
+		desiredCommand[0] = convertData(RADIO_RANGE, COMPASS_RANGE, 0, RawRcCommand.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, RawRcCommand.Throttle);
-	case 5:
 		break;
 	default:
 		break;
 	}
 }
 
+
 /**************************************************************************
 * BRIEF: Initializes the pid profile PID controller				          *
 * INFORMATION: Recommended to use if unexpected values occur of profile	  *
@@ -153,6 +172,16 @@ void pidUAVInit(pidProfile_t *pidProfile)
 
 }
 
+/**************************************************************************
+* BRIEF: Initializes PID profiles			 					          *
+* INFORMATION: 															  *
+**************************************************************************/
+void pidInit()
+{
+
+	int test;
+}
+
 /**************************************************************************
 * BRIEF: Dynamic PID controller, able to handle several PID controller    *
 * 		 connected to different profiles.
@@ -249,3 +278,61 @@ 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:
+
+		pidUAV(&PidGyroProfile);
+
+		break;
+	case PID_ID_ACCELEROMETER:
+
+		if (!PidAccelerometerProfile.pidEnabled)
+		{
+			PidAccelerometerProfile.PID_Out[ROLL] = RawRcCommand.Roll;
+			PidAccelerometerProfile.PID_Out[PITCH] = RawRcCommand.Pitch;
+		}
+		else
+		{
+			pidUAV(&PidAccelerometerProfile);
+		}
+
+		break;
+	case PID_ID_COMPASS:
+
+		if (!PidCompassProfile.pidEnabled)
+		{
+			PidCompassProfile.PID_Out[0] = RawRcCommand.Yaw;
+		}
+		else
+		{
+			pidUAV(&PidCompassProfile);
+		}
+
+		break;
+	case PID_ID_BAROMETER:
+
+		if (!PidBarometerProfile.pidEnabled)
+		{
+			PidBarometerProfile.PID_Out[0] = RawRcCommand.Throttle;
+		}
+		else
+		{
+			//convertData(1000, 3000, 0, );
+
+			pidUAV(&PidBarometerProfile);
+		}
+
+		break;
+	default:
+		break;
+	}
+}

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"
 
 /**************************************************************************
 * BRIEF: Should contain all the initializations of the system, needs to
@@ -47,7 +48,6 @@ void init_system()
 	/* 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(USART3);
 
@@ -57,6 +57,8 @@ void init_system()
 	//init motors to run with oneshot 125
 	pwmEnableAllMotors(Oneshot125);
 
+	pidInit();
+
 #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);
@@ -78,9 +91,10 @@ void systemTaskRx(void)
 	}
 
 	//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;
 
 }
 
@@ -155,12 +169,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)