diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 73b75b8a43..80a1bdb0f9 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -33,6 +33,13 @@ RC_Channel::set_angle(int angle) _high = angle; } +void +RC_Channel::set_reverse(bool reverse) +{ + if (reverse) _reverse = -1; + else _reverse = 1; +} + void RC_Channel::set_filter(bool filter) { @@ -138,9 +145,27 @@ int16_t RC_Channel::pwm_to_angle() { if(radio_in < radio_trim) - return _high * ((float)(radio_in - radio_trim) / (float)(radio_trim - radio_min)); + return _reverse * _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 _reverse * _high * ((float)(radio_in - radio_trim) / (float)(radio_max - radio_trim)); +} + +float +RC_Channel::norm_input() +{ + if(radio_in < radio_trim) + return _reverse * (float)(radio_in - radio_trim) / (float)(radio_trim - radio_min); + else + return _reverse * (float)(radio_in - radio_trim) / (float)(radio_max - radio_trim); +} + +float +RC_Channel::norm_output() +{ + if(radio_out < radio_trim) + return (float)(radio_out - radio_trim) / (float)(radio_trim - radio_min); + else + return (float)(radio_out - radio_trim) / (float)(radio_max - radio_trim); } int16_t @@ -157,7 +182,7 @@ RC_Channel::angle_to_pwm() int16_t RC_Channel::pwm_to_range() { - return _low + ((_high - _low) * ((float)(radio_in - radio_min) / (float)(radio_max - radio_min))); + return _reverse * (_low + ((_high - _low) * ((float)(radio_in - radio_min) / (float)(radio_max - radio_min)))); } int16_t diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 06fa01512a..717b2b2ddc 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -44,6 +44,7 @@ class RC_Channel{ // setup the control preferences void set_range(int low, int high); void set_angle(int angle); + void set_reverse(bool reverse); // read input from APM_RC - create a control_in value void set_pwm(int pwm); @@ -81,12 +82,15 @@ class RC_Channel{ //int16_t get_radio_out(void); int16_t pwm_to_angle(); + float norm_input(); + float norm_output(); int16_t angle_to_pwm(); int16_t pwm_to_range(); int16_t range_to_pwm(); private: bool _filter; + int8_t _reverse; int16_t _address; ///< EEPROM address for save/restore of P/I/D bool _type;