Successful acro and level mode now! Relying on MPU9250 as base sensor. I'm working continuously on tuning and features :) NEWEST VERSION ON: https://github.com/MaEtUgR/FlyBed (CODE 100% compatible/copyable)
PID/PID.cpp
- Committer:
- maetugr
- Date:
- 2015-11-19
- Revision:
- 8:609a2ad4c30e
- Parent:
- 5:8ea99e98de73
File content as of revision 8:609a2ad4c30e:
#include "PID.h" PID::PID(float P, float I, float D, float Integral_Max) { Value = 0; Integral = 0; LastTime = 0; Integrate = true; RollBufferIndex = 0; PID::P = P; PID::I = I; PID::D = D; PID::Integral_Max = Integral_Max; dtTimer.start(); } void PID::compute(float SetPoint, float ProcessValue) { // meassure dt float dt = dtTimer.read() - LastTime; // time in us since last loop LastTime = dtTimer.read(); // set new time for next measurement // Proportional float Error = ProcessValue - SetPoint; // Integral if (dt > 2 || !Integrate) // Todo: 2 secs is the maximal time between two computations Integral = 0; else if (abs(Integral + Error) <= Integral_Max) Integral += Error * dt; // Derivative RollBuffer[RollBufferIndex] = (Error - PreviousError) / dt; RollBufferIndex++; if (RollBufferIndex == BUFFERSIZE) RollBufferIndex = 0; float Derivative = 0; for(int i=0; i<BUFFERSIZE ;i++) Derivative += RollBuffer[i]; Derivative /= BUFFERSIZE; PreviousError = Error; // Final Formula Value = P * Error + I * Integral + D * Derivative; } void PID::setPID(float P, float I, float D) { PID::P = P; PID::I = I; PID::D = D; } void PID::setIntegrate(bool Integrate) { PID::Integrate = Integrate; }