Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues 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 13:23:18 +11:00
parent 9c0cf0871b
commit 3fe3c8ecdc
2 changed files with 68 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
libraries/AP_HAL_ChibiOS/RCInput.cpp
View File

@ -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
if (pulse_input_enabled) {
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);
rcprot.process_pulse_list(p, n*2, sig_reader.need_swap);
sig_reader.sigbuf.advance(n);
}
}
#endif
#if HAL_USE_EICU == TRUE
if (pulse_input_enabled) {
uint32_t width_s0, width_s1;
while(sig_reader.read(width_s0, width_s1)) {
AP::RC().process_pulse(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,19 +190,19 @@ 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();
{
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;
@ -184,7 +211,7 @@ void RCInput::_timer_tick(void)
_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();
{
WITH_SEMAPHORE(rcin_mutex);
if (AP_BoardConfig::io_enabled()) {
iomcu.bind_dsm(dsmMode);
}
rcin_mutex.give();
}
#endif
#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;
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;