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

AP_Radio: add ChibiOS support

Browse Source
This commit is contained in:
Andrew Tridgell 2018-01-05 20:50:10 +11:00
parent 14dcee75f1
commit 6ef5f61faa
2 changed files with 99 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
libraries/AP_Radio/AP_Radio_cypress.cpp
View File

@ -23,9 +23,19 @@
configuration code and register defines
https://github.com/esden/superbitrf-firmware
*/
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
static THD_WORKING_AREA(_irq_handler_wa, 512);
#define TIMEOUT_PRIORITY 250 //Right above timer thread
#define EVT_TIMEOUT EVENT_MASK(0)
#define EVT_IRQ EVENT_MASK(1)
#endif
#ifndef CYRF_SPI_DEVICE
# define CYRF_SPI_DEVICE "cypress"
#endif
#ifndef CYRF_IRQ_INPUT
# error "CYRF_IRQ_INPUT must be defined"
# define CYRF_IRQ_INPUT (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
#endif
#ifndef CYRF_RESET_PIN
@ -235,6 +245,9 @@ enum {
// object instance for trampoline
AP_Radio_cypress *AP_Radio_cypress::radio_instance;
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
thread_t *AP_Radio_cypress::_irq_handler_ctx;
#endif
/*
constructor
*/
@ -250,7 +263,18 @@ AP_Radio_cypress::AP_Radio_cypress(AP_Radio &_radio) :
*/
bool AP_Radio_cypress::init(void)
{
dev = hal.spi->get_device("cypress");
dev = hal.spi->get_device(CYRF_SPI_DEVICE);
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
if(_irq_handler_ctx != nullptr) {
AP_HAL::panic("AP_Radio_cypress: double instantiation of irq_handler\n");
}
chVTObjectInit(&timeout_vt);
_irq_handler_ctx = chThdCreateStatic(_irq_handler_wa,
sizeof(_irq_handler_wa),
TIMEOUT_PRIORITY, /* Initial priority. */
irq_handler_thd, /* Thread function. */
NULL); /* Thread parameter. */
#endif
load_bind_info();
sem = hal.util->new_semaphore();
@ -276,7 +300,13 @@ bool AP_Radio_cypress::reset(void)
hal.scheduler->delay(500);
stm32_gpiowrite(CYRF_RESET_PIN, 0);
hal.scheduler->delay(500);
// use AUX5 as radio IRQ pin
stm32_configgpio(CYRF_IRQ_INPUT);
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
hal.gpio->write(HAL_GPIO_RADIO_RESET, 1);
hal.scheduler->delay(500);
hal.gpio->write(HAL_GPIO_RADIO_RESET, 0);
hal.scheduler->delay(500);
#endif
radio_init();
dev->get_semaphore()->give();
@ -532,15 +562,14 @@ uint8_t AP_Radio_cypress::read_status_debounced(uint8_t adr)
/*
force the initial state of the radio
*/
bool AP_Radio_cypress::force_initial_state(void)
void AP_Radio_cypress::force_initial_state(void)
{
const uint32_t force_initial_state_start_ms = AP_HAL::millis();
while (AP_HAL::millis() - force_initial_state_start_ms < 100) {
while (true) {
write_register(CYRF_XACT_CFG, CYRF_FRC_END);
uint32_t start_ms = AP_HAL::millis();
do {
if ((read_register(CYRF_XACT_CFG) & CYRF_FRC_END) == 0) {
return true; // FORCE_END done (osc running)
return; // FORCE_END done (osc running)
}
} while (AP_HAL::millis() - start_ms < 5);
@ -549,7 +578,6 @@ bool AP_Radio_cypress::force_initial_state(void)
// trying to start oscillator again.
write_register(CYRF_XACT_CFG, CYRF_MODE_SLEEP);
}
return false;
}
/*
@ -606,10 +634,7 @@ void AP_Radio_cypress::radio_init(void)
dsm_setup_transfer_dsmx();
write_register(CYRF_XTAL_CTRL,0x80); // XOUT=BitSerial
if (!force_initial_state()) {
Debug(1, "Cypress: radio_init failed\n");
return;
}
force_initial_state();
write_register(CYRF_PWR_CTRL,0x20); // Disable PMU
// start in RECV state
@ -622,6 +647,8 @@ void AP_Radio_cypress::radio_init(void)
// setup handler for rising edge of IRQ pin
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
stm32_gpiosetevent(CYRF_IRQ_INPUT, true, false, false, irq_radio_trampoline);
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
hal.gpio->attach_interrupt(HAL_GPIO_RADIO_IRQ, trigger_irq_radio_event, HAL_GPIO_INTERRUPT_RISING);
#endif
}
@ -988,6 +1015,8 @@ void AP_Radio_cypress::process_packet(const uint8_t *pkt, uint8_t len)
state = STATE_SEND_TELEM;
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
hrt_call_after(&wait_call, 1000, (hrt_callout)irq_timeout_trampoline, nullptr);
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
chVTSet(&timeout_vt, MS2ST(1), trigger_timeout_event, nullptr);
#endif
}
}
@ -1021,6 +1050,9 @@ void AP_Radio_cypress::start_receive(void)
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
//dsm.receive_timeout_usec = 45000;
hrt_call_after(&wait_call, dsm.receive_timeout_usec, (hrt_callout)irq_timeout_trampoline, nullptr);
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
//Note: please review Frequency in chconf.h to ensure the range of wait
chVTSet(&timeout_vt, US2ST(dsm.receive_timeout_usec), trigger_timeout_event, nullptr);
#endif
}
@ -1138,6 +1170,9 @@ void AP_Radio_cypress::irq_timeout(void)
// schedule a new timeout
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
hrt_call_after(&wait_call, dsm.receive_timeout_usec, (hrt_callout)irq_timeout_trampoline, nullptr);
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
//Note: please review Frequency in chconf.h to ensure the range of wait
chVTSet(&timeout_vt, US2ST(dsm.receive_timeout_usec), trigger_timeout_event, nullptr);
#endif
return;
}
@ -1192,6 +1227,40 @@ int AP_Radio_cypress::irq_timeout_trampoline(int irq, void *context)
radio_instance->irq_timeout();
return 0;
}
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
void AP_Radio_cypress::irq_handler_thd(void *arg)
{
(void)arg;
while(true) {
eventmask_t evt = chEvtWaitAny(ALL_EVENTS);
switch(evt) {
case EVT_IRQ:
radio_instance->irq_handler();
break;
case EVT_TIMEOUT:
radio_instance->irq_timeout();
break;
default: break;
}
}
}
void AP_Radio_cypress::trigger_timeout_event(void *arg)
{
(void)arg;
//we are called from ISR context
chSysLockFromISR();
chEvtSignalI(_irq_handler_ctx, EVT_TIMEOUT);
chSysUnlockFromISR();
}
void AP_Radio_cypress::trigger_irq_radio_event()
{
//we are called from ISR context
chSysLockFromISR();
chEvtSignalI(_irq_handler_ctx, EVT_IRQ);
chSysUnlockFromISR();
}
#endif
/*
Set the current DSM channel with SOP, CRC and data code
@ -1556,6 +1625,9 @@ void AP_Radio_cypress::send_telem_packet(void)
state = STATE_SEND_TELEM_WAIT;
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
hrt_call_after(&wait_call, 2000, (hrt_callout)irq_timeout_trampoline, nullptr);
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
//Note: please review Frequency in chconf.h to ensure the range of wait
chVTSet(&timeout_vt, US2ST(dsm.receive_timeout_usec), trigger_timeout_event, nullptr);
#endif
}
@ -1616,6 +1688,9 @@ void AP_Radio_cypress::send_FCC_test_packet(void)
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
hrt_call_after(&wait_call, 500000, (hrt_callout)irq_timeout_trampoline, nullptr);
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
//Note: please review Frequency in chconf.h to ensure the range of wait
chVTSet(&timeout_vt, MS2ST(500), trigger_timeout_event, nullptr);
#endif
} else {
write_register(CYRF_XACT_CFG, CYRF_MODE_SYNTH_TX | CYRF_FRC_END);
@ -1623,6 +1698,9 @@ void AP_Radio_cypress::send_FCC_test_packet(void)
transmit16(pkt);
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
hrt_call_after(&wait_call, 10000, (hrt_callout)irq_timeout_trampoline, nullptr);
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
//Note: please review Frequency in chconf.h to ensure the range of wait
chVTSet(&timeout_vt, MS2ST(10), trigger_timeout_event, nullptr);
#endif
}
#else

11
libraries/AP_Radio/AP_Radio_cypress.h
View File

@ -28,6 +28,8 @@
#include <nuttx/arch.h>
#include <systemlib/systemlib.h>
#include <drivers/drv_hrt.h>
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#include "hal.h"
#endif
#include "telem_structure.h"
@ -82,7 +84,7 @@ private:
void dump_registers(uint8_t n);
bool force_initial_state(void);
void force_initial_state(void);
void set_channel(uint8_t channel);
uint8_t read_status_debounced(uint8_t adr);
@ -112,6 +114,9 @@ private:
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
sem_t irq_sem;
struct hrt_call wait_call;
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
virtual_timer_t timeout_vt;
static thread_t *_irq_handler_ctx;
#endif
void radio_set_config(const struct config *config, uint8_t size);
@ -131,6 +136,10 @@ private:
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
static int irq_radio_trampoline(int irq, void *context);
static int irq_timeout_trampoline(int irq, void *context);
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
static void irq_handler_thd(void* arg);
static void trigger_irq_radio_event(void);
static void trigger_timeout_event(void *arg);
#endif
static const uint8_t max_channels = 16;