philip/MDH-UAV-Control-System
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

pid SHIT FIX

Browse Source
This commit is contained in:
johan9107 2016-10-31 18:17:13 +01:00
parent dcf36fc542
commit 3039f3a394
2 changed files with 25 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
UAV-ControlSystem/src/Flight/pid.c
View File

@ -151,6 +151,17 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
mpu6000_read_accel(&accelProfile); /*Reads data from accelerometer*/ 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; float alpha = 0.5;
/*May need Low pass filter since the accelerometer may drift*/ /*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[0] = sensorValues[ROLL];
oldSensorValue[1] = sensorValues[PITCH]; oldSensorValue[1] = sensorValues[PITCH];
// sensorValues[ROLL] = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); // sensorValues[ROLL] = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
// sensorValues[PITCH] = calcAngle(PITCH, 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: case PID_ID_GYRO:
PidProfileBuff[ID].DOF = 3; 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; break;
case PID_ID_ACCELEROMETER: case PID_ID_ACCELEROMETER:
PidProfileBuff[ID].DOF = 2; PidProfileBuff[ID].DOF = 2;
PidProfileBuff[ID].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000; PidProfileBuff[ID].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000000;
break; break;
case PID_ID_COMPASS: case PID_ID_COMPASS:
PidProfileBuff[ID].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000; PidProfileBuff[ID].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000000;
break; break;
case PID_ID_BAROMETER: case PID_ID_BAROMETER:
PidProfileBuff[ID].dT = SystemTasks[TASK_BARO].desiredPeriod/1000; PidProfileBuff[ID].dT = SystemTasks[TASK_BARO].desiredPeriod/1000000;
break; break;
default: default:
@ -618,4 +631,11 @@ void pidEproom(void)
PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 2; PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 2;
PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100; 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;
} }

2
UAV-ControlSystem/src/main.c
View File

@ -60,7 +60,7 @@ void init_system()
pidEproom(); pidEproom();
//initialize the CLI NOTE: Cant use the same usart as anything else or there will be some big trouble //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 //init sbus, using USART1
sbus_init(); sbus_init();