From 3fe3c8ecdc4534e8fbc4afc112ecba73e9282f82 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 29 Jan 2020 13:23:18 +1100 Subject: [PATCH] HAL_ChibiOS: implement pulse_input_enable() and cleanup use of mutexes in RC input --- libraries/AP_HAL_ChibiOS/RCInput.cpp | 98 ++++++++++++++++++---------- libraries/AP_HAL_ChibiOS/RCInput.h | 5 ++ 2 files changed, 68 insertions(+), 35 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/RCInput.cpp b/libraries/AP_HAL_ChibiOS/RCInput.cpp index 114fca9173..69d06fb577 100644 --- a/libraries/AP_HAL_ChibiOS/RCInput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCInput.cpp @@ -43,15 +43,31 @@ void RCInput::init() #if HAL_USE_ICU == TRUE //attach timer channel on which the signal will be received sig_reader.attach_capture_timer(&RCIN_ICU_TIMER, RCIN_ICU_CHANNEL, STM32_RCIN_DMA_STREAM, STM32_RCIN_DMA_CHANNEL); + pulse_input_enabled = true; #endif #if HAL_USE_EICU == TRUE sig_reader.init(&RCININT_EICU_TIMER, RCININT_EICU_CHANNEL); + pulse_input_enabled = true; #endif _init = true; } +/* + enable or disable pulse input for RC input. This is used to reduce + load when we are decoding R/C via a UART +*/ +void RCInput::pulse_input_enable(bool enable) +{ + pulse_input_enabled = enable; +#if HAL_USE_ICU == TRUE || HAL_USE_EICU == TRUE + if (!enable) { + sig_reader.disable(); + } +#endif +} + bool RCInput::new_input() { if (!_init) { @@ -90,9 +106,11 @@ uint16_t RCInput::read(uint8_t channel) if (!_init || (channel >= MIN(RC_INPUT_MAX_CHANNELS, _num_channels))) { return 0; } - rcin_mutex.take_blocking(); - uint16_t v = _rc_values[channel]; - rcin_mutex.give(); + uint16_t v; + { + WITH_SEMAPHORE(rcin_mutex); + v = _rc_values[channel]; + } #if HAL_RCINPUT_WITH_AP_RADIO if (radio && channel == 0) { // hook to allow for update of radio on main thread, for mavlink sends @@ -111,9 +129,16 @@ uint8_t RCInput::read(uint16_t* periods, uint8_t len) if (len > RC_INPUT_MAX_CHANNELS) { len = RC_INPUT_MAX_CHANNELS; } - for (uint8_t i = 0; i < len; i++){ - periods[i] = read(i); + { + WITH_SEMAPHORE(rcin_mutex); + memcpy(periods, _rc_values, len*sizeof(periods[0])); } +#if HAL_RCINPUT_WITH_AP_RADIO + if (radio && channel == 0) { + // hook to allow for update of radio on main thread, for mavlink sends + radio->update(); + } +#endif return len; } @@ -122,6 +147,7 @@ void RCInput::_timer_tick(void) if (!_init) { return; } + AP_RCProtocol &rcprot = AP::RC(); #ifndef HAL_NO_UARTDRIVER const char *rc_protocol = nullptr; @@ -129,33 +155,34 @@ void RCInput::_timer_tick(void) #ifndef HAL_BUILD_AP_PERIPH #if HAL_USE_ICU == TRUE - const uint32_t *p; - uint32_t n; - while ((p = (const uint32_t *)sig_reader.sigbuf.readptr(n)) != nullptr) { - AP::RC().process_pulse_list(p, n*2, sig_reader.need_swap); - sig_reader.sigbuf.advance(n); + if (pulse_input_enabled) { + const uint32_t *p; + uint32_t n; + while ((p = (const uint32_t *)sig_reader.sigbuf.readptr(n)) != nullptr) { + rcprot.process_pulse_list(p, n*2, sig_reader.need_swap); + sig_reader.sigbuf.advance(n); + } } #endif #if HAL_USE_EICU == TRUE - uint32_t width_s0, width_s1; - while(sig_reader.read(width_s0, width_s1)) { - AP::RC().process_pulse(width_s0, width_s1); + if (pulse_input_enabled) { + uint32_t width_s0, width_s1; + while(sig_reader.read(width_s0, width_s1)) { + rcprot.process_pulse(width_s0, width_s1); + } } #endif - if (AP::RC().new_input()) { - rcin_mutex.take_blocking(); + if (rcprot.new_input()) { + WITH_SEMAPHORE(rcin_mutex); _rcin_timestamp_last_signal = AP_HAL::micros(); - _num_channels = AP::RC().num_channels(); + _num_channels = rcprot.num_channels(); _num_channels = MIN(_num_channels, RC_INPUT_MAX_CHANNELS); - for (uint8_t i=0; i<_num_channels; i++) { - _rc_values[i] = AP::RC().read(i); - } - _rssi = AP::RC().get_RSSI(); - rcin_mutex.give(); + rcprot.read(_rc_values, _num_channels); + _rssi = rcprot.get_RSSI(); #ifndef HAL_NO_UARTDRIVER - rc_protocol = AP::RC().protocol_name(); + rc_protocol = rcprot.protocol_name(); #endif } #endif // HAL_BUILD_AP_PERIPH @@ -163,28 +190,28 @@ void RCInput::_timer_tick(void) #if HAL_RCINPUT_WITH_AP_RADIO if (radio && radio->last_recv_us() != last_radio_us) { last_radio_us = radio->last_recv_us(); - rcin_mutex.take_blocking(); + WITH_SEMAPHORE(rcin_mutex); _rcin_timestamp_last_signal = last_radio_us; _num_channels = radio->num_channels(); _num_channels = MIN(_num_channels, RC_INPUT_MAX_CHANNELS); for (uint8_t i=0; i<_num_channels; i++) { _rc_values[i] = radio->read(i); } - rcin_mutex.give(); } #endif #if HAL_WITH_IO_MCU - rcin_mutex.take_blocking(); - if (AP_BoardConfig::io_enabled() && - iomcu.check_rcinput(last_iomcu_us, _num_channels, _rc_values, RC_INPUT_MAX_CHANNELS)) { - _rcin_timestamp_last_signal = last_iomcu_us; + { + WITH_SEMAPHORE(rcin_mutex); + if (AP_BoardConfig::io_enabled() && + iomcu.check_rcinput(last_iomcu_us, _num_channels, _rc_values, RC_INPUT_MAX_CHANNELS)) { + _rcin_timestamp_last_signal = last_iomcu_us; #ifndef HAL_NO_UARTDRIVER - rc_protocol = iomcu.get_rc_protocol(); - _rssi = iomcu.get_RSSI(); + rc_protocol = iomcu.get_rc_protocol(); + _rssi = iomcu.get_RSSI(); #endif + } } - rcin_mutex.give(); #endif #ifndef HAL_NO_UARTDRIVER @@ -204,11 +231,12 @@ void RCInput::_timer_tick(void) bool RCInput::rc_bind(int dsmMode) { #if HAL_WITH_IO_MCU - rcin_mutex.take_blocking(); - if (AP_BoardConfig::io_enabled()) { - iomcu.bind_dsm(dsmMode); + { + WITH_SEMAPHORE(rcin_mutex); + if (AP_BoardConfig::io_enabled()) { + iomcu.bind_dsm(dsmMode); + } } - rcin_mutex.give(); #endif #ifndef HAL_BUILD_AP_PERIPH diff --git a/libraries/AP_HAL_ChibiOS/RCInput.h b/libraries/AP_HAL_ChibiOS/RCInput.h index a841279205..1751ea842d 100644 --- a/libraries/AP_HAL_ChibiOS/RCInput.h +++ b/libraries/AP_HAL_ChibiOS/RCInput.h @@ -45,6 +45,10 @@ public: uint16_t read(uint8_t ch) override; uint8_t read(uint16_t* periods, uint8_t len) override; + /* enable or disable pulse input for RC input. This is used to + reduce load when we are decoding R/C via a UART */ + void pulse_input_enable(bool enable) override; + int16_t get_rssi(void) override { return _rssi; } @@ -64,6 +68,7 @@ private: uint32_t _rcin_timestamp_last_signal; bool _init; const char *last_protocol; + bool pulse_input_enabled; #if HAL_RCINPUT_WITH_AP_RADIO bool _radio_init;