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

/home/jgoppert/Projects/ap/libraries/PID/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                 derivative = _last_derivative + 
00033                         ((delta_time / (_RC + delta_time)) * (derivative - _last_derivative));
00034 
00035                 // update state
00036                 _last_error             = error;
00037                 _last_derivative    = derivative;
00038 
00039                 // add in derivative component
00040                 output                          += _kd * derivative;
00041         }
00042 
00043         // scale the P and D components
00044         output *= scaler;
00045         
00046         // Compute integral component if time has elapsed
00047         if (_ki && (dt > 0)) {
00048                 _integrator             += (error * _ki) * scaler * delta_time; 
00049                 if (_integrator < -_imax) {
00050                         _integrator = -_imax;
00051                 } else if (_integrator > _imax) {
00052                         _integrator = _imax;
00053                 }
00054                 output                          += _integrator;
00055         }
00056         
00057         return output;
00058 }
00059 
00060 void
00061 PID::imax(float v)
00062 {
00063         _imax = fabs(v); 
00064 }
00065 
00066 void
00067 PID::load_gains()
00068 {
00069         switch(_storage)
00070         {
00071                 case STORE_OFF:
00072                         // load is a NOP if the gain array is managed externally
00073                         break;
00074                 case STORE_EEPROM_UINT16:
00075                 //      _kp     = (float)(eeprom_read_word((uint16_t *)  _address))      / 1000.0;
00076                 //      _ki     = (float)(eeprom_read_word((uint16_t *) (_address + 2))) / 1000.0;
00077                         //_kd   = (float)(eeprom_read_word((uint16_t *) (_address + 4))) / 1000.0;
00078                         //_imax         = eeprom_read_word((uint16_t *) (_address + 6)) * 100;
00079                         
00080                         _kp     = (float)_ee.read_int(_address) / 1000.0;
00081                         _ki     = (float)_ee.read_int(_address + 2) / 1000.0;
00082                         _kd     = (float)_ee.read_int(_address + 4) / 1000.0;
00083                         _imax   = (float)_ee.read_int(_address + 6) * 100;
00084 
00085                         break;
00086                         
00087                 case STORE_EEPROM_FLOAT:
00088                         //eeprom_read_block((void*)&_kp,(const void*)(_address),sizeof(_kp));
00089                         //eeprom_read_block((void*)&_ki,(const void*)(_address+4),sizeof(_ki));
00090                         //eeprom_read_block((void*)&_kd,(const void*)(_address+8),sizeof(_kd));
00091                         //eeprom_read_block((void*)&_imax,(const void*)(_address+12),sizeof(_imax));
00092                         break;
00093         }
00094 }
00095 
00096 void
00097 PID::save_gains()
00098 {
00099         switch(_storage)
00100         {
00101                 case STORE_OFF:
00102                         // save is a NOP if the gain array is managed externally
00103                         break;
00104                 case STORE_EEPROM_UINT16:
00105                         /*
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                         break;
00116                 case STORE_EEPROM_FLOAT:
00117                         //eeprom_write_block((const void *)&_kp,(void *)(_address),sizeof(_kp));
00118                         //eeprom_write_block((const void *)&_ki,(void *)(_address+4),sizeof(_ki));
00119                         //eeprom_write_block((const void *)&_kd,(void *)(_address+8),sizeof(_kd));
00120                         //eeprom_write_block((const void *)&_imax,(void *)(_address+12),sizeof(_imax));
00121                         break;
00122         }
00123 }
00124 

Generated for ArduPilot Libraries by doxygen