Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

update use EEPROM class, new scaled input for RC_Channel

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1264 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2010-12-26 01:17:04 +00:00
parent dd8c20b03d
commit 5d6c90b723
5 changed files with 92 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/PID/PID.cpp
View File

@ -72,16 +72,23 @@ PID::load_gains()
// 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;
// _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;
_kp = (float)_ee.read_int(_address) / 1000.0;
_ki = (float)_ee.read_int(_address + 2) / 1000.0;
_kd = (float)_ee.read_int(_address + 4) / 1000.0;
_imax = (float)_ee.read_int(_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));
//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;
}
}
@ -95,16 +102,22 @@ PID::save_gains()
// 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);
*/
_ee.write_int(_address, (int)(_kp * 1000));
_ee.write_int(_address + 2, (int)(_ki * 1000));
_ee.write_int(_address + 4, (int)(_kd * 1000));
_ee.write_int(_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));
//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;
}
}

13
libraries/PID/PID.h
View File

@ -7,6 +7,7 @@
#define PID_h
#include <stdint.h>
#include <AP_EEPROMB.h>
/// @class PID
/// @brief Object managing one PID control
@ -57,6 +58,7 @@ public:
/// array is used as kP, kI, kD and imax
/// respectively.
///
PID(float *gain_array) :
_storage(STORE_OFF),
_gain_array(gain_array)
@ -75,7 +77,7 @@ public:
/// @param scaler An arbitrary scale factor
///
/// @returns The updated control output.
///
///
long get_pid(int32_t error, uint16_t dt, float scaler = 1.0);
/// Reset the PID integrator
@ -105,15 +107,18 @@ public:
void kI(const float v) { _gain_array[1] = v; }
void kD(const float v) { _gain_array[2] = v; }
void imax(const float v);
void address(const uint16_t v) { _address = v; }
// one-shot operator for setting all of the gain properties at once
void operator ()(const float p, const float i, const float d, const float max)
{ kP(p); kI(i); kD(d); imax(max); }
//void operator ()(const float p, const float i, const float d, const float max)
//{ kP(p); kI(i); kD(d); imax(max); }
//@}
float get_integrator() { return _integrator; }
private:
AP_EEPROMB _ee;
uint16_t _address; ///< EEPROM address for save/restore of P/I/D
float *_gain_array; ///< pointer to the gains for this pid

3
libraries/PID/examples/pid/pid.pde
View File

