diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 2ee9e09374..581a306e36 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -511,3 +511,11 @@ uint16_t RC_Channel::get_limit_pwm(LimitValue limit) const // invalid limit value, return trim return radio_trim; } + +/* + Return true if the channel is at trim and within the DZ +*/ +bool RC_Channel::in_trim_dz() +{ + return is_bounded(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 4dfe990ec1..3562b13b06 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -135,6 +135,8 @@ public: static RC_Channel *rc_channel(uint8_t i); + bool in_trim_dz(); + private: AP_Int8 _reverse; AP_Int16 _dead_zone;