|
|
|
@ -36,15 +36,15 @@ using namespace ChibiOS;
@@ -36,15 +36,15 @@ using namespace ChibiOS;
|
|
|
|
|
extern const AP_HAL::HAL& hal; |
|
|
|
|
void RCInput::init() |
|
|
|
|
{ |
|
|
|
|
AP::RC().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); |
|
|
|
|
rcin_prot.init(); |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
#if HAL_USE_EICU == TRUE |
|
|
|
|
sig_reader.init(&RCININT_EICU_TIMER, RCININT_EICU_CHANNEL); |
|
|
|
|
rcin_prot.init(); |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
_init = true; |
|
|
|
@ -125,7 +125,7 @@ void RCInput::_timer_tick(void)
@@ -125,7 +125,7 @@ void RCInput::_timer_tick(void)
|
|
|
|
|
const uint32_t *p; |
|
|
|
|
uint32_t n; |
|
|
|
|
while ((p = (const uint32_t *)sig_reader.sigbuf.readptr(n)) != nullptr) { |
|
|
|
|
rcin_prot.process_pulse_list(p, n*2, sig_reader.need_swap); |
|
|
|
|
AP::RC().process_pulse_list(p, n*2, sig_reader.need_swap); |
|
|
|
|
sig_reader.sigbuf.advance(n); |
|
|
|
|
} |
|
|
|
|
#endif |
|
|
|
@ -133,7 +133,7 @@ void RCInput::_timer_tick(void)
@@ -133,7 +133,7 @@ void RCInput::_timer_tick(void)
|
|
|
|
|
#if HAL_USE_EICU == TRUE |
|
|
|
|
uint32_t width_s0, width_s1; |
|
|
|
|
while(sig_reader.read(width_s0, width_s1)) { |
|
|
|
|
rcin_prot.process_pulse(width_s0, width_s1); |
|
|
|
|
AP::RC().process_pulse(width_s0, width_s1); |
|
|
|
|
} |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
@ -141,21 +141,19 @@ void RCInput::_timer_tick(void)
@@ -141,21 +141,19 @@ void RCInput::_timer_tick(void)
|
|
|
|
|
const char *rc_protocol = nullptr; |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
#if HAL_USE_ICU == TRUE || HAL_USE_EICU == TRUE |
|
|
|
|
if (rcin_prot.new_input()) { |
|
|
|
|
if (AP::RC().new_input()) { |
|
|
|
|
rcin_mutex.take(HAL_SEMAPHORE_BLOCK_FOREVER); |
|
|
|
|
_rcin_timestamp_last_signal = AP_HAL::micros(); |
|
|
|
|
_num_channels = rcin_prot.num_channels(); |
|
|
|
|
_num_channels = AP::RC().num_channels(); |
|
|
|
|
_num_channels = MIN(_num_channels, RC_INPUT_MAX_CHANNELS); |
|
|
|
|
for (uint8_t i=0; i<_num_channels; i++) { |
|
|
|
|
_rc_values[i] = rcin_prot.read(i); |
|
|
|
|
_rc_values[i] = AP::RC().read(i); |
|
|
|
|
} |
|
|
|
|
rcin_mutex.give(); |
|
|
|
|
#ifndef HAL_NO_UARTDRIVER |
|
|
|
|
rc_protocol = rcin_prot.protocol_name(); |
|
|
|
|
rc_protocol = AP::RC().protocol_name(); |
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
#if HAL_RCINPUT_WITH_AP_RADIO |
|
|
|
|
if (radio && radio->last_recv_us() != last_radio_us) { |
|
|
|
@ -207,10 +205,8 @@ bool RCInput::rc_bind(int dsmMode)
@@ -207,10 +205,8 @@ bool RCInput::rc_bind(int dsmMode)
|
|
|
|
|
rcin_mutex.give(); |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
#if HAL_USE_ICU == TRUE |
|
|
|
|
// ask AP_RCProtocol to start a bind
|
|
|
|
|
rcin_prot.start_bind(); |
|
|
|
|
#endif |
|
|
|
|
AP::RC().start_bind(); |
|
|
|
|
|
|
|
|
|
#if HAL_RCINPUT_WITH_AP_RADIO |
|
|
|
|
if (radio) { |
|
|
|
|