373 lines
11 KiB
C
373 lines
11 KiB
C
/**************************************************************************
|
|
* NAME: pid.c *
|
|
* AUTHOR: Johan Gärtner *
|
|
*
|
|
* PURPOSE: This file contains pid functions *
|
|
* INFORMATION: pidUAV is the final pid controller which only takes *
|
|
* pidProfile_t as input. pidProfile_t will be updated and *
|
|
* contains all the information about the current state of *
|
|
* the pid. *
|
|
* GLOBAL VARIABLES: *
|
|
* Variable Type Description *
|
|
* -------- ---- ----------- *
|
|
*
|
|
* **************************************************************************/
|
|
|
|
#include "Flight/pid.h"
|
|
#include "drivers/accel_gyro.h"
|
|
#include <math.h>
|
|
|
|
#define BAROMETER_RANGE 2000
|
|
#define RADIO_RANGE 1000
|
|
#define ACCELEROMETER_RANGE 90 // Accelerometer takes int to max 16 G
|
|
#define GYRO_RANGE 2000
|
|
#define COMPASS_RANGE 360
|
|
|
|
control_data_t RawRcCommand = {0};
|
|
pidProfile_t PidGyroProfile = {0}, PidAccelerometerProfile = {0};
|
|
pidProfile_t PidCompassProfile = {0}, PidBarometerProfile = {0};
|
|
|
|
accel_t accelProfile;
|
|
gyro_t gyroProfile;
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Constrain float values within a defined limit *
|
|
* INFORMATION: Used in PID loop to limit values *
|
|
**************************************************************************/
|
|
float constrainf(float amt, int low, int high)
|
|
{
|
|
if (amt < low)
|
|
return low;
|
|
else if (amt > high)
|
|
return high;
|
|
else
|
|
return amt;
|
|
}
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Update current sensor values *
|
|
* INFORMATION: *
|
|
**************************************************************************/
|
|
void getCurrentValues(float *sensorValues, uint8_t ID_profile)
|
|
{
|
|
switch (ID_profile)
|
|
{
|
|
case PID_ID_GYRO:
|
|
mpu6000_read_gyro(&gyroProfile);
|
|
|
|
sensorValues[ROLL] = gyroProfile.gyroY;
|
|
sensorValues[PITCH] = gyroProfile.gyroX;
|
|
sensorValues[YAW] = gyroProfile.gyroZ;
|
|
break;
|
|
case PID_ID_ACCELEROMETER:
|
|
mpu6000_read_accel(&accelProfile);
|
|
|
|
/*May need Low pass filter since the accelerometer may drift*/
|
|
sensorValues[ROLL] = atan2(accelProfile.accelXconv,accelProfile.accelZconv)*180*M_PI;//acos(accelProfile.accelXconv/R)*180/M_PI;
|
|
sensorValues[PITCH] = atan2(-accelProfile.accelXconv, sqrt(accelProfile.accelYconv*accelProfile.accelYconv + accelProfile.accelZconv*accelProfile.accelZconv))*180/M_PI;
|
|
break;
|
|
case PID_ID_COMPASS:
|
|
break;
|
|
case PID_ID_BAROMETER:
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
};
|
|
|
|
float convertData(int inputRange, int outputRange, int offset, float value)
|
|
{
|
|
return (outputRange/inputRange)*(value-offset);
|
|
}
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Update desired values from rc command *
|
|
* INFORMATION: *
|
|
**************************************************************************/
|
|
void getPointRate(float *desiredCommand, uint8_t ID_profile)
|
|
{
|
|
//*Do something smart*//
|
|
switch(ID_profile)
|
|
{
|
|
case PID_ID_GYRO:
|
|
|
|
if (!PidAccelerometerProfile.pidEnabled)
|
|
{
|
|
desiredCommand[ROLL] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidAccelerometerProfile.PID_Out[ROLL]);
|
|
desiredCommand[PITCH] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidAccelerometerProfile.PID_Out[PITCH]);
|
|
}
|
|
else
|
|
{
|
|
desiredCommand[ROLL] = convertData(ACCELEROMETER_RANGE, GYRO_RANGE, 0, PidAccelerometerProfile.PID_Out[ROLL]);
|
|
desiredCommand[PITCH] = convertData(ACCELEROMETER_RANGE, GYRO_RANGE, 0, PidAccelerometerProfile.PID_Out[PITCH]);
|
|
}
|
|
|
|
|
|
if (!PidCompassProfile.pidEnabled)
|
|
{
|
|
desiredCommand[YAW] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidCompassProfile.PID_Out[0]);
|
|
}
|
|
else
|
|
{
|
|
desiredCommand[YAW] = convertData(ACCELEROMETER_RANGE, GYRO_RANGE, 0, PidCompassProfile.PID_Out[0]);
|
|
}
|
|
|
|
break;
|
|
case PID_ID_ACCELEROMETER:
|
|
desiredCommand[ROLL] = convertData(RADIO_RANGE, ACCELEROMETER_RANGE, 0, RawRcCommand.Roll);
|
|
desiredCommand[PITCH] = convertData(RADIO_RANGE, ACCELEROMETER_RANGE, 0, RawRcCommand.Pitch);
|
|
break;
|
|
case PID_ID_COMPASS:
|
|
desiredCommand[0] = convertData(RADIO_RANGE, COMPASS_RANGE, 0, RawRcCommand.Yaw); /*desiredCommand must have an index of 0, since its only loop once*/
|
|
break;
|
|
case PID_ID_BAROMETER:
|
|
desiredCommand[THROTTLE] = convertData(RADIO_RANGE,BAROMETER_RANGE,0, RawRcCommand.Throttle);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Initializes the pid profile PID controller *
|
|
* INFORMATION: Recommended to use if unexpected values occur of profile *
|
|
**************************************************************************/
|
|
void pidUAVInit(pidProfile_t *pidProfile)
|
|
{
|
|
/*This is recommended init settings*/
|
|
pidProfile->ID_profile = 0;
|
|
|
|
pidProfile-> pidStabilisationEnabled = true;
|
|
|
|
pidProfile->DOF = 0;
|
|
|
|
pidProfile->P[ROLL] = 10;
|
|
pidProfile->P[PITCH] = 10;
|
|
pidProfile->P[YAW] = 10;
|
|
|
|
pidProfile->I[ROLL] = 0;
|
|
pidProfile->I[PITCH] = 0;
|
|
pidProfile->I[YAW] = 0;
|
|
|
|
pidProfile->D[ROLL] = 0;
|
|
pidProfile->D[PITCH] = 0;
|
|
pidProfile->D[YAW] = 0;
|
|
|
|
pidProfile->Max_PID_Term = 3000;
|
|
pidProfile->Max_Error_Term = 5000;
|
|
|
|
pidProfile->Max_P_Term = 0;
|
|
pidProfile->Max_I_Term = 0;
|
|
pidProfile->Max_D_Term = 0;
|
|
|
|
|
|
pidProfile->dterm_lpf = 0;
|
|
pidProfile->yaw_lpf = 0;
|
|
|
|
pidProfile->Integral[ROLL] = 0;
|
|
pidProfile->Integral[PITCH] = 0;
|
|
pidProfile->Integral[YAW] = 0;
|
|
|
|
pidProfile->LastError[ROLL] = 0;
|
|
pidProfile->LastError[PITCH] = 0;
|
|
pidProfile->LastError[YAW] = 0;
|
|
|
|
pidProfile->PID_Out[ROLL] = 0;
|
|
pidProfile->PID_Out[PITCH] = 0;
|
|
pidProfile->PID_Out[YAW] = 0;
|
|
|
|
pidProfile->deltaFilter[ROLL].RC = 0;
|
|
pidProfile->deltaFilter[ROLL].dT = 0;
|
|
pidProfile->deltaFilter[ROLL].state = 0;
|
|
|
|
pidProfile->deltaFilter[PITCH].RC = 0;
|
|
pidProfile->deltaFilter[PITCH].dT = 0;
|
|
pidProfile->deltaFilter[PITCH].state = 0;
|
|
|
|
pidProfile->deltaFilter[YAW].RC = 0;
|
|
pidProfile->deltaFilter[YAW].dT = 0;
|
|
pidProfile->deltaFilter[YAW].state = 0;
|
|
|
|
pidProfile->yawFilter.RC = 0;
|
|
pidProfile->yawFilter.dT = 0;
|
|
pidProfile->yawFilter.state = 0;
|
|
|
|
pidProfile->dT = 1;
|
|
|
|
}
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Initializes PID profiles *
|
|
* INFORMATION: *
|
|
**************************************************************************/
|
|
void pidInit()
|
|
{
|
|
pidUAVInit(&PidGyroProfile);
|
|
pidUAVInit(&PidAccelerometerProfile);
|
|
pidUAVInit(&PidCompassProfile);
|
|
pidUAVInit(&PidBarometerProfile);
|
|
}
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Dynamic PID controller, able to handle several PID controller *
|
|
* connected to different profiles.
|
|
* INFORMATION: *
|
|
**************************************************************************/
|
|
void pidUAV(pidProfile_t *pidProfile)
|
|
{
|
|
int DOF = pidProfile->DOF;
|
|
|
|
float errorRate;
|
|
float Derivative;
|
|
|
|
float ITerm, PTerm, DTerm;
|
|
float PID_Out[XYZ_AXIS_COUNT];
|
|
|
|
float errorAxis[3] = {0}; /*Array of errors for each axis*/
|
|
float pointRate[3] = {0}; /*Array of desired values for each axis*/
|
|
|
|
/*PID values*/
|
|
float Kp[3] = {0};
|
|
float Ki[3] = {0};
|
|
float Kd[3] = {0};
|
|
|
|
getCurrentValues(errorAxis, pidProfile->ID_profile); /*Get sensor values*/
|
|
getPointRate(pointRate, pidProfile->ID_profile); /*Get reference values or desired values*/
|
|
|
|
/* -------------PID controller------------- */
|
|
for (int axis = 0; axis < DOF; axis++)
|
|
{
|
|
|
|
errorRate = pointRate[axis] - errorAxis[axis]; /*Calculates current error of one axis*/
|
|
errorRate = constrainf(errorRate, -pidProfile->Max_Error_Term, pidProfile->Max_Error_Term); /*Limits the error if necessary*/
|
|
|
|
Kp[axis] = PTERM_SCALE * pidProfile->P[axis]; /*Calculates Kp, Ki, Kd value with scale values*/
|
|
Ki[axis] = ITERM_SCALE * pidProfile->I[axis];
|
|
Kd[axis] = DTERM_SCALE * pidProfile->D[axis];
|
|
|
|
|
|
/*--------P term--------*/
|
|
PTerm = Kp[axis] * errorRate;
|
|
|
|
if (axis == YAW)
|
|
{
|
|
/*Low pass filter on P term for Yaw axis*/
|
|
if (pidProfile->yaw_lpf)
|
|
{
|
|
PTerm = pt1FilterApply4(&pidProfile->yawFilter, PTerm, pidProfile->yaw_lpf, pidProfile->dT);
|
|
}
|
|
|
|
/*Constrains PTerm if needed*/
|
|
if (pidProfile->Max_P_Term)
|
|
{
|
|
PTerm = constrainf(PTerm, -pidProfile->Max_P_Term, pidProfile->Max_P_Term);
|
|
}
|
|
}
|
|
|
|
|
|
/*--------I term--------*/
|
|
pidProfile->Integral[axis] = pidProfile->Integral[axis] + errorRate;
|
|
ITerm = Ki[axis] * pidProfile->Integral[axis];
|
|
|
|
/*Limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated*/
|
|
if (pidProfile->Max_I_Term)
|
|
{
|
|
ITerm = constrainf(ITerm, -pidProfile->Max_I_Term, pidProfile->Max_I_Term);
|
|
}
|
|
|
|
|
|
/*--------D term--------*/
|
|
Derivative = errorRate - pidProfile->LastError[axis];
|
|
pidProfile->LastError[axis] = errorRate;
|
|
|
|
/*Divide delta by dT to get differential (ie dr/dt)*/
|
|
Derivative *= (1.0f / pidProfile->dT);
|
|
|
|
/*DTerm delta low pass filter*/
|
|
if (pidProfile->dterm_lpf)
|
|
{
|
|
Derivative = pt1FilterApply4(&pidProfile->deltaFilter[axis], Derivative, pidProfile->dterm_lpf, pidProfile->dT);
|
|
}
|
|
|
|
DTerm = Kd[axis] * Derivative;
|
|
|
|
|
|
/* --------------PID Out ----------------- */
|
|
PID_Out[axis] = PTerm + ITerm + DTerm;
|
|
|
|
/*Constrains PID out term*/
|
|
PID_Out[axis] = constrainf(PID_Out[axis], -pidProfile->Max_PID_Term, pidProfile->Max_PID_Term);
|
|
|
|
if (!pidProfile->pidStabilisationEnabled) PID_Out[axis] = 0;
|
|
|
|
/*Update PID out values to profile*/
|
|
pidProfile->PID_Out[axis] = PID_Out[axis];
|
|
}
|
|
}
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Runs a certain PID Controller *
|
|
* INFORMATION: *
|
|
**************************************************************************/
|
|
void pidRun(uint8_t ID)
|
|
{
|
|
switch(ID)
|
|
{
|
|
|
|
case PID_ID_GYRO:
|
|
|
|
if (!PidGyroProfile.pidEnabled)
|
|
{
|
|
pidUAV(&PidGyroProfile);
|
|
}
|
|
else
|
|
{
|
|
PidGyroProfile.PID_Out[ROLL] = RawRcCommand.Roll;
|
|
PidGyroProfile.PID_Out[PITCH] = RawRcCommand.Pitch;
|
|
PidGyroProfile.PID_Out[YAW] = RawRcCommand.Yaw;
|
|
}
|
|
|
|
break;
|
|
case PID_ID_ACCELEROMETER:
|
|
|
|
if (!PidAccelerometerProfile.pidEnabled)
|
|
{
|
|
PidAccelerometerProfile.PID_Out[ROLL] = RawRcCommand.Roll;
|
|
PidAccelerometerProfile.PID_Out[PITCH] = RawRcCommand.Pitch;
|
|
}
|
|
else
|
|
{
|
|
pidUAV(&PidAccelerometerProfile);
|
|
}
|
|
|
|
break;
|
|
case PID_ID_COMPASS:
|
|
|
|
if (!PidCompassProfile.pidEnabled)
|
|
{
|
|
PidCompassProfile.PID_Out[0] = RawRcCommand.Yaw;
|
|
}
|
|
else
|
|
{
|
|
pidUAV(&PidCompassProfile);
|
|
}
|
|
|
|
break;
|
|
case PID_ID_BAROMETER:
|
|
|
|
if (!PidBarometerProfile.pidEnabled)
|
|
{
|
|
PidBarometerProfile.PID_Out[0] = RawRcCommand.Throttle;
|
|
}
|
|
else
|
|
{
|
|
pidUAV(&PidBarometerProfile);
|
|
}
|
|
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|