aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

HAL_ChibiOS: implement pulse_input_enable()

Browse Source 
and cleanup use of mutexes in RC input
This commit is contained in:
Andrew Tridgell 2020-01-29 18:38:33 +11:00 committed by Randy Mackay
parent 89d0562e9d
commit 50a6d63101
2 changed files with 67 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
libraries/AP_HAL_ChibiOS/RCInput.cpp
View File

@ -43,15 +43,31 @@ void RCInput::init()
#if HAL_USE_ICU == TRUE #if HAL_USE_ICU == TRUE
//attach timer channel on which the signal will be received //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); sig_reader.attach_capture_timer(&RCIN_ICU_TIMER, RCIN_ICU_CHANNEL, STM32_RCIN_DMA_STREAM, STM32_RCIN_DMA_CHANNEL);
pulse_input_enabled = true;
#endif #endif
#if HAL_USE_EICU == TRUE #if HAL_USE_EICU == TRUE
sig_reader.init(&RCININT_EICU_TIMER, RCININT_EICU_CHANNEL); sig_reader.init(&RCININT_EICU_TIMER, RCININT_EICU_CHANNEL);
pulse_input_enabled = true;
#endif #endif
_init = true; _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() bool RCInput::new_input()
{ {
if (!_init) { if (!_init) {
@ -90,9 +106,11 @@ uint16_t RCInput::read(uint8_t channel)
if (!_init || (channel >= MIN(RC_INPUT_MAX_CHANNELS, _num_channels))) { if (!_init || (channel >= MIN(RC_INPUT_MAX_CHANNELS, _num_channels))) {
return 0; return 0;
} }
rcin_mutex.take(HAL_SEMAPHORE_BLOCK_FOREVER); uint16_t v;
uint16_t v = _rc_values[channel]; {
rcin_mutex.give(); WITH_SEMAPHORE(rcin_mutex);
v = _rc_values[channel];
}
#if HAL_RCINPUT_WITH_AP_RADIO #if HAL_RCINPUT_WITH_AP_RADIO
if (radio && channel == 0) { if (radio && channel == 0) {
// hook to allow for update of radio on main thread, for mavlink sends // 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) { if (len > RC_INPUT_MAX_CHANNELS) {
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; return len;
} }
@ -122,39 +147,41 @@ void RCInput::_timer_tick(void)
if (!_init) { if (!_init) {
return; return;
} }
#ifndef HAL_NO_UARTDRIVER #ifndef HAL_NO_UARTDRIVER
const char *rc_protocol = nullptr; const char *rc_protocol = nullptr;
#endif #endif
#ifndef HAL_BUILD_AP_PERIPH #ifndef HAL_BUILD_AP_PERIPH
AP_RCProtocol &rcprot = AP::RC();
#if HAL_USE_ICU == TRUE #if HAL_USE_ICU == TRUE
const uint32_t *p; if (pulse_input_enabled) {
uint32_t n; const uint32_t *p;
while ((p = (const uint32_t *)sig_reader.sigbuf.readptr(n)) != nullptr) { uint32_t n;
AP::RC().process_pulse_list(p, n*2, sig_reader.need_swap); while ((p = (const uint32_t *)sig_reader.sigbuf.readptr(n)) != nullptr) {
sig_reader.sigbuf.advance(n); rcprot.process_pulse_list(p, n*2, sig_reader.need_swap);
sig_reader.sigbuf.advance(n);
}
} }
#endif #endif
#if HAL_USE_EICU == TRUE #if HAL_USE_EICU == TRUE
uint32_t width_s0, width_s1; if (pulse_input_enabled) {
while(sig_reader.read(width_s0, width_s1)) { uint32_t width_s0, width_s1;
AP::RC().process_pulse(width_s0, width_s1); while(sig_reader.read(width_s0, width_s1)) {
rcprot.process_pulse(width_s0, width_s1);
}
} }
#endif #endif
if (AP::RC().new_input()) { if (rcprot.new_input()) {
rcin_mutex.take(HAL_SEMAPHORE_BLOCK_FOREVER); WITH_SEMAPHORE(rcin_mutex);
_rcin_timestamp_last_signal = AP_HAL::micros(); _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); _num_channels = MIN(_num_channels, RC_INPUT_MAX_CHANNELS);
for (uint8_t i=0; i<_num_channels; i++) { rcprot.read(_rc_values, _num_channels);
_rc_values[i] = AP::RC().read(i);
}
rcin_mutex.give();
#ifndef HAL_NO_UARTDRIVER #ifndef HAL_NO_UARTDRIVER
rc_protocol = AP::RC().protocol_name(); rc_protocol = rcprot.protocol_name();
#endif #endif
} }
#endif // HAL_BUILD_AP_PERIPH #endif // HAL_BUILD_AP_PERIPH
@ -162,27 +189,27 @@ void RCInput::_timer_tick(void)
#if HAL_RCINPUT_WITH_AP_RADIO #if HAL_RCINPUT_WITH_AP_RADIO
if (radio && radio->last_recv_us() != last_radio_us) { if (radio && radio->last_recv_us() != last_radio_us) {
last_radio_us = radio->last_recv_us(); last_radio_us = radio->last_recv_us();
rcin_mutex.take(HAL_SEMAPHORE_BLOCK_FOREVER); WITH_SEMAPHORE(rcin_mutex);
_rcin_timestamp_last_signal = last_radio_us; _rcin_timestamp_last_signal = last_radio_us;
_num_channels = radio->num_channels(); _num_channels = radio->num_channels();
_num_channels = MIN(_num_channels, RC_INPUT_MAX_CHANNELS); _num_channels = MIN(_num_channels, RC_INPUT_MAX_CHANNELS);
for (uint8_t i=0; i<_num_channels; i++) { for (uint8_t i=0; i<_num_channels; i++) {
_rc_values[i] = radio->read(i); _rc_values[i] = radio->read(i);
} }
rcin_mutex.give();
} }
#endif #endif
#if HAL_WITH_IO_MCU #if HAL_WITH_IO_MCU
rcin_mutex.take(HAL_SEMAPHORE_BLOCK_FOREVER); {
if (AP_BoardConfig::io_enabled() && WITH_SEMAPHORE(rcin_mutex);
iomcu.check_rcinput(last_iomcu_us, _num_channels, _rc_values, RC_INPUT_MAX_CHANNELS)) { if (AP_BoardConfig::io_enabled() &&
_rcin_timestamp_last_signal = last_iomcu_us; 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 #ifndef HAL_NO_UARTDRIVER
rc_protocol = iomcu.get_rc_protocol(); rc_protocol = iomcu.get_rc_protocol();
#endif #endif
}
} }
rcin_mutex.give();
#endif #endif
#ifndef HAL_NO_UARTDRIVER #ifndef HAL_NO_UARTDRIVER
@ -202,11 +229,12 @@ void RCInput::_timer_tick(void)
bool RCInput::rc_bind(int dsmMode) bool RCInput::rc_bind(int dsmMode)
{ {
#if HAL_WITH_IO_MCU #if HAL_WITH_IO_MCU
rcin_mutex.take(HAL_SEMAPHORE_BLOCK_FOREVER); {
if (AP_BoardConfig::io_enabled()) { WITH_SEMAPHORE(rcin_mutex);
iomcu.bind_dsm(dsmMode); if (AP_BoardConfig::io_enabled()) {
iomcu.bind_dsm(dsmMode);
}
} }
rcin_mutex.give();
#endif #endif
#ifndef HAL_BUILD_AP_PERIPH #ifndef HAL_BUILD_AP_PERIPH

5
libraries/AP_HAL_ChibiOS/RCInput.h
View File

@ -45,6 +45,10 @@ public:
uint16_t read(uint8_t ch) override; uint16_t read(uint8_t ch) override;
uint8_t read(uint16_t* periods, uint8_t len) 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 { int16_t get_rssi(void) override {
return _rssi; return _rssi;
} }
@ -64,6 +68,7 @@ private:
uint32_t _rcin_timestamp_last_signal; uint32_t _rcin_timestamp_last_signal;
bool _init; bool _init;
const char *last_protocol; const char *last_protocol;
bool pulse_input_enabled;
#if HAL_RCINPUT_WITH_AP_RADIO #if HAL_RCINPUT_WITH_AP_RADIO
bool _radio_init; bool _radio_init;