• Main Page
  • Related Pages
  • Namespaces
  • Classes
  • Files
  • File List
  • File Members

PID.cpp

Go to the documentation of this file.
00001 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
00002 
00005 
00006 #include <math.h>
00007 #include <avr/eeprom.h>
00008 
00009 #include "PID.h"
00010 
00011 // make the gain array members a little easier to identify
00012 #define _kp             _gain_array[0]
00013 #define _ki             _gain_array[1]
00014 #define _kd             _gain_array[2]
00015 #define _imax   _gain_array[3]
00016 
00017 long
00018 PID::get_pid(int32_t error, uint16_t dt, float scaler)
00019 {
00020         float output            = 0;
00021         float delta_time        = (float)dt / 1000.0;
00022 
00023         // Compute proportional component
00024         output += error * _kp;
00025 
00026         // Compute derivative component if time has elapsed
00027         if (_kd && (dt > 0)) {
00028                 float derivative = (error - _last_error) / delta_time;
00029 
00030                 // discrete low pass filter, cuts out the 
00031                 // high frequency noise that can drive the controller crazy
00032                 float RC = 1/(2*M_PI*_fCut);
00033                 derivative = _last_derivative + 
00034                         (delta_time / (RC + delta_time)) * (derivative - _last_derivative);
00035 
00036                 // update state
00037                 _last_error             = error;
00038                 _last_derivative    = derivative;
00039 
00040                 // add in derivative component
00041                 output                          += _kd * derivative;
00042         }
00043 
00044         // scale the P and D components
00045         output *= scaler;
00046         
00047         // Compute integral component if time has elapsed
00048         if (_ki && (dt > 0)) {
00049                 _integrator             += (error * _ki) * scaler * delta_time; 
00050                 if (_integrator < -_imax) {
00051                         _integrator = -_imax;
00052                 } else if (_integrator > _imax) {
00053                         _integrator = _imax;
00054                 }
00055                 output                          += _integrator;
00056         }
00057         
00058         return output;
00059 }
00060 
00061 void
00062 PID::imax(float v)
00063 {
00064         _imax = fabs(v); 
00065 }
00066 
00067 void
00068 PID::load_gains()
00069 {
00070         switch(_storage)
00071         {
00072                 case STORE_OFF:
00073                         // load is a NOP if the gain array is managed externally
00074                         break;
00075                 case STORE_EEPROM_UINT16:
00076                         _kp     = (float)(eeprom_read_word((uint16_t *)  _address))      / 1000.0;
00077                         _ki     = (float)(eeprom_read_word((uint16_t *) (_address + 2))) / 1000.0;
00078                         _kd     = (float)(eeprom_read_word((uint16_t *) (_address + 4))) / 1000.0;
00079                         _imax   = eeprom_read_word((uint16_t *) (_address + 6)) * 100;
00080                         
00081                         //_kp   = (float)_ee.read_int(_address) / 1000.0;
00082                         //_ki   = (float)_ee.read_int(_address + 2) / 1000.0;
00083                         //_kd   = (float)_ee.read_int(_address + 4) / 1000.0;
00084                         //_imax         = (float)_ee.read_int(_address + 6) * 100;
00085 
00086                         break;
00087                         
00088                 case STORE_EEPROM_FLOAT:
00089                         //eeprom_read_block((void*)&_kp,(const void*)(_address),sizeof(_kp));
00090                         //eeprom_read_block((void*)&_ki,(const void*)(_address+4),sizeof(_ki));
00091                         //eeprom_read_block((void*)&_kd,(const void*)(_address+8),sizeof(_kd));
00092                         //eeprom_read_block((void*)&_imax,(const void*)(_address+12),sizeof(_imax));
00093                         break;
00094         }
00095 }
00096 
00097 void
00098 PID::save_gains()
00099 {
00100         switch(_storage)
00101         {
00102                 case STORE_OFF:
00103                         // save is a NOP if the gain array is managed externally
00104                         break;
00105                 case STORE_EEPROM_UINT16:
00106                         eeprom_write_word((uint16_t *)   _address,              (int)(_kp * 1000));
00107                         eeprom_write_word((uint16_t *)  (_address + 2), (int)(_ki * 1000));
00108                         eeprom_write_word((uint16_t *)  (_address + 4), (int)(_kd * 1000));
00109                         eeprom_write_word((uint16_t *)  (_address + 6), (int)_imax/100);
00110 
00111                         /*_ee.write_int(_address,       (int)(_kp * 1000));
00112                         _ee.write_int(_address + 2, (int)(_ki * 1000));
00113                         _ee.write_int(_address + 4, (int)(_kd * 1000));
00114                         _ee.write_int(_address + 6, (int)(_imax /100));
00115                         */
00116                         break;
00117                 case STORE_EEPROM_FLOAT:
00118                         //eeprom_write_block((const void *)&_kp,(void *)(_address),sizeof(_kp));
00119                         //eeprom_write_block((const void *)&_ki,(void *)(_address+4),sizeof(_ki));
00120                         //eeprom_write_block((const void *)&_kd,(void *)(_address+8),sizeof(_kd));
00121                         //eeprom_write_block((const void *)&_imax,(void *)(_address+12),sizeof(_imax));
00122                         break;
00123         }
00124 }
00125 

Generated for ArduPilot Libraries by doxygen