diff --git a/libraries/PID/PID.cpp b/libraries/PID/PID.cpp index 0646c1bcc6..d5c2c56646 100644 --- a/libraries/PID/PID.cpp +++ b/libraries/PID/PID.cpp @@ -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; } } diff --git a/libraries/PID/PID.h b/libraries/PID/PID.h index 14b1c772d1..7426154387 100644 --- a/libraries/PID/PID.h +++ b/libraries/PID/PID.h @@ -7,6 +7,7 @@ #define PID_h #include +#include /// @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 diff --git a/libraries/PID/examples/pid/pid.pde b/libraries/PID/examples/pid/pid.pde index a86d8fcad1..1d041fe3e3 100644 --- a/libraries/PID/examples/pid/pid.pde +++ b/libraries/PID/examples/pid/pid.pde @@ -6,10 +6,11 @@ #include // ArduPilot Mega RC Library #include // ArduPilot RC Library -PID pid; long radio_in; long radio_trim; +PID pid(0x16); + void setup() { Serial.begin(38400); diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 80a1bdb0f9..626f65de57 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -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); } + + diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 717b2b2ddc..3325c1a181 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -7,6 +7,7 @@ #define RC_Channel_h #include +#include /// @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