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

PID accelerometer added low pass filter

Browse Source
This commit is contained in:
johan9107 2016-10-25 10:13:49 +02:00
parent 6994bcaf11
commit facfcb7023
1 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -56,6 +56,7 @@ pidProfileBuff_t PidProfileBuff[PID_COUNT] = {0};
accel_t accelProfile; /*Struct profile for input data from sensor*/ accel_t accelProfile; /*Struct profile for input data from sensor*/
gyro_t gyroProfile; /*Struct profile for input data from sensor*/ gyro_t gyroProfile; /*Struct profile for input data from sensor*/
pt1Filter_t accelFilter[2] = {0};
/************************************************************************** /**************************************************************************
* BRIEF: Calculates angle from accelerometer * * BRIEF: Calculates angle from accelerometer *
@ -127,10 +128,19 @@ 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*/
/*May need Low pass filter since the accelerometer may drift*/ /*May need Low pass filter since the accelerometer may drift*/
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);
//float sensorRoll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
//sensorValues[ROLL] = pt1FilterApply4(&accelFilter[0], sensorRoll, 90, PidProfileBuff[ROLL].dT);
//float sensorPitch = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
//sensorValues[PITCH] = pt1FilterApply4(&accelFilter[1], sensorPitch, 90, PidProfileBuff[PITCH].dT);
break; break;
case PID_ID_COMPASS: case PID_ID_COMPASS:
case PID_ID_BAROMETER: case PID_ID_BAROMETER: