From c3e969e982ed81e95d4aa710aaf71ea5219856c4 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Thu, 27 Oct 2016 11:55:16 +0200 Subject: [PATCH] PID Added new values to improve filter --- UAV-ControlSystem/src/Flight/pid.c | 20 +++++++++++++++++--- UAV-ControlSystem/src/drivers/motormix.c | 8 ++++---- 2 files changed, 21 insertions(+), 7 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 065ad46..98ffe07 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -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 * diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index 22624cf..3da3fec 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -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); }