diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index c0be4ca5d8..bd06912e3b 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -18,13 +18,6 @@ RC_Channel::RC_Channel() : _high(1) { } -void -RC_Channel::set_radio_range(int r_min, int r_max) -{ - _radio_min = r_min; - _radio_max = r_max; -} - // setup the control preferences void RC_Channel::set_range(int low, int high) @@ -43,9 +36,10 @@ RC_Channel::set_angle(int angle) // call after first read void -RC_Channel::set_trim(int pwm) +RC_Channel::trim() { - _radio_trim = pwm; + radio_trim = radio_in; + } // read input from APM_RC - create a control_in value @@ -54,16 +48,18 @@ RC_Channel::set_pwm(int pwm) { //Serial.print(pwm,DEC); - if(_radio_in == 0) - _radio_in = pwm; + if(radio_in == 0) + radio_in = pwm; else - _radio_in = ((pwm + _radio_in) >> 1); // Small filtering + radio_in = ((pwm + radio_in) >> 1); // Small filtering if(_type == RANGE){ //Serial.print("range "); control_in = pwm_to_range(); + control_in = (control_in < dead_zone) ? 0 : control_in; }else{ control_in = pwm_to_angle(); + control_in = (abs(control_in) < dead_zone) ? 0 : control_in; } } @@ -71,7 +67,7 @@ RC_Channel::set_pwm(int pwm) boolean RC_Channel::get_failsafe(void) { - return (_radio_in < (_radio_min - 50)); + return (radio_in < (radio_min - 50)); } // returns just the PWM without the offset from radio_min @@ -84,47 +80,50 @@ RC_Channel::calc_pwm(void) }else{ pwm_out = angle_to_pwm(); } - radio_out = pwm_out + _radio_min; + radio_out = pwm_out + radio_min; } void RC_Channel::load_eeprom(int address) { - //Serial.println("load gains "); - //Serial.println(address, DEC); - //_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; + 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)); } void RC_Channel::save_eeprom(int 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); + 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); } +// ------------------------------------------ +void +RC_Channel::update_min_max() +{ + radio_min = min(radio_min, radio_in); + radio_max = min(radio_max, radio_in); +} // ------------------------------------------ int16_t RC_Channel::pwm_to_angle() { - if(_radio_in < _radio_trim) - return _high * ((float)(_radio_in - _radio_trim) / (float)(_radio_trim - _radio_min)); + if(radio_in < radio_trim) + return _high * ((float)(radio_in - radio_trim) / (float)(radio_trim - radio_min)); else - return _high * ((float)(_radio_in - _radio_trim) / (float)(_radio_max - _radio_trim)); + return _high * ((float)(radio_in - radio_trim) / (float)(radio_max - radio_trim)); } int16_t RC_Channel::angle_to_pwm() { if(servo_out < 0) - return (((float)servo_out / (float)_high) * (float)(_radio_max - _radio_trim)); + return (((float)servo_out / (float)_high) * (float)(radio_max - radio_trim)); else - return (((float)servo_out / (float)_high) * (float)(_radio_trim - _radio_min)); + return (((float)servo_out / (float)_high) * (float)(radio_trim - radio_min)); } // ------------------------------------------ @@ -132,12 +131,12 @@ RC_Channel::angle_to_pwm() int16_t RC_Channel::pwm_to_range() { - return _low + _high * ((float)(_radio_in - _radio_min) / (float)(_radio_max - _radio_min)); + return _low + _high * ((float)(radio_in - radio_min) / (float)(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 / (float)(_high - _low)) * (float)(radio_max - radio_min)); } diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index e930a99a0a..74eb45d0ee 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -8,6 +8,7 @@ #define RC_Channel_h #include +#include #include "WProgram.h" class RC_Channel @@ -15,26 +16,32 @@ class RC_Channel public: RC_Channel(); + // setup in CLI + void update_min_max(); + // startup void load_eeprom(int address); void save_eeprom(int address); - void set_radio_range(int r_min, int r_max); // setup the control preferences void set_range(int low, int high); void set_angle(int angle); - // call after first read - void set_trim(int pwm); - // read input from APM_RC - create a control_in value void set_pwm(int pwm); + // pwm is stored here + int16_t radio_in; + + // call after first set_pwm + void trim(); + // did our read come in 50µs below the min? boolean get_failsafe(void); // value generated from PWM int16_t control_in; + int8_t dead_zone; // used to keep noise down and create a dead zone. // current values to the servos - degrees * 100 (approx assuming servo is -45 to 45 degrees except [3] is 0 to 100 int16_t servo_out; @@ -45,6 +52,10 @@ class RC_Channel // PWM is without the offset from radio_min int16_t pwm_out; int16_t radio_out; + + int16_t radio_min; + int16_t radio_trim; + int16_t radio_max; // includes offset from PWM //int16_t get_radio_out(void); @@ -60,10 +71,6 @@ class RC_Channel boolean _type; boolean _filter; - int16_t _radio_in; - int16_t _radio_min; - int16_t _radio_trim; - int16_t _radio_max; int16_t _high; int16_t _low; diff --git a/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde b/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde index 4c9819dd0c..813de81fc6 100644 --- a/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde +++ b/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde @@ -28,10 +28,17 @@ void setup() // setup radio // read eepom or set manually - rc_1.set_radio_range(1100,1900); - rc_2.set_radio_range(1100,1900); - rc_3.set_radio_range(1100,1900); - rc_4.set_radio_range(1100,1900); + rc_1.radio_min = 1100; + rc_1.radio_max = 1900; + + rc_2.radio_min = 1100; + rc_2.radio_max = 1900; + + rc_3.radio_min = 1100; + rc_3.radio_max = 1900; + + rc_4.radio_min = 1100; + rc_4.radio_max = 1900; // set type of output, symmetrical angles or a number range; rc_1.set_angle(4500); @@ -40,11 +47,15 @@ void setup() rc_4.set_angle(3000); // set midpoint value - rc_1.set_trim(APM_RC.InputCh(CH_1)); - rc_2.set_trim(APM_RC.InputCh(CH_2)); - rc_3.set_trim(APM_RC.InputCh(CH_3)); - rc_4.set_trim(APM_RC.InputCh(CH_4)); - + rc_1.set_pwm(APM_RC.InputCh(CH_1)); + rc_2.set_pwm(APM_RC.InputCh(CH_2)); + rc_3.set_pwm(APM_RC.InputCh(CH_3)); + rc_4.set_pwm(APM_RC.InputCh(CH_4)); + + rc_1.trim(); + rc_2.trim(); + rc_3.trim(); + rc_4.trim(); } void loop()