541 lines
17 KiB
C
541 lines
17 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"
|
||
#include "utilities.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 3 /*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 throttleRate = 1;
|
||
float accRollFineTune = 0;
|
||
float accPitchFineTune = 0;
|
||
|
||
|
||
/**************************************************************************
|
||
* 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);
|
||
}
|
||
|
||
/**************************************************************************
|
||
* 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_angle(&accelProfile,&gyroProfile); /*Reads data from accelerometer*/
|
||
|
||
sensorValues[ROLL] = accelProfile.rollAngle + accRollFineTune;
|
||
sensorValues[PITCH] = accelProfile.pitchAngle + accPitchFineTune;
|
||
|
||
/*Checks the biggest angle */
|
||
throttleRate = 2 - (ABS_FLOAT(sensorValues[ROLL]) > ABS_FLOAT(sensorValues[PITCH]) )? cos(sensorValues[ROLL]*M_PI/180) : cos(sensorValues[PITCH]*M_PI/180);
|
||
|
||
break;
|
||
case PID_ID_COMPASS:
|
||
|
||
sensorValues[ROLL] = 0;
|
||
sensorValues[PITCH] = 0;
|
||
sensorValues[YAW] = 0;
|
||
|
||
|
||
break;
|
||
case PID_ID_BAROMETER:
|
||
|
||
sensorValues[0] = barometer_GetCurrentAveragedtAltitude();
|
||
break;
|
||
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*throttleRate);
|
||
|
||
desiredCommand[THROTTLE] = 1.9;
|
||
|
||
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.
|
||
ITerm = constrainf(ITerm, -(int)PID_MAX_I, (int)PID_MAX_I);
|
||
|
||
// Anti windup protection
|
||
if (motorLimitReached)
|
||
{
|
||
ITerm = constrainf(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]);
|
||
}
|
||
else
|
||
{
|
||
pidProfileBuff->ITermLimit[axis] = ABS_FLOAT(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;
|
||
pidProfileBuff->ITermLimit[axis] = 0;
|
||
}
|
||
pidProfile->PID_Out[axis] = constrainf(PTerm + ITerm + DTerm, -(int)pidProfile->pid_out_limit, (int)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 && flags_IsSet_ID(systemFlags_flightmode_barometer_id)))
|
||
{
|
||
PidProfile[PID_ID_BAROMETER].PID_Out[0] = rc_input.Throttle*throttleRate;
|
||
}
|
||
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/1000000; //<2F>NDRA TILL SEKUNDER inte ms
|
||
|
||
break;
|
||
case PID_ID_ACCELEROMETER:
|
||
|
||
PidProfileBuff[ID].DOF = 2;
|
||
PidProfileBuff[ID].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000000;
|
||
|
||
break;
|
||
case PID_ID_COMPASS:
|
||
|
||
PidProfileBuff[ID].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000000;
|
||
|
||
break;
|
||
case PID_ID_BAROMETER:
|
||
|
||
PidProfileBuff[ID].dT = SystemTasks[TASK_BARO].desiredPeriod/1000000;
|
||
|
||
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].I[ROLL] = 50;
|
||
PidProfile[ID].I[PITCH] = 50;
|
||
PidProfile[ID].I[YAW] = 50;
|
||
|
||
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] = 120;
|
||
PidProfile[ID].P[PITCH] = 250;
|
||
PidProfile[ID].P[YAW] = 0;
|
||
|
||
PidProfile[ID].D[ROLL] = 0;
|
||
PidProfile[ID].D[PITCH] = 0;
|
||
PidProfile[ID].D[YAW] = 0;
|
||
|
||
PidProfile[ID].PIDweight[ROLL] = 4;
|
||
PidProfile[ID].PIDweight[PITCH] = 4;
|
||
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].PIDweight[ROLL] = 100;
|
||
|
||
PidProfile[ID].pidEnabled = false;
|
||
|
||
break;
|
||
case PID_ID_BAROMETER:
|
||
|
||
PidProfile[ID].P[ROLL] = 1;
|
||
|
||
PidProfile[ID].PIDweight[ROLL] = 100;
|
||
|
||
PidProfile[ID].pidEnabled = false;
|
||
PidProfile[ID].dterm_lpf = 90;
|
||
PidProfile[ID].pid_out_limit = 1000;
|
||
|
||
break;
|
||
default:
|
||
|
||
break;
|
||
};
|
||
|
||
}
|
||
|
||
/**************************************************************************
|
||
* BRIEF: Initializes PID profiles *
|
||
* INFORMATION: *
|
||
**************************************************************************/
|
||
void pidInit()
|
||
{
|
||
mpu6000_init(&gyroProfile,&accelProfile); /*Init gyro and accelerometer*/
|
||
|
||
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_BAROMETER].PIDweight[ROLL] = 200;
|
||
|
||
PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100;
|
||
|
||
PidProfileBuff[PID_ID_GYRO].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000; //<2F>NDRA TILL SEKUNDER inte ms
|
||
PidProfileBuff[PID_ID_ACCELEROMETER].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000;
|
||
PidProfileBuff[PID_ID_COMPASS].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000;
|
||
PidProfileBuff[PID_ID_BAROMETER].dT = SystemTasks[TASK_BARO].desiredPeriod/1000;
|
||
|
||
|
||
}
|