/************************************************************************** * 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 #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; }