From 3039f3a3948ea69e0ff558ba0f617b928161316b Mon Sep 17 00:00:00 2001
From: johan9107 <jgr12001@student.mdh.se>
Date: Mon, 31 Oct 2016 18:17:13 +0100
Subject: [PATCH] pid SHIT FIX

---
 UAV-ControlSystem/src/Flight/pid.c | 28 ++++++++++++++++++++++++----
 UAV-ControlSystem/src/main.c       |  2 +-
 2 files changed, 25 insertions(+), 5 deletions(-)

diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c
index e538621..018181f 100644
--- a/UAV-ControlSystem/src/Flight/pid.c
+++ b/UAV-ControlSystem/src/Flight/pid.c
@@ -151,6 +151,17 @@ 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*/
@@ -188,6 +199,8 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
 		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);
 
@@ -456,23 +469,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:
@@ -618,4 +631,11 @@ void pidEproom(void)
 	PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 2;
 	PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100;
 
+	PidProfileBuff[PID_ID_GYRO].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000000; //ÄNDRA TILL SEKUNDER inte ms
+	PidProfileBuff[PID_ID_ACCELEROMETER].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000000;
+	PidProfileBuff[PID_ID_COMPASS].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000000;
+	PidProfileBuff[PID_ID_BAROMETER].dT = SystemTasks[TASK_BARO].desiredPeriod/1000000;
+
+
+
 }
diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c
index 2c9dfaf..f678bab 100644
--- a/UAV-ControlSystem/src/main.c
+++ b/UAV-ControlSystem/src/main.c
@@ -60,7 +60,7 @@ void init_system()
 	pidEproom();
 
 	//initialize the CLI NOTE: Cant use the same usart as anything else or there will be some big trouble
-	cliInit(USART3);
+	cliInit(USART6);
 
 	//init sbus, using USART1
 	sbus_init();