diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index cc14becea7..470f4a50e6 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -146,6 +146,18 @@ public: return (chan < NUM_RC_CHANNELS)?&channels[chan]:nullptr; } + static uint16_t get_radio_in(const uint8_t chan); // returns the last read radio_in value from a chan, 0 if the channel is out of range + static uint8_t get_radio_in(uint16_t *chans, const uint8_t num_channels); // reads a block of chanel radio_in values starting from channel 0 + // returns the number of valid channels + + static uint8_t get_valid_channel_count(void); // returns the number of valid channels in the last read + static int16_t get_receiver_rssi(void); // returns [0, 255] for receiver RSSI (0 is no link) if present, otherwise -1 + static bool has_new_input(void); // returns true if there has been new input since last checked + static void clear_overrides(void); // clears any active overrides + static bool receiver_bind(const int dsmMode); // puts the reciever in bind mode if present, returns true if success + static bool set_override(const uint8_t chan, const int16_t value); // set a channels override value + static bool set_overrides(int16_t *overrides, const uint8_t len); // set multiple overrides at once + static void set_pwm_all(void); private: diff --git a/libraries/RC_Channel/RC_Channels.cpp b/libraries/RC_Channel/RC_Channels.cpp index 37e2a35d6e..699000529e 100644 --- a/libraries/RC_Channel/RC_Channels.cpp +++ b/libraries/RC_Channel/RC_Channels.cpp @@ -115,6 +115,29 @@ RC_Channels::RC_Channels(void) } } +uint16_t RC_Channels::get_radio_in(const uint8_t chan) +{ + if (chan >= NUM_RC_CHANNELS) { + return 0; + } + return channels[chan].get_radio_in(); +} + +uint8_t RC_Channels::get_radio_in(uint16_t *chans, const uint8_t num_channels) +{ + uint8_t read_channels = MIN(num_channels, NUM_RC_CHANNELS); + for (uint8_t i = 0; i < read_channels; i++) { + chans[i] = channels[i].get_radio_in(); + } + + // clear any excess channels we couldn't read + if (read_channels < num_channels) { + memset(&chans[NUM_RC_CHANNELS], 0, sizeof(uint16_t) * (num_channels - read_channels)); + } + + return read_channels; +} + /* call read() and set_pwm() on all channels */ @@ -125,3 +148,41 @@ RC_Channels::set_pwm_all(void) channels[i].set_pwm(channels[i].read()); } } + +uint8_t RC_Channels::get_valid_channel_count(void) +{ + return MIN(NUM_RC_CHANNELS, hal.rcin->num_channels()); +} + +int16_t RC_Channels::get_receiver_rssi(void) +{ + return hal.rcin->get_rssi(); +} + +bool RC_Channels::has_new_input(void) +{ + return hal.rcin->new_input(); +} + +void RC_Channels::clear_overrides(void) +{ + hal.rcin->clear_overrides(); +} + +bool RC_Channels::set_override(const uint8_t chan, const int16_t value) +{ + if (chan < NUM_RC_CHANNELS) { + return hal.rcin->set_override(chan, value); + } + return false; +} + +bool RC_Channels::set_overrides(int16_t *overrides, const uint8_t len) +{ + return hal.rcin->set_overrides(overrides, len); +} + +bool RC_Channels::receiver_bind(const int dsmMode) +{ + return hal.rcin->rc_bind(dsmMode); +}