From 1f1ba54990102aee0fa6f01ad1b47b98fea4cf5f Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Thu, 26 Apr 2018 16:17:06 -0700 Subject: [PATCH] AP_HAL_VRBrain: Remove RC overrides --- libraries/AP_HAL_VRBRAIN/RCInput.cpp | 39 +--------------------------- libraries/AP_HAL_VRBRAIN/RCInput.h | 7 ----- 2 files changed, 1 insertion(+), 45 deletions(-) diff --git a/libraries/AP_HAL_VRBRAIN/RCInput.cpp b/libraries/AP_HAL_VRBRAIN/RCInput.cpp index 430035d213..55297bba01 100644 --- a/libraries/AP_HAL_VRBRAIN/RCInput.cpp +++ b/libraries/AP_HAL_VRBRAIN/RCInput.cpp @@ -21,7 +21,6 @@ void VRBRAINRCInput::init() if (_rc_sub == -1) { AP_HAL::panic("Unable to subscribe to input_rc"); } - clear_overrides(); pthread_mutex_init(&rcin_mutex, nullptr); } @@ -33,12 +32,7 @@ bool VRBRAINRCInput::new_input() // don't consider input valid if we are in RC failsafe. valid = false; } - if (_override_valid) { - // if we have RC overrides active, then always consider it valid - valid = true; - } _last_read = _rcin.timestamp_last_signal; - _override_valid = false; pthread_mutex_unlock(&rcin_mutex); if (_rcin.input_source != last_input_source) { gcs().send_text(MAV_SEVERITY_DEBUG, "RCInput: decoding %s", input_source_name(_rcin.input_source)); @@ -57,19 +51,10 @@ uint8_t VRBRAINRCInput::num_channels() uint16_t VRBRAINRCInput::read(uint8_t ch) { - if (ch >= RC_INPUT_MAX_CHANNELS) { + if (ch >= MIN(RC_INPUT_MAX_CHANNELS, _rcin.channel_count)) { return 0; } pthread_mutex_lock(&rcin_mutex); - if (_override[ch]) { - uint16_t v = _override[ch]; - pthread_mutex_unlock(&rcin_mutex); - return v; - } - if (ch >= _rcin.channel_count) { - pthread_mutex_unlock(&rcin_mutex); - return 0; - } uint16_t v = _rcin.values[ch]; pthread_mutex_unlock(&rcin_mutex); return v; @@ -86,28 +71,6 @@ uint8_t VRBRAINRCInput::read(uint16_t* periods, uint8_t len) return len; } -bool VRBRAINRCInput::set_override(uint8_t channel, int16_t override) { - if (override < 0) { - return false; /* -1: no change. */ - } - if (channel >= RC_INPUT_MAX_CHANNELS) { - return false; - } - _override[channel] = override; - if (override != 0) { - _override_valid = true; - return true; - } - return false; -} - -void VRBRAINRCInput::clear_overrides() -{ - for (uint8_t i = 0; i < RC_INPUT_MAX_CHANNELS; i++) { - set_override(i, 0); - } -} - const char *VRBRAINRCInput::input_source_name(uint8_t id) const { switch(id) { diff --git a/libraries/AP_HAL_VRBRAIN/RCInput.h b/libraries/AP_HAL_VRBRAIN/RCInput.h index 989c497ecd..875a82fa95 100644 --- a/libraries/AP_HAL_VRBRAIN/RCInput.h +++ b/libraries/AP_HAL_VRBRAIN/RCInput.h @@ -22,21 +22,14 @@ public: return _rssi; } - - bool set_override(uint8_t channel, int16_t override) override; - void clear_overrides() override; - void _timer_tick(void); bool rc_bind(int dsmMode) override; private: - /* override state */ - uint16_t _override[RC_INPUT_MAX_CHANNELS]; struct rc_input_values _rcin; int _rc_sub; uint64_t _last_read; - bool _override_valid; perf_counter_t _perf_rcin; pthread_mutex_t rcin_mutex; int16_t _rssi = -1;