@ -6,10 +6,11 @@
#include <PID.h> // ArduPilot Mega RC Library
#include <APM_RC.h> // ArduPilot RC Library
PID pid;
long radio_in;
long radio_trim;
PID pid(0x16);
void setup()
{
Serial.begin(38400);

63
libraries/RC_Channel/RC_Channel.cpp
View File

@ -73,9 +73,16 @@ RC_Channel::set_pwm(int pwm)
//Serial.print("range ");
control_in = pwm_to_range();
control_in = (control_in < dead_zone) ? 0 : control_in;
if(scale_output){
control_in *= scale_output;
}
}else{
control_in = pwm_to_angle();
control_in = (abs(control_in) < dead_zone) ? 0 : control_in;
if(scale_output){
control_in *= scale_output;
}
}
}
@ -102,7 +109,11 @@ RC_Channel::calc_pwm(void)
}else{
pwm_out = angle_to_pwm();
}
//if(scale_output){
// pwm_out *= scale_output;
//}
radio_out = pwm_out + radio_min;
radio_out = constrain(radio_out,radio_min, radio_max);
}
// ------------------------------------------
@ -110,28 +121,42 @@ RC_Channel::calc_pwm(void)
void
RC_Channel::load_eeprom(void)
{
radio_min = eeprom_read_word((uint16_t *) _address);
radio_max = eeprom_read_word((uint16_t *) (_address + 2));
radio_trim = eeprom_read_word((uint16_t *) (_address + 4));
//radio_min = eeprom_read_word((uint16_t *) _address);
//radio_max = eeprom_read_word((uint16_t *) (_address + 2));
//radio_trim = eeprom_read_word((uint16_t *) (_address + 4));
radio_min = _ee.read_int(_address);
radio_max = _ee.read_int(_address + 2);
radio_trim = _ee.read_int(_address + 4);
}
void
RC_Channel::save_eeprom(void)
{
eeprom_write_word((uint16_t *) _address, radio_min);
eeprom_write_word((uint16_t *) (_address + 2), radio_max);
eeprom_write_word((uint16_t *) (_address + 4), radio_trim);
//eeprom_write_word((uint16_t *) _address, radio_min);
//eeprom_write_word((uint16_t *) (_address + 2), radio_max);
//eeprom_write_word((uint16_t *) (_address + 4), radio_trim);
_ee.write_int(_address, radio_min);
_ee.write_int((_address + 2), radio_max);
_ee.write_int((_address + 4), radio_trim);
}
// ------------------------------------------
void
RC_Channel::save_trim(void)
{
eeprom_write_word((uint16_t *) (_address + 4), radio_trim);
//eeprom_write_word((uint16_t *) (_address + 4), radio_trim);
_ee.write_int((_address + 4), radio_trim);
}
// ------------------------------------------
void
RC_Channel::zero_min_max()
{
radio_min = radio_min = radio_in;
}
void
RC_Channel::update_min_max()
{
@ -141,13 +166,16 @@ RC_Channel::update_min_max()
// ------------------------------------------
int16_t
int16_t
RC_Channel::pwm_to_angle()
{
if(radio_in < radio_trim)
return _reverse * _high * ((float)(radio_in - radio_trim) / (float)(radio_trim - radio_min));
return _reverse * ((long)_high * (long)(radio_in - radio_trim)) / (long)(radio_trim - radio_min);
else
return _reverse * _high * ((float)(radio_in - radio_trim) / (float)(radio_max - radio_trim));
return _reverse * ((long)_high * (long)(radio_in - radio_trim)) / (long)(radio_max - radio_trim);
//return _reverse * _high * ((float)(radio_in - radio_trim) / (float)(radio_max - radio_trim));
//return _reverse * _high * ((float)(radio_in - radio_trim) / (float)(radio_trim - radio_min));
}
float
@ -172,9 +200,12 @@ int16_t
RC_Channel::angle_to_pwm()
{
if(servo_out < 0)
return (((float)servo_out / (float)_high) * (float)(radio_max - radio_trim));
return ((long)servo_out * (long)(radio_max - radio_trim)) / (long)_high;
else
return (((float)servo_out / (float)_high) * (float)(radio_trim - radio_min));
return ((long)servo_out * (long)(radio_trim - radio_min)) / (long)_high;
//return (((float)servo_out / (float)_high) * (float)(radio_max - radio_trim));
//return (((float)servo_out / (float)_high) * (float)(radio_trim - radio_min));
}
// ------------------------------------------
@ -182,12 +213,16 @@ RC_Channel::angle_to_pwm()
int16_t
RC_Channel::pwm_to_range()
{
return _reverse * (_low + ((_high - _low) * ((float)(radio_in - radio_min) / (float)(radio_max - radio_min))));
//return (_low + ((_high - _low) * ((float)(radio_in - radio_min) / (float)(radio_max - radio_min))));
return (_low + ((long)(_high - _low) * (long)(radio_in - radio_min)) / (long)(radio_max - radio_min));
}
int16_t
RC_Channel::range_to_pwm()
{
return (((float)servo_out / (float)(_high - _low)) * (float)(radio_max - radio_min));
//return (((float)(servo_out - _low) / (float)(_high - _low)) * (float)(radio_max - radio_min));
return ((long)(servo_out - _low) * (long)(radio_max - radio_min)) / (long)(_high - _low);
}

8
libraries/RC_Channel/RC_Channel.h
View File

@ -7,6 +7,7 @@
#define RC_Channel_h
#include <stdint.h>
#include <AP_EEPROMB.h>
/// @class RC_Channel
/// @brief Object managing one RC channel
@ -29,11 +30,13 @@ class RC_Channel{
RC_Channel(uint16_t address) :
_address(address),
_high(1),
_filter(true)
_filter(true),
_reverse(1)
{}
// setup min and max radio values in CLI
void update_min_max();
void zero_min_max();
// startup
void load_eeprom(void);
@ -87,6 +90,8 @@ class RC_Channel{
int16_t angle_to_pwm();
int16_t pwm_to_range();
int16_t range_to_pwm();
float scale_output;
private:
bool _filter;
@ -96,6 +101,7 @@ class RC_Channel{
bool _type;
int16_t _high;
int16_t _low;
AP_EEPROMB _ee;
};
#endif