Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Added storage options to PID.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1011 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
james.goppert 2010-12-04 10:32:19 +00:00
parent 2f2aaa88fd
commit 665dbefbd4
2 changed files with 47 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
libraries/PID/PID.cpp
View File

@ -66,24 +66,46 @@ PID::imax(float v)
void
PID::load_gains()
{
if ((_gain_array == &_local_gains[0]) && (_address)) {
_kp = (float)(eeprom_read_word((uint16_t *) _address)) / 1000.0;
_ki = (float)(eeprom_read_word((uint16_t *) (_address + 2))) / 1000.0;
_kd = (float)(eeprom_read_word((uint16_t *) (_address + 4))) / 1000.0;
_imax = eeprom_read_word((uint16_t *) (_address + 6)) * 100;
switch(_storage)
{
case STORE_OFF:
// load is a NOP if the gain array is managed externally
break;
case STORE_EEPROM_UINT16:
_kp = (float)(eeprom_read_word((uint16_t *) _address)) / 1000.0;
_ki = (float)(eeprom_read_word((uint16_t *) (_address + 2))) / 1000.0;
_kd = (float)(eeprom_read_word((uint16_t *) (_address + 4))) / 1000.0;
_imax = eeprom_read_word((uint16_t *) (_address + 6)) * 100;
break;
case STORE_EEPROM_FLOAT:
eeprom_read_block((void*)&_kp,(const void*)(_address),sizeof(_kp));
eeprom_read_block((void*)&_ki,(const void*)(_address+4),sizeof(_ki));
eeprom_read_block((void*)&_kd,(const void*)(_address+8),sizeof(_kd));
eeprom_read_block((void*)&_imax,(const void*)(_address+12),sizeof(_imax));
break;
}
// load is a NOP if the gain array is managed externally
}
void
PID::save_gains()
{
if ((_gain_array == &_local_gains[0]) && (_address)) {
eeprom_write_word((uint16_t *) _address, (int)(_kp * 1000));
eeprom_write_word((uint16_t *) (_address + 2), (int)(_ki * 1000));
eeprom_write_word((uint16_t *) (_address + 4), (int)(_kd * 1000));
eeprom_write_word((uint16_t *) (_address + 6), (int)_imax/100);
switch(_storage)
{
case STORE_OFF:
// save is a NOP if the gain array is managed externally
break;
case STORE_EEPROM_UINT16:
eeprom_write_word((uint16_t *) _address, (int)(_kp * 1000));
eeprom_write_word((uint16_t *) (_address + 2), (int)(_ki * 1000));
eeprom_write_word((uint16_t *) (_address + 4), (int)(_kd * 1000));
eeprom_write_word((uint16_t *) (_address + 6), (int)_imax/100);
break;
case STORE_EEPROM_FLOAT:
eeprom_write_block((const void *)&_kp,(void *)(_address),sizeof(_kp));
eeprom_write_block((const void *)&_ki,(void *)(_address+4),sizeof(_ki));
eeprom_write_block((const void *)&_kd,(void *)(_address+8),sizeof(_kd));
eeprom_write_block((const void *)&_imax,(void *)(_address+12),sizeof(_imax));
break;
}
// save is a NOP if the gain array is managed externally
}

14
libraries/PID/PID.h
View File

@ -12,6 +12,15 @@
/// @brief Object managing one PID control
class PID {
public:
///
// EEProm Storage Type
enum storage_t
{
STORE_OFF,
STORE_EEPROM_FLOAT,
STORE_EEPROM_UINT16
} _storage;
/// Constructor
///
/// A PID constructed in this fashion does not support save/restore.
@ -19,6 +28,7 @@ public:
/// accessor functions.
///
PID() :
_storage(STORE_OFF),
_address(0),
_gain_array(&_local_gains[0])
{}
@ -31,7 +41,8 @@ public:
/// @param address EEPROM base address at which PID parameters
/// are stored.
///
PID(uint16_t address) :
PID(uint16_t address, storage_t storage = STORE_EEPROM_UINT16) :
_storage(storage),
_address(address),
_gain_array(&_local_gains[0])
{}
@ -45,6 +56,7 @@ public:
/// respectively.
///
PID(float *gain_array) :
_storage(STORE_OFF),
_gain_array(gain_array)
{}