AP_HAL_ChibiOS: move handling of AP_Radio RC input down into AP_RCProtocol
This commit is contained in:
parent
6d9a75b5ec
commit
c521a5edfe
@ -84,15 +84,6 @@ bool RCInput::new_input()
|
||||
_last_read = _rcin_timestamp_last_signal;
|
||||
}
|
||||
|
||||
#if HAL_RCINPUT_WITH_AP_RADIO
|
||||
if (!_radio_init) {
|
||||
_radio_init = true;
|
||||
radio = AP_Radio::get_singleton();
|
||||
if (radio) {
|
||||
radio->init();
|
||||
}
|
||||
}
|
||||
#endif
|
||||
return valid;
|
||||
}
|
||||
|
||||
@ -114,12 +105,6 @@ uint16_t RCInput::read(uint8_t channel)
|
||||
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
|
||||
radio->update();
|
||||
}
|
||||
#endif
|
||||
return v;
|
||||
}
|
||||
|
||||
@ -136,12 +121,6 @@ uint8_t RCInput::read(uint16_t* periods, uint8_t len)
|
||||
WITH_SEMAPHORE(rcin_mutex);
|
||||
memcpy(periods, _rc_values, len*sizeof(periods[0]));
|
||||
}
|
||||
#if HAL_RCINPUT_WITH_AP_RADIO
|
||||
if (radio) {
|
||||
// hook to allow for update of radio on main thread, for mavlink sends
|
||||
radio->update();
|
||||
}
|
||||
#endif
|
||||
return len;
|
||||
}
|
||||
|
||||
@ -186,7 +165,7 @@ void RCInput::_timer_tick(void)
|
||||
if (!have_iocmu_rc) {
|
||||
_rcin_last_iomcu_ms = 0;
|
||||
}
|
||||
#elif AP_RCPROTOCOL_ENABLED || HAL_RCINPUT_WITH_AP_RADIO
|
||||
#elif AP_RCPROTOCOL_ENABLED
|
||||
const bool have_iocmu_rc = false;
|
||||
#endif
|
||||
|
||||
@ -206,22 +185,6 @@ void RCInput::_timer_tick(void)
|
||||
}
|
||||
#endif // AP_RCPROTOCOL_ENABLED
|
||||
|
||||
#if HAL_RCINPUT_WITH_AP_RADIO
|
||||
if (radio && radio->last_recv_us() != last_radio_us && !have_iocmu_rc) {
|
||||
last_radio_us = radio->last_recv_us();
|
||||
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);
|
||||
}
|
||||
#ifndef HAL_NO_UARTDRIVER
|
||||
source = RCSource::APRADIO;
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
#if HAL_WITH_IO_MCU
|
||||
{
|
||||
WITH_SEMAPHORE(rcin_mutex);
|
||||
@ -269,11 +232,6 @@ bool RCInput::rc_bind(int dsmMode)
|
||||
AP::RC().start_bind();
|
||||
#endif
|
||||
|
||||
#if HAL_RCINPUT_WITH_AP_RADIO
|
||||
if (radio) {
|
||||
radio->start_recv_bind();
|
||||
}
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
#endif //#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
|
||||
|
@ -19,10 +19,6 @@
|
||||
#include "AP_HAL_ChibiOS.h"
|
||||
#include "Semaphores.h"
|
||||
|
||||
#if HAL_RCINPUT_WITH_AP_RADIO
|
||||
#include <AP_Radio/AP_Radio.h>
|
||||
#endif
|
||||
|
||||
#include <AP_RCProtocol/AP_RCProtocol.h>
|
||||
|
||||
#if HAL_USE_ICU == TRUE
|
||||
@ -80,17 +76,10 @@ private:
|
||||
IOMCU = 1,
|
||||
RCPROT_PULSES = 2,
|
||||
RCPROT_BYTES = 3,
|
||||
APRADIO = 4,
|
||||
} last_source;
|
||||
|
||||
bool pulse_input_enabled;
|
||||
|
||||
#if HAL_RCINPUT_WITH_AP_RADIO
|
||||
bool _radio_init;
|
||||
AP_Radio *radio;
|
||||
uint32_t last_radio_us;
|
||||
#endif
|
||||
|
||||
#if HAL_USE_ICU == TRUE
|
||||
ChibiOS::SoftSigReader sig_reader;
|
||||
#endif
|
||||
|
@ -109,11 +109,15 @@ IMU Invensense SPI:icm20789 ROTATION_NONE
|
||||
# radio IRQ is on GPIO(100)
|
||||
define HAL_GPIO_RADIO_IRQ 100
|
||||
|
||||
define HAL_RCINPUT_WITH_AP_RADIO 1
|
||||
define AP_RADIO_ENABLED 1
|
||||
define AP_RADIO_BACKEND_DEFAULT_ENABLED 0
|
||||
define AP_RADIO_CC2500_ENABLED 1
|
||||
define AP_RADIO_CYRF6936_ENABLED 1
|
||||
|
||||
define AP_RCPROTOCOL_ENABLED 1
|
||||
define AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED 0
|
||||
define AP_RCPROTOCOL_RADIO_ENABLED 1
|
||||
|
||||
STORAGE_FLASH_PAGE 1
|
||||
define HAL_STORAGE_SIZE 15360
|
||||
|
||||
|
@ -99,12 +99,16 @@ IMU Invensense I2C:1:0x68 ROTATION_NONE
|
||||
# radio IRQ is on GPIO(100)
|
||||
define HAL_GPIO_RADIO_IRQ 100
|
||||
|
||||
define HAL_RCINPUT_WITH_AP_RADIO 1
|
||||
define AP_RADIO_ENABLED 1
|
||||
define AP_RADIO_BACKEND_DEFAULT_ENABLED 0
|
||||
define AP_RADIO_CC2500_ENABLED 1
|
||||
define AP_RADIO_CYRF6936_ENABLED 1
|
||||
define AP_RADIO_BK2425_ENABLED 1
|
||||
|
||||
define AP_RCPROTOCOL_ENABLED 1
|
||||
define AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED 0
|
||||
define AP_RCPROTOCOL_RADIO_ENABLED 1
|
||||
|
||||
STORAGE_FLASH_PAGE 1
|
||||
define HAL_STORAGE_SIZE 15360
|
||||
|
||||
|
@ -31,7 +31,7 @@ define CHIBIOS_SHORT_BOARD_NAME "skyviper-v245"
|
||||
SERIAL_ORDER OTG1 USART2 EMPTY UART4
|
||||
|
||||
# enable AP_Radio support
|
||||
define HAL_RCINPUT_WITH_AP_RADIO 1
|
||||
define AP_RADIO_ENABLED 1
|
||||
define AP_RADIO_BACKEND_DEFAULT_ENABLED 0
|
||||
define AP_RADIO_CC2500_ENABLED 1
|
||||
define AP_RADIO_CYRF6936_ENABLED 1
|
||||
@ -170,7 +170,9 @@ define AP_CAMERA_BACKEND_DEFAULT_ENABLED 0
|
||||
define AP_CAMERA_MAVLINK_ENABLED 1
|
||||
|
||||
# SkyViper uses AP_Radio, which does its own RC protocol decoding:
|
||||
define AP_RCPROTOCOL_ENABLED 0
|
||||
define AP_RCPROTOCOL_ENABLED 1
|
||||
define AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED 0
|
||||
define AP_RCPROTOCOL_RADIO_ENABLED 1
|
||||
|
||||
// few prelcnad backends are applicable
|
||||
define AC_PRECLAND_BACKEND_DEFAULT_ENABLED 0
|
||||
|
Loading…
Reference in New Issue
Block a user