diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 34dfb90e92..6e5c766e05 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -612,6 +612,12 @@ public: // get failsafe timeout in milliseconds uint32_t get_fs_timeout_ms() const { return MAX(_fs_timeout * 1000, 100); } + // methods which return RC input channels used for various axes. + RC_Channel &get_roll_channel(); + RC_Channel &get_pitch_channel(); + RC_Channel &get_yaw_channel(); + RC_Channel &get_throttle_channel(); + protected: void new_override_received() { @@ -653,6 +659,9 @@ private: void set_aux_cached(RC_Channel::AUX_FUNC aux_fn, RC_Channel::AuxSwitchPos pos); #endif + + RC_Channel &get_rcmap_channel_nonnull(uint8_t rcmap_number); + RC_Channel dummy_rcchannel; }; RC_Channels &rc(); diff --git a/libraries/RC_Channel/RC_Channels.cpp b/libraries/RC_Channel/RC_Channels.cpp index 9ec73c5dab..cd32cf938b 100644 --- a/libraries/RC_Channel/RC_Channels.cpp +++ b/libraries/RC_Channel/RC_Channels.cpp @@ -30,6 +30,7 @@ extern const AP_HAL::HAL& hal; #include #include +#include #include "RC_Channel.h" @@ -307,6 +308,39 @@ void RC_Channels::set_aux_cached(RC_Channel::AUX_FUNC aux_fn, RC_Channel::AuxSwi } #endif // AP_SCRIPTING_ENABLED +#if AP_RCMAPPER_ENABLED +// these methods return an RC_Channel pointers based on values from +// AP_::rcmap(). The return value is guaranteed to be not-null to +// allow use of the pointer without checking it for null-ness. If an +// invalid option has been chosen somehow then the returned channel +// will be a dummy channel. +RC_Channel &RC_Channels::get_rcmap_channel_nonnull(uint8_t rcmap_number) +{ + RC_Channel *ret = RC_Channels::rc_channel(rcmap_number-1); + if (ret != nullptr) { + return *ret; + } + return dummy_rcchannel; +} +RC_Channel &RC_Channels::get_roll_channel() +{ + return get_rcmap_channel_nonnull(AP::rcmap()->roll()); +}; +RC_Channel &RC_Channels::get_pitch_channel() +{ + return get_rcmap_channel_nonnull(AP::rcmap()->pitch()); +}; +RC_Channel &RC_Channels::get_throttle_channel() +{ + return get_rcmap_channel_nonnull(AP::rcmap()->throttle()); +}; +RC_Channel &RC_Channels::get_yaw_channel() +{ + return get_rcmap_channel_nonnull(AP::rcmap()->yaw()); +}; +#endif // AP_RCMAPPER_ENABLED + + // singleton instance RC_Channels *RC_Channels::_singleton;