diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 4ff53f19cd..d868bfd303 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -182,7 +182,7 @@ int16_t RC_Channel::get_control_mid() const the current radio_in value using the specified dead_zone */ int16_t -RC_Channel::pwm_to_angle_dz_trim(uint16_t _dead_zone, uint16_t _trim) +RC_Channel::pwm_to_angle_dz_trim(uint16_t _dead_zone, uint16_t _trim) const { int16_t radio_trim_high = _trim + _dead_zone; int16_t radio_trim_low = _trim - _dead_zone; @@ -202,7 +202,7 @@ RC_Channel::pwm_to_angle_dz_trim(uint16_t _dead_zone, uint16_t _trim) the current radio_in value using the specified dead_zone */ int16_t -RC_Channel::pwm_to_angle_dz(uint16_t _dead_zone) +RC_Channel::pwm_to_angle_dz(uint16_t _dead_zone) const { return pwm_to_angle_dz_trim(_dead_zone, radio_trim); } @@ -212,7 +212,7 @@ RC_Channel::pwm_to_angle_dz(uint16_t _dead_zone) the current radio_in value */ int16_t -RC_Channel::pwm_to_angle() +RC_Channel::pwm_to_angle() const { return pwm_to_angle_dz(dead_zone); } @@ -223,7 +223,7 @@ RC_Channel::pwm_to_angle() range, using the specified deadzone */ int16_t -RC_Channel::pwm_to_range_dz(uint16_t _dead_zone) +RC_Channel::pwm_to_range_dz(uint16_t _dead_zone) const { int16_t r_in = constrain_int16(radio_in, radio_min.get(), radio_max.get()); @@ -244,13 +244,13 @@ RC_Channel::pwm_to_range_dz(uint16_t _dead_zone) range */ int16_t -RC_Channel::pwm_to_range() +RC_Channel::pwm_to_range() const { return pwm_to_range_dz(dead_zone); } -int16_t RC_Channel::get_control_in_zero_dz(void) +int16_t RC_Channel::get_control_in_zero_dz(void) const { if (type_in == RC_CHANNEL_TYPE_RANGE) { return pwm_to_range_dz(0); @@ -261,7 +261,7 @@ int16_t RC_Channel::get_control_in_zero_dz(void) // ------------------------------------------ float -RC_Channel::norm_input() +RC_Channel::norm_input() const { float ret; int16_t reverse_mul = (reversed?-1:1); @@ -280,7 +280,7 @@ RC_Channel::norm_input() } float -RC_Channel::norm_input_dz() +RC_Channel::norm_input_dz() const { int16_t dz_min = radio_trim - dead_zone; int16_t dz_max = radio_trim + dead_zone; @@ -300,7 +300,7 @@ RC_Channel::norm_input_dz() get percentage input from 0 to 100. This ignores the trim value. */ uint8_t -RC_Channel::percent_input() +RC_Channel::percent_input() const { if (radio_in <= radio_min) { return reversed?100:0; @@ -318,7 +318,7 @@ RC_Channel::percent_input() /* Return true if the channel is at trim and within the DZ */ -bool RC_Channel::in_trim_dz() +bool RC_Channel::in_trim_dz() const { return is_bounded_int32(radio_in, radio_trim - dead_zone, radio_trim + dead_zone); } diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 1ebc3b2b31..f1ccad0575 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -50,28 +50,28 @@ public: // calculate an angle given dead_zone and trim. This is used by the quadplane code // for hover throttle - int16_t pwm_to_angle_dz_trim(uint16_t dead_zone, uint16_t trim); + int16_t pwm_to_angle_dz_trim(uint16_t dead_zone, uint16_t trim) const; /* return a normalised input for a channel, in range -1 to 1, centered around the channel trim. Ignore deadzone. */ - float norm_input(); + float norm_input() const; /* return a normalised input for a channel, in range -1 to 1, centered around the channel trim. Take into account the deadzone */ - float norm_input_dz(); + float norm_input_dz() const; - uint8_t percent_input(); - int16_t pwm_to_range(); - int16_t pwm_to_range_dz(uint16_t dead_zone); + uint8_t percent_input() const; + int16_t pwm_to_range() const; + int16_t pwm_to_range_dz(uint16_t dead_zone) const; static const struct AP_Param::GroupInfo var_info[]; // return true if input is within deadzone of trim - bool in_trim_dz(); + bool in_trim_dz() const; int16_t get_radio_in() const { return radio_in;} void set_radio_in(int16_t val) {radio_in = val;} @@ -84,7 +84,7 @@ public: bool has_override() const; // get control input with zero deadzone - int16_t get_control_in_zero_dz(void); + int16_t get_control_in_zero_dz(void) const; int16_t get_radio_min() const {return radio_min.get();} void set_radio_min(int16_t val) { radio_min = val;} @@ -227,8 +227,8 @@ private: uint16_t override_value; uint32_t last_override_time; - int16_t pwm_to_angle(); - int16_t pwm_to_angle_dz(uint16_t dead_zone); + int16_t pwm_to_angle() const; + int16_t pwm_to_angle_dz(uint16_t dead_zone) const; // pwm value above which the option will be invoked: static const uint16_t AUX_PWM_TRIGGER_HIGH = 1800;