AP_HAL_ChibiOS: move handling of AP_Radio RC input down into AP_RCProtocol

This commit is contained in:
Peter Barker 2024-05-01 13:23:46 +10:00 committed by Andrew Tridgell
parent 6d9a75b5ec
commit c521a5edfe
5 changed files with 15 additions and 58 deletions

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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