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

PID Added new values to improve filter

Browse Source
This commit is contained in:
johan9107 2016-10-27 11:55:16 +02:00
parent 69c9f22dad
commit c3e969e982
2 changed files with 21 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -26,7 +26,7 @@
#define RADIO_RANGE 500 /*Radio range input*/
#define BAROMETER_RANGE 2000 /*Determines the range of the maximum height (limits the rc input)*/
#define ACCELEROMETER_RANGE 20 /*Determines the range of the maximum angle (limits the rc input) & (Accelerometer takes int to max 16 G)*/
#define ACCELEROMETER_RANGE 35 /*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 COMPASS_RANGE 180 /*Determines the maximum compass limit (limits the rc input)*/
@ -113,6 +113,8 @@ float constrainf(float amt, int low, int high)
return amt;
}
float oldSensorValue[2] = {0};
/**************************************************************************
* BRIEF: Update current sensor values *
* INFORMATION: *
@ -136,10 +138,20 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
float alpha = 0.5;
/*May need Low pass filter since the accelerometer may drift*/
sensorValues[ROLL] = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
sensorValues[PITCH] = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
float X_roll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
float X_pitch = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
sensorValues[ROLL] = alpha*X_roll + (1-alpha)*oldSensorValue[0];
sensorValues[PITCH] = alpha*X_pitch + (1-alpha)*oldSensorValue[1];
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);
//float sensorRoll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
//sensorValues[ROLL] = pt1FilterApply4(&accelFilter[0], sensorRoll, 90, PidProfileBuff[ROLL].dT);
@ -384,6 +396,8 @@ void pidRun(uint8_t ID)
}
}
/*--------------------------------------------Init Functions----------------------------------------------------------------------------------*/
/**************************************************************************
* BRIEF: Initializes a certain pidbuffer profile connected to *
* a PID controller *

8
UAV-ControlSystem/src/drivers/motormix.c
View File

@ -114,8 +114,8 @@ void mix()
* Calculation is: Output from control system * weight from model for each motor
*/
RPY_Mix[i] = \
PidProfile[PID_ID_GYRO].PID_Out[ROLL] * mixerUAV[i].roll + \
PidProfile[PID_ID_GYRO].PID_Out[PITCH] * mixerUAV[i].pitch + \
(int)PidProfile[PID_ID_GYRO].PID_Out[ROLL] * mixerUAV[i].roll + \
(int)PidProfile[PID_ID_GYRO].PID_Out[PITCH] * mixerUAV[i].pitch + \
PidProfile[PID_ID_GYRO].PID_Out[YAW] * mixerUAV[i].yaw * ((mixerConfig.yaw_reverse_direction) ? -1 : 1);
@ -194,8 +194,8 @@ void mix()
{
RPY_Mix[i] = \
throttle * mixerUAV[i].throttle + \
PidProfile[PID_ID_GYRO].PID_Out[ROLL] * mixerUAV[i].roll + \
PidProfile[PID_ID_GYRO].PID_Out[PITCH] * mixerUAV[i].pitch + \
(int)PidProfile[PID_ID_GYRO].PID_Out[ROLL] * mixerUAV[i].roll + \
(int)PidProfile[PID_ID_GYRO].PID_Out[PITCH] * mixerUAV[i].pitch + \
PidProfile[PID_ID_GYRO].PID_Out[YAW] * mixerUAV[i].yaw * ((mixerConfig.yaw_reverse_direction) ? -1 : 1);
}