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

PID changed buffer to filter

Browse Source
This commit is contained in:
johan9107 2016-10-31 14:44:34 +01:00
parent f787b19572
commit dcf36fc542
1 changed files with 14 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -27,7 +27,7 @@
#define RADIO_RANGE 500 /*Radio range input*/ #define RADIO_RANGE 500 /*Radio range input*/
#define BAROMETER_RANGE 2000 /*Determines the range of the maximum height (limits the rc input)*/ #define BAROMETER_RANGE 2000 /*Determines the range of the maximum height (limits the rc input)*/
#define ACCELEROMETER_RANGE 35 /*Determines the range of the maximum angle (limits the rc input) & (Accelerometer takes int to max 16 G)*/ #define ACCELEROMETER_RANGE 60 /*Determines the range of the maximum angle (limits the rc input) & (Accelerometer takes int to max 16 G)*/
#define GYRO_RANGE 720 /*Determines the maximum rotational limit (limits the rc input)*/ #define GYRO_RANGE 720 /*Determines the maximum rotational limit (limits the rc input)*/
#define COMPASS_RANGE 180 /*Determines the maximum compass limit (limits the rc input)*/ #define COMPASS_RANGE 180 /*Determines the maximum compass limit (limits the rc input)*/
@ -92,6 +92,11 @@ float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, cons
return angle; return angle;
} }
float calcGravity(accel_t profile ) //const float x_axis, const float y_axis, const float z_axis)
{
return sqrt(profile.accelXconv*profile.accelXconv + profile.accelYconv*profile.accelYconv + profile.accelZconv*profile.accelZconv);
}
/************************************************************************** /**************************************************************************
* BRIEF: Scales data from input range to output range * * BRIEF: Scales data from input range to output range *
* INFORMATION: * * INFORMATION: *
@ -118,8 +123,8 @@ float constrainf(float amt, int low, int high)
float oldSensorValue[2] = {0}; float oldSensorValue[2] = {0};
float oldSensorValueRoll[20] = {0}; float oldSensorValueRoll[12] = {0};
float oldSensorValuePitch[20] = {0}; float oldSensorValuePitch[12] = {0};
int i = 0; int i = 0;
@ -147,7 +152,6 @@ 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*/
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*/
@ -165,7 +169,7 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
float RollValue = 0; float RollValue = 0;
float PitchValue = 0; float PitchValue = 0;
for (int ii = 0; ii < 20; ii++) for (int ii = 0; ii < 12; ii++)
{ {
RollValue = RollValue + oldSensorValueRoll[ii]; RollValue = RollValue + oldSensorValueRoll[ii];
PitchValue = PitchValue + oldSensorValuePitch[ii]; PitchValue = PitchValue + oldSensorValuePitch[ii];
@ -173,13 +177,13 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
} }
i = (i < 19)? i + 1:0; i = (i < 11)? i + 1:0;
sensorValues[ROLL] = RollValue/20; sensorValues[ROLL] = RollValue/12;
sensorValues[PITCH] = PitchValue/20; sensorValues[PITCH] = PitchValue/12;
sensorValues[ROLL] = alpha*RollValue/20 + (1-alpha)*oldSensorValue[0]; sensorValues[ROLL] = alpha*RollValue/12 + (1-alpha)*oldSensorValue[0];
sensorValues[PITCH] = alpha*PitchValue/20 + (1-alpha)*oldSensorValue[1]; sensorValues[PITCH] = alpha*PitchValue/12 + (1-alpha)*oldSensorValue[1];
// //
oldSensorValue[0] = sensorValues[ROLL]; oldSensorValue[0] = sensorValues[ROLL];
oldSensorValue[1] = sensorValues[PITCH]; oldSensorValue[1] = sensorValues[PITCH];