From 2227ab997fa2261ca5a0d8c9901738cc34eee47e Mon Sep 17 00:00:00 2001
From: johan9107 <jgr12001@student.mdh.se>
Date: Mon, 17 Oct 2016 11:58:36 +0200
Subject: [PATCH] PID changes

Added ID profile to PID init and new init func
---
 UAV-ControlSystem/inc/Flight/pid.h |  2 +-
 UAV-ControlSystem/src/Flight/pid.c | 24 ++++++++++++++----------
 2 files changed, 15 insertions(+), 11 deletions(-)

diff --git a/UAV-ControlSystem/inc/Flight/pid.h b/UAV-ControlSystem/inc/Flight/pid.h
index 9fcb40f..205ac18 100644
--- a/UAV-ControlSystem/inc/Flight/pid.h
+++ b/UAV-ControlSystem/inc/Flight/pid.h
@@ -111,7 +111,7 @@ 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    *
diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c
index 5bfd0e4..7143944 100644
--- a/UAV-ControlSystem/src/Flight/pid.c
+++ b/UAV-ControlSystem/src/Flight/pid.c
@@ -133,11 +133,12 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile)
 * 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;
@@ -203,10 +204,13 @@ void pidUAVInit(pidProfile_t *pidProfile)
 **************************************************************************/
 void pidInit()
 {
-	pidUAVInit(&PidGyroProfile);
-	pidUAVInit(&PidAccelerometerProfile);
-	pidUAVInit(&PidCompassProfile);
-	pidUAVInit(&PidBarometerProfile);
+
+	mpu6000_init(&gyroProfile,&accelProfile);
+
+	pidUAVInit(&PidGyroProfile, PID_ID_GYRO);
+	pidUAVInit(&PidAccelerometerProfile, PID_ID_ACCELEROMETER);
+	pidUAVInit(&PidCompassProfile, PID_ID_COMPASS);
+	pidUAVInit(&PidBarometerProfile, PID_ID_BAROMETER);
 }
 
 /**************************************************************************
@@ -318,15 +322,15 @@ void pidRun(uint8_t ID)
 	case PID_ID_GYRO:
 
 		if (!PidGyroProfile.pidEnabled)
-		{
-			pidUAV(&PidGyroProfile);
-		}
-		else
 		{
 			PidGyroProfile.PID_Out[ROLL] = RawRcCommand.Roll;
 			PidGyroProfile.PID_Out[PITCH] = RawRcCommand.Pitch;
 			PidGyroProfile.PID_Out[YAW] = RawRcCommand.Yaw;
 		}
+		else
+		{
+			pidUAV(&PidGyroProfile);
+		}
 
 		break;
 	case PID_ID_ACCELEROMETER: