622 lines
19 KiB
C
622 lines
19 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/sbus.h"
|
|
#include "scheduler/scheduler.h"
|
|
#include <math.h>
|
|
#include "drivers/failsafe_toggles.h"
|
|
#include "drivers/motormix.h"
|
|
|
|
|
|
#define PTERM_SCALE 0.032029f /*P-term used as a scale value to the PID controller*/
|
|
#define ITERM_SCALE 0.0012f /*I-term used as a scale value to the PID controller*/
|
|
#define DTERM_SCALE 0.000529f /*D-term used as a scale value to the PID controller*/
|
|
|
|
#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 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 COMPASS_RANGE 180 /*Determines the maximum compass limit (limits the rc input)*/
|
|
|
|
#define PID_MAX_I 256 /*Constrains ITerm*/
|
|
#define PID_MAX_D 512 /*Constrains DTerm*/
|
|
|
|
/*Struct that belongs to a certain PID controller*/
|
|
typedef struct pidProfileBuff_s {
|
|
|
|
uint8_t ID_profile; /*ID of a certain PID, shall be referenced to a certain sensor*/
|
|
uint8_t DOF; /*DOF = degrees of freedom*/
|
|
|
|
pt1Filter_t deltaFilter[XYZ_AXIS_COUNT]; /*Struct, Contains filter value for each PID of D term*/
|
|
pt1Filter_t yawFilter; /*Struct, Contains filter value for each PID of p term for yaw axis*/
|
|
|
|
float ITermLimit[XYZ_AXIS_COUNT];
|
|
float lastITerm[XYZ_AXIS_COUNT]; /*Buffer which contains integral values, do not to be init */
|
|
float LastRateForError[XYZ_AXIS_COUNT]; /*Buffer which contains last error values, do not to be init */
|
|
float dT; /*Delta time of each PID cycle*/
|
|
|
|
} pidProfileBuff_t;
|
|
|
|
|
|
pidProfile_t PidProfile[PID_COUNT] = {0}; /*Array of all pid profiles of the system*/
|
|
pidProfileBuff_t PidProfileBuff[PID_COUNT] = {0};
|
|
|
|
accel_t accelProfile = {0}; /*Struct profile for input data from sensor*/
|
|
gyro_t gyroProfile = {0}; /*Struct profile for input data from sensor*/
|
|
|
|
pt1Filter_t accelFilter[2] = {0};
|
|
|
|
float accRollFineTune = 0;
|
|
float accPitchFineTune = 0;
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Calculates angle from accelerometer *
|
|
* INFORMATION: *
|
|
**************************************************************************/
|
|
float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, const float z_axis)
|
|
{
|
|
float angle;
|
|
|
|
switch (axis)
|
|
{
|
|
case ROLL:
|
|
|
|
angle = atan2(x_axis, sqrt(y_axis*y_axis + z_axis*z_axis))*180/M_PI;
|
|
angle = -1*((angle > 0)? (z_axis < 0 )? 180 - angle: angle : (z_axis < 0 )? - 180 - angle: angle);
|
|
|
|
break;
|
|
case PITCH:
|
|
|
|
angle = atan2( y_axis, sqrt(z_axis*z_axis + x_axis*x_axis))*180/M_PI; /*down (the front down against ground) = pos angle*/
|
|
angle = (angle > 0)? ((z_axis < 0))? 180 - angle: angle : (z_axis < 0 )? - 180 - angle: angle;
|
|
|
|
break;
|
|
default:
|
|
angle = 0;
|
|
break;
|
|
}
|
|
|
|
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 *
|
|
* INFORMATION: *
|
|
**************************************************************************/
|
|
float convertData(int inputRange, int outputRange, int offset, float value)
|
|
{
|
|
return ((float)outputRange/(float)inputRange)*(value-(float)offset);
|
|
//return 1.0;
|
|
}
|
|
|
|
/**************************************************************************
|
|
* 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;
|
|
}
|
|
|
|
|
|
float oldSensorValue[2] = {0};
|
|
float oldSensorValueRoll[12] = {0};
|
|
float oldSensorValuePitch[12] = {0};
|
|
|
|
int i = 0;
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Update current sensor values *
|
|
* INFORMATION: *
|
|
**************************************************************************/
|
|
void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
|
|
{
|
|
|
|
|
|
switch (ID_profile)
|
|
{
|
|
case PID_ID_GYRO:
|
|
|
|
mpu6000_read_gyro(&gyroProfile); /*Reads data from gyro*/
|
|
|
|
sensorValues[ROLL] = gyroProfile.gyroY;
|
|
sensorValues[PITCH] = gyroProfile.gyroX;
|
|
sensorValues[YAW] = gyroProfile.gyroZ;
|
|
|
|
break;
|
|
case PID_ID_ACCELEROMETER:
|
|
|
|
mpu6000_read_accel(&accelProfile); /*Reads data from accelerometer*/
|
|
|
|
|
|
float alpha = 0.5;
|
|
/*May need Low pass filter since the accelerometer may drift*/
|
|
|
|
float X_roll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
|
|
float X_pitch = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
|
|
|
|
/*TODO add finetune for roll and pitch*/
|
|
X_roll += accRollFineTune;
|
|
X_pitch += accPitchFineTune;
|
|
|
|
|
|
oldSensorValueRoll[i] = X_roll;
|
|
oldSensorValuePitch[i] = X_pitch;
|
|
|
|
float RollValue = 0;
|
|
float PitchValue = 0;
|
|
|
|
for (int ii = 0; ii < 12; ii++)
|
|
{
|
|
RollValue = RollValue + oldSensorValueRoll[ii];
|
|
PitchValue = PitchValue + oldSensorValuePitch[ii];
|
|
|
|
}
|
|
|
|
|
|
i = (i < 11)? i + 1:0;
|
|
|
|
sensorValues[ROLL] = RollValue/12;
|
|
sensorValues[PITCH] = PitchValue/12;
|
|
|
|
sensorValues[ROLL] = alpha*RollValue/12 + (1-alpha)*oldSensorValue[0];
|
|
sensorValues[PITCH] = alpha*PitchValue/12 + (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);
|
|
|
|
//float sensorPitch = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
|
|
//sensorValues[PITCH] = pt1FilterApply4(&accelFilter[1], sensorPitch, 90, PidProfileBuff[PITCH].dT);
|
|
|
|
break;
|
|
case PID_ID_COMPASS:
|
|
case PID_ID_BAROMETER:
|
|
default:
|
|
|
|
sensorValues[ROLL] = 0;
|
|
sensorValues[PITCH] = 0;
|
|
sensorValues[YAW] = 0;
|
|
|
|
break;
|
|
}
|
|
};
|
|
|
|
|
|
/**************************************************************************
|
|
* 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 (!(PidProfile[PID_ID_ACCELEROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_acceleromter_id)))
|
|
{
|
|
desiredCommand[ROLL] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL]);
|
|
desiredCommand[PITCH] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH]);
|
|
}
|
|
else
|
|
{
|
|
desiredCommand[ROLL] = convertData(ACCELEROMETER_RANGE, GYRO_RANGE, 0, PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL]);
|
|
desiredCommand[PITCH] = convertData(ACCELEROMETER_RANGE, GYRO_RANGE, 0, PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH]);
|
|
}
|
|
|
|
|
|
if (!PidProfile[PID_ID_COMPASS].pidEnabled)
|
|
{
|
|
desiredCommand[YAW] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidProfile[PID_ID_COMPASS].PID_Out[0]);
|
|
}
|
|
else
|
|
{
|
|
desiredCommand[YAW] = convertData(COMPASS_RANGE, GYRO_RANGE, 0, PidProfile[PID_ID_COMPASS].PID_Out[0]);
|
|
}
|
|
|
|
break;
|
|
case PID_ID_ACCELEROMETER:
|
|
|
|
desiredCommand[ROLL] = convertData(RADIO_RANGE, ACCELEROMETER_RANGE, 0, rc_input.Roll);
|
|
desiredCommand[PITCH] = convertData(RADIO_RANGE, ACCELEROMETER_RANGE, 0, rc_input.Pitch);
|
|
|
|
break;
|
|
case PID_ID_COMPASS:
|
|
|
|
desiredCommand[0] = convertData(RADIO_RANGE, COMPASS_RANGE, 0, rc_input.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, rc_input.Throttle);
|
|
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
/**************************************************************************
|
|
* BRIEF: The PID core *
|
|
* INFORMATION: The actual PID controller, calculates the final output of *
|
|
* the controller
|
|
**************************************************************************/
|
|
void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
|
|
float desiredValue, float errorAxis, uint8_t axis)
|
|
{
|
|
const float rateError = desiredValue - errorAxis;
|
|
|
|
|
|
/* -----calculate P component ---- */
|
|
float PTerm = PTERM_SCALE * rateError * (float)pidProfile->P[axis] * (float)pidProfile-> PIDweight[axis] / 100.0;
|
|
|
|
// Constrain YAW by yaw_p_limit value
|
|
if (axis == YAW)
|
|
{
|
|
/*pidProfile->pterm_yaw_lpf = the cut of frequency*/
|
|
if (pidProfile->pterm_yaw_lpf)
|
|
{
|
|
PTerm = pt1FilterApply4(&pidProfileBuff->yawFilter, PTerm, pidProfile->pterm_yaw_lpf, pidProfileBuff->dT);
|
|
}
|
|
/*Limits the PTerm of the Yaw axis */
|
|
if (pidProfile->yaw_p_limit)
|
|
{
|
|
PTerm = constrainf(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
|
|
}
|
|
}
|
|
|
|
|
|
/* -----calculate I component ---- */
|
|
float ITerm = pidProfileBuff->lastITerm[axis] + ITERM_SCALE * rateError * pidProfileBuff->dT * (float)pidProfile->I[axis];
|
|
|
|
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
|
|
// I coefficient (I) moved before integration to make limiting independent from PID settings
|
|
ITerm = constrainf(ITerm, -PID_MAX_I, PID_MAX_I);
|
|
|
|
// Anti windup protection
|
|
if (motorLimitReached)
|
|
{
|
|
ITerm = constrainf(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]);
|
|
}
|
|
else
|
|
{
|
|
pidProfileBuff->ITermLimit[axis] = abs(ITerm);
|
|
}
|
|
|
|
pidProfileBuff->lastITerm[axis] = ITerm;
|
|
|
|
|
|
/* -----calculate D component ---- */
|
|
float DTerm;
|
|
|
|
if (pidProfile->D[axis] == 0)
|
|
{
|
|
// Optimization when D is zero, often used by YAW axis
|
|
DTerm = 0;
|
|
}
|
|
else
|
|
{
|
|
float delta;
|
|
|
|
delta = rateError - pidProfileBuff->LastRateForError[axis];
|
|
pidProfileBuff->LastRateForError[axis] = rateError;
|
|
|
|
// Divide delta by dT to get differential (ie dr/dt)
|
|
delta *= (1.0f / pidProfileBuff->dT);
|
|
|
|
/*pidProfile->dterm_lpf = cut of frequency for low pass filter*/
|
|
if (pidProfile->dterm_lpf)
|
|
{
|
|
// DTerm delta low pass filter
|
|
delta = pt1FilterApply4(&pidProfileBuff->deltaFilter[axis], delta, pidProfile->dterm_lpf, pidProfileBuff->dT);
|
|
}
|
|
|
|
DTerm = DTERM_SCALE * delta * (float)pidProfile->D[axis] * (float)pidProfile->PIDweight[axis] / 100.0;
|
|
DTerm = constrainf(DTerm, -PID_MAX_D, PID_MAX_D);
|
|
}
|
|
|
|
|
|
/*----PID OUT----*/
|
|
if(!flags_IsSet_ID(systemFlags_armed_id) || (rc_input.Throttle < mixerConfig.minCheck))
|
|
{
|
|
ITerm = 0;
|
|
pidProfileBuff->lastITerm[axis] = 0;
|
|
}
|
|
pidProfile->PID_Out[axis] = constrainf(PTerm + ITerm + DTerm, -pidProfile->pid_out_limit, pidProfile->pid_out_limit);
|
|
}
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Dynamic PID controller, able to handle several PID controller *
|
|
* connected to different profiles.
|
|
* INFORMATION: *
|
|
**************************************************************************/
|
|
void pidUAV(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff)
|
|
{
|
|
float errorAxis[3] = { 0 }; /*Array of errors for each axis*/
|
|
float pointRate[3] = { 0 }; /*Array of desired values for each axis*/
|
|
|
|
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 < pidProfileBuff->DOF; axis++)
|
|
{
|
|
pidUAVcore(pidProfile, pidProfileBuff, pointRate[axis], errorAxis[axis], axis);
|
|
}
|
|
}
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Runs a certain PID Controller *
|
|
* INFORMATION: *
|
|
**************************************************************************/
|
|
void pidRun(uint8_t ID)
|
|
{
|
|
switch(ID)
|
|
{
|
|
|
|
case PID_ID_GYRO:
|
|
|
|
if (!PidProfile[PID_ID_GYRO].pidEnabled)
|
|
{
|
|
PidProfile[PID_ID_GYRO].PID_Out[ROLL] = rc_input.Roll;
|
|
PidProfile[PID_ID_GYRO].PID_Out[PITCH] = rc_input.Pitch;
|
|
PidProfile[PID_ID_GYRO].PID_Out[YAW] = rc_input.Yaw;
|
|
}
|
|
else
|
|
{
|
|
pidUAV(&PidProfile[PID_ID_GYRO], &PidProfileBuff[PID_ID_GYRO]);
|
|
}
|
|
|
|
break;
|
|
case PID_ID_ACCELEROMETER:
|
|
|
|
if (!(PidProfile[PID_ID_ACCELEROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_acceleromter_id)))
|
|
{
|
|
PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL] = rc_input.Roll;
|
|
PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH] = rc_input.Pitch;
|
|
}
|
|
else
|
|
{
|
|
pidUAV(&PidProfile[PID_ID_ACCELEROMETER], &PidProfileBuff[PID_ID_ACCELEROMETER]);
|
|
}
|
|
|
|
break;
|
|
case PID_ID_COMPASS:
|
|
|
|
if (!PidProfile[PID_ID_COMPASS].pidEnabled)
|
|
{
|
|
PidProfile[PID_ID_COMPASS].PID_Out[0] = rc_input.Yaw;
|
|
}
|
|
else
|
|
{
|
|
pidUAV(&PidProfile[PID_ID_COMPASS], &PidProfileBuff[PID_ID_COMPASS]);
|
|
}
|
|
|
|
break;
|
|
case PID_ID_BAROMETER:
|
|
|
|
if (!PidProfile[PID_ID_BAROMETER].pidEnabled)
|
|
{
|
|
PidProfile[PID_ID_BAROMETER].PID_Out[0] = rc_input.Throttle;
|
|
}
|
|
else
|
|
{
|
|
pidUAV(&PidProfile[PID_ID_BAROMETER], &PidProfileBuff[PID_ID_BAROMETER]);
|
|
}
|
|
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
/*--------------------------------------------Init Functions----------------------------------------------------------------------------------*/
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Initializes a certain pidbuffer profile connected to *
|
|
* a PID controller *
|
|
* INFORMATION: Recommended to use if unexpected values occur of profile *
|
|
**************************************************************************/
|
|
void pidUAVInitBuff(pidProfileBuff_t *pidProfile, uint8_t ID)
|
|
{
|
|
/*This is recommended init settings*/
|
|
PidProfileBuff[ID].DOF = 1;
|
|
|
|
switch (ID)
|
|
{
|
|
case PID_ID_GYRO:
|
|
|
|
PidProfileBuff[ID].DOF = 3;
|
|
PidProfileBuff[ID].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000; //ÄNDRA TILL SEKUNDER inte ms
|
|
|
|
break;
|
|
case PID_ID_ACCELEROMETER:
|
|
|
|
PidProfileBuff[ID].DOF = 2;
|
|
PidProfileBuff[ID].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000;
|
|
|
|
break;
|
|
case PID_ID_COMPASS:
|
|
|
|
PidProfileBuff[ID].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000;
|
|
|
|
break;
|
|
case PID_ID_BAROMETER:
|
|
|
|
PidProfileBuff[ID].dT = SystemTasks[TASK_BARO].desiredPeriod/1000;
|
|
|
|
break;
|
|
default:
|
|
|
|
break;
|
|
};
|
|
|
|
PidProfileBuff[ID].ID_profile = ID;
|
|
|
|
PidProfileBuff[ID].ITermLimit[ROLL] = 0;
|
|
PidProfileBuff[ID].ITermLimit[PITCH] = 0;
|
|
PidProfileBuff[ID].ITermLimit[YAW] = 0;
|
|
|
|
PidProfileBuff[ID].LastRateForError[ROLL] = 0;
|
|
PidProfileBuff[ID].LastRateForError[PITCH] = 0;
|
|
PidProfileBuff[ID].LastRateForError[YAW] = 0;
|
|
|
|
PidProfileBuff[ID].deltaFilter[ROLL].RC = 0;
|
|
PidProfileBuff[ID].deltaFilter[PITCH].RC = 0;
|
|
PidProfileBuff[ID].deltaFilter[YAW].RC = 0;
|
|
|
|
PidProfileBuff[ID].lastITerm[ROLL] = 0;
|
|
PidProfileBuff[ID].lastITerm[PITCH] = 0;
|
|
PidProfileBuff[ID].lastITerm[YAW] = 0;
|
|
|
|
PidProfileBuff[ID].yawFilter.RC = 0;
|
|
}
|
|
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Initializes a certain pid profile connected to a PID controller *
|
|
* INFORMATION: Recommended to use if unexpected values occur of profile *
|
|
**************************************************************************/
|
|
void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
|
|
{
|
|
/*This is recommended init settings*/
|
|
PidProfile[ID].ID_profile = ID;
|
|
|
|
PidProfile[ID].PID_Out[ROLL] = 0;
|
|
PidProfile[ID].PID_Out[PITCH] = 0;
|
|
PidProfile[ID].PID_Out[YAW] = 0;
|
|
|
|
|
|
|
|
|
|
switch (ID)
|
|
{
|
|
case PID_ID_GYRO:
|
|
|
|
PidProfile[ID].P[ROLL] = 150;
|
|
PidProfile[ID].P[PITCH] = 135;
|
|
PidProfile[ID].P[YAW] = 150;
|
|
|
|
PidProfile[ID].D[ROLL] = 75;
|
|
PidProfile[ID].D[PITCH] = 95;
|
|
PidProfile[ID].D[YAW] = 50;
|
|
|
|
PidProfile[ID].PIDweight[ROLL] = 100;
|
|
PidProfile[ID].PIDweight[PITCH] = 100;
|
|
PidProfile[ID].PIDweight[YAW] = 100;
|
|
|
|
PidProfile[ID].pidEnabled = true;
|
|
PidProfile[ID].dterm_lpf = 90;
|
|
PidProfile[ID].pid_out_limit = 3000;
|
|
|
|
break;
|
|
case PID_ID_ACCELEROMETER:
|
|
|
|
PidProfile[ID].P[ROLL] = 90;
|
|
PidProfile[ID].P[PITCH] = 90;
|
|
PidProfile[ID].P[YAW] = 0;
|
|
|
|
PidProfile[ID].D[ROLL] = 40;
|
|
PidProfile[ID].D[PITCH] = 40;
|
|
PidProfile[ID].D[YAW] = 0;
|
|
|
|
PidProfile[ID].PIDweight[ROLL] = 2;
|
|
PidProfile[ID].PIDweight[PITCH] = 2;
|
|
PidProfile[ID].PIDweight[YAW] = 100;
|
|
|
|
PidProfile[ID].pidEnabled = true;
|
|
PidProfile[ID].dterm_lpf = 90;
|
|
PidProfile[ID].pid_out_limit = 1000;
|
|
|
|
break;
|
|
case PID_ID_COMPASS:
|
|
|
|
PidProfile[ID].P[ROLL] = 10;
|
|
PidProfile[ID].P[PITCH] = 10;
|
|
PidProfile[ID].P[YAW] = 10;
|
|
|
|
PidProfile[ID].PIDweight[ROLL] = 100;
|
|
PidProfile[ID].PIDweight[PITCH] = 100;
|
|
PidProfile[ID].PIDweight[YAW] = 100;
|
|
|
|
PidProfile[ID].pidEnabled = false;
|
|
|
|
break;
|
|
case PID_ID_BAROMETER:
|
|
|
|
PidProfile[ID].P[ROLL] = 10;
|
|
PidProfile[ID].P[PITCH] = 10;
|
|
PidProfile[ID].P[YAW] = 10;
|
|
|
|
PidProfile[ID].PIDweight[ROLL] = 100;
|
|
PidProfile[ID].PIDweight[PITCH] = 100;
|
|
PidProfile[ID].PIDweight[YAW] = 100;
|
|
|
|
PidProfile[ID].pidEnabled = false;
|
|
|
|
break;
|
|
default:
|
|
|
|
break;
|
|
};
|
|
|
|
}
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Initializes PID profiles *
|
|
* INFORMATION: *
|
|
**************************************************************************/
|
|
void pidInit()
|
|
{
|
|
mpu6000_init(&gyroProfile,&accelProfile); /*Init gyro and accelerometer*/
|
|
motorLimitReached = false;
|
|
|
|
pidUAVInitBuff(&PidProfileBuff[PID_ID_GYRO], PID_ID_GYRO);
|
|
pidUAVInitBuff(&PidProfileBuff[PID_ID_ACCELEROMETER], PID_ID_ACCELEROMETER);
|
|
pidUAVInitBuff(&PidProfileBuff[PID_ID_BAROMETER], PID_ID_BAROMETER);
|
|
pidUAVInitBuff(&PidProfileBuff[PID_ID_COMPASS], PID_ID_COMPASS);
|
|
|
|
pidUAVInit(&PidProfile[PID_ID_GYRO], PID_ID_GYRO);
|
|
pidUAVInit(&PidProfile[PID_ID_ACCELEROMETER], PID_ID_ACCELEROMETER);
|
|
pidUAVInit(&PidProfile[PID_ID_BAROMETER], PID_ID_BAROMETER);
|
|
pidUAVInit(&PidProfile[PID_ID_COMPASS], PID_ID_COMPASS);
|
|
}
|
|
|
|
void pidEproom(void)
|
|
{
|
|
|
|
PidProfile[PID_ID_ACCELEROMETER].PIDweight[ROLL] = 2;
|
|
PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 2;
|
|
PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100;
|
|
|
|
}
|