121 lines
5.6 KiB
C
121 lines
5.6 KiB
C
/**************************************************************************
|
|
* NAME: pid.h *
|
|
* 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 *
|
|
* -------- ---- ----------- *
|
|
*
|
|
* **************************************************************************/
|
|
#ifndef FLIGHT_PID_H_
|
|
#define FLIGHT_PID_H_
|
|
|
|
#include<stdio.h>
|
|
#include<stdint.h>
|
|
|
|
#include "Flight/filter.h"
|
|
|
|
#define PTERM_SCALE 0.032029f /*P-term used as a scale value to the PID controller*/
|
|
#define ITERM_SCALE 0.244381f /*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 XYZ_AXIS_COUNT 3 /*The maximum number of DOF that belongings to the PID*/
|
|
|
|
#define PID_ID_GYRO 0
|
|
#define PID_ID_ACCELEROMETER 1
|
|
#define PID_ID_COMPASS 2
|
|
#define PID_ID_BAROMETER 3
|
|
|
|
#define THROTTLE 0 /*Index terms to the PID*/
|
|
#define ROLL 0 /*Index terms to the PID*/
|
|
#define PITCH 1 /*Index terms to the PID*/
|
|
#define YAW 2 /*Index terms to the PID*/
|
|
|
|
|
|
/*Struct that belongs to a certain PID controller*/
|
|
typedef struct pidProfile_s {
|
|
|
|
bool pidEnabled;
|
|
bool pidStabilisationEnabled; /*Enables/Dissables PID controller*/
|
|
|
|
uint8_t ID_profile; /*ID of a certain PID, shall be referenced to a certain sensor*/
|
|
uint8_t DOF; /*DOF = degrees of freedom*/
|
|
|
|
uint8_t P[XYZ_AXIS_COUNT]; /*PID value*/
|
|
uint8_t I[XYZ_AXIS_COUNT]; /*PID value*/
|
|
uint8_t D[XYZ_AXIS_COUNT]; /*PID value*/
|
|
|
|
int16_t Max_PID_Term; /*The max and minimum output value of the PID*/
|
|
int16_t Max_Error_Term; /*The max and minimum output value of the PID error*/
|
|
|
|
int16_t Max_P_Term; /*The max and minimum output value of the PTerm*/
|
|
int16_t Max_I_Term; /*The max and minimum output value of the ITerm*/
|
|
int16_t Max_D_Term; /*The max and minimum output value of the DTerm*/
|
|
|
|
uint16_t dterm_lpf; /*Derivative low pass filter in hz, 80 - 90 is a good init value (same as clean flight)*/
|
|
uint16_t yaw_lpf; /*Proportional (p term) low pass filter for yaw axis in hz, 80 - 90 is a good init value (same as clean flight)*/
|
|
|
|
pt1Filter_t deltaFilter[XYZ_AXIS_COUNT];/*Struct, Contains filter value for each PID of D term, do not to be init*/
|
|
pt1Filter_t yawFilter; /*Struct, Contains filter value for each PID of p term for yaw axis, do not to be init*/
|
|
|
|
float Integral[XYZ_AXIS_COUNT]; /*Buffer which contains integral values, do not to be init */
|
|
float LastError[XYZ_AXIS_COUNT]; /*Buffer which contains last error values, do not to be init */
|
|
float PID_Out[XYZ_AXIS_COUNT]; /*Output values of PID loop*/
|
|
float dT; /*Delta time of each PID cycle*/
|
|
|
|
} pidProfile_t;
|
|
|
|
|
|
extern pidProfile_t PidGyroProfile, PidAccelerometerProfile; /*Global variables to certain PID profiles*/
|
|
extern pidProfile_t PidCompassProfile, PidBarometerProfile;
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Constrain float values within a defined limit *
|
|
* INFORMATION: Used in PID loop to limit values *
|
|
**************************************************************************/
|
|
float constrainf(float amt, int low, int high);
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Update current sensor values *
|
|
* INFORMATION: *
|
|
**************************************************************************/
|
|
void getCurrentValues(float *sensorValues, uint8_t ID_profile);
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Update desired values from rc command *
|
|
* INFORMATION: *
|
|
**************************************************************************/
|
|
void getPointRate(float *desiredCommand, uint8_t ID_profile);
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Initializes PID profiles *
|
|
* INFORMATION: *
|
|
**************************************************************************/
|
|
void pidInit();
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Initializes the pid profile PID controller *
|
|
* INFORMATION: Recommended to use if unexpected values occur of profile *
|
|
**************************************************************************/
|
|
void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID);
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Dynamic PID controller, able to handle several PID controller *
|
|
* connected to different profiles.
|
|
* INFORMATION: *
|
|
**************************************************************************/
|
|
void pidUAV(pidProfile_t *pidProfile);
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Runs a certain PID Controller *
|
|
* INFORMATION: *
|
|
**************************************************************************/
|
|
void pidRun(uint8_t ID);
|
|
|
|
#endif /* FLIGHT_PID_H_ */
|