Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 09:43:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Radio: improved timeout abstraction in cypress driver

Browse Source
This commit is contained in:
Andrew Tridgell 2018-01-20 13:16:14 +11:00
parent 6faace278a
commit 65f4c0ba50
2 changed files with 41 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
libraries/AP_Radio/AP_Radio_cypress.cpp
View File

@ -25,7 +25,7 @@
*/ */
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
static THD_WORKING_AREA(_irq_handler_wa, 512); static THD_WORKING_AREA(_irq_handler_wa, 512);
#define TIMEOUT_PRIORITY 250 //Right above timer thread #define TIMEOUT_PRIORITY 181
#define EVT_TIMEOUT EVENT_MASK(0) #define EVT_TIMEOUT EVENT_MASK(0)
#define EVT_IRQ EVENT_MASK(1) #define EVT_IRQ EVENT_MASK(1)
#endif #endif
@ -943,6 +943,18 @@ void AP_Radio_cypress::dsm2_start_sync(void)
} }
} }
/*
setup a timeout in timeout_ms milliseconds
*/
void AP_Radio_cypress::setup_timeout(uint32_t timeout_ms)
{
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
hrt_call_after(&wait_call, timeout_ms*1000, (hrt_callout)irq_timeout_trampoline, nullptr);
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
chVTSet(&timeout_vt, MS2ST(timeout_ms), trigger_timeout_event, nullptr);
#endif
}
/* /*
process an incoming packet process an incoming packet
*/ */
@ -1013,11 +1025,7 @@ void AP_Radio_cypress::process_packet(const uint8_t *pkt, uint8_t len)
RX bound to the same TX) RX bound to the same TX)
*/ */
state = STATE_SEND_TELEM; state = STATE_SEND_TELEM;
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 setup_timeout(1);
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
} }
} }
} else { } else {
@ -1041,19 +1049,13 @@ void AP_Radio_cypress::start_receive(void)
dsm.receive_start_us = AP_HAL::micros(); dsm.receive_start_us = AP_HAL::micros();
if (state == STATE_AUTOBIND) { if (state == STATE_AUTOBIND) {
dsm.receive_timeout_usec = 90000; dsm.receive_timeout_msec = 90;
} else if (state == STATE_BIND) { } else if (state == STATE_BIND) {
dsm.receive_timeout_usec = 15000; dsm.receive_timeout_msec = 15;
} else { } else {
dsm.receive_timeout_usec = 12000; dsm.receive_timeout_msec = 12;
} }
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 setup_timeout(dsm.receive_timeout_msec);
//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
} }
/* /*
@ -1168,12 +1170,7 @@ void AP_Radio_cypress::irq_timeout(void)
stats.timeouts++; stats.timeouts++;
if (!dev->get_semaphore()->take_nonblocking()) { if (!dev->get_semaphore()->take_nonblocking()) {
// schedule a new timeout // schedule a new timeout
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 setup_timeout(dsm.receive_timeout_msec);
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; return;
} }
@ -1230,17 +1227,15 @@ int AP_Radio_cypress::irq_timeout_trampoline(int irq, void *context)
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS #elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
void AP_Radio_cypress::irq_handler_thd(void *arg) void AP_Radio_cypress::irq_handler_thd(void *arg)
{ {
_irq_handler_ctx = chThdGetSelfX();
(void)arg; (void)arg;
while(true) { while(true) {
eventmask_t evt = chEvtWaitAny(ALL_EVENTS); eventmask_t evt = chEvtWaitAny(ALL_EVENTS);
switch(evt) { if (evt & EVT_IRQ) {
case EVT_IRQ:
radio_instance->irq_handler(); radio_instance->irq_handler();
break; }
case EVT_TIMEOUT: if (evt & EVT_TIMEOUT) {
radio_instance->irq_timeout(); radio_instance->irq_timeout();
break;
default: break;
} }
} }
} }
@ -1250,7 +1245,9 @@ void AP_Radio_cypress::trigger_timeout_event(void *arg)
(void)arg; (void)arg;
//we are called from ISR context //we are called from ISR context
chSysLockFromISR(); chSysLockFromISR();
if (_irq_handler_ctx) {
chEvtSignalI(_irq_handler_ctx, EVT_TIMEOUT); chEvtSignalI(_irq_handler_ctx, EVT_TIMEOUT);
}
chSysUnlockFromISR(); chSysUnlockFromISR();
} }
@ -1258,7 +1255,9 @@ void AP_Radio_cypress::trigger_irq_radio_event()
{ {
//we are called from ISR context //we are called from ISR context
chSysLockFromISR(); chSysLockFromISR();
if (_irq_handler_ctx) {
chEvtSignalI(_irq_handler_ctx, EVT_IRQ); chEvtSignalI(_irq_handler_ctx, EVT_IRQ);
}
chSysUnlockFromISR(); chSysUnlockFromISR();
} }
#endif #endif
@ -1546,6 +1545,8 @@ void AP_Radio_cypress::load_bind_info(void)
dsm.protocol = DSM_DSM2_2; dsm.protocol = DSM_DSM2_2;
dsm2_start_sync(); dsm2_start_sync();
} else if (bind_storage.read_block(&info, 0, sizeof(info)) && info.magic == bind_magic) { } else if (bind_storage.read_block(&info, 0, sizeof(info)) && info.magic == bind_magic) {
Debug(1, "Loaded mfg_id %02x:%02x:%02x:%02x\n",
info.mfg_id[0], info.mfg_id[1], info.mfg_id[2], info.mfg_id[3]);
memcpy(dsm.mfg_id, info.mfg_id, sizeof(info.mfg_id)); memcpy(dsm.mfg_id, info.mfg_id, sizeof(info.mfg_id));
dsm.protocol = info.protocol; dsm.protocol = info.protocol;
} }
@ -1623,12 +1624,7 @@ void AP_Radio_cypress::send_telem_packet(void)
transmit16((uint8_t*)&pkt); transmit16((uint8_t*)&pkt);
state = STATE_SEND_TELEM_WAIT; state = STATE_SEND_TELEM_WAIT;
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 setup_timeout(2);
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
} }
/* /*
@ -1686,33 +1682,18 @@ void AP_Radio_cypress::send_FCC_test_packet(void)
write_register(CYRF_TX_OVERRIDE, CYRF_FRC_PRE); write_register(CYRF_TX_OVERRIDE, CYRF_FRC_PRE);
write_register(CYRF_TX_CTRL, CYRF_TX_GO); write_register(CYRF_TX_CTRL, CYRF_TX_GO);
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 setup_timeout(500);
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 { } else {
write_register(CYRF_XACT_CFG, CYRF_MODE_SYNTH_TX | CYRF_FRC_END); write_register(CYRF_XACT_CFG, CYRF_MODE_SYNTH_TX | CYRF_FRC_END);
write_register(CYRF_RX_ABORT, 0); write_register(CYRF_RX_ABORT, 0);
transmit16(pkt); transmit16(pkt);
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 setup_timeout(10);
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 #else
write_register(CYRF_XACT_CFG, CYRF_MODE_SYNTH_TX | CYRF_FRC_END); write_register(CYRF_XACT_CFG, CYRF_MODE_SYNTH_TX | CYRF_FRC_END);
write_register(CYRF_RX_ABORT, 0); write_register(CYRF_RX_ABORT, 0);
transmit16(pkt); transmit16(pkt);
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 setup_timeout(10);
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
#endif #endif
} }

5
libraries/AP_Radio/AP_Radio_cypress.h
View File

@ -185,7 +185,7 @@ private:
uint8_t last_data_code[16]; uint8_t last_data_code[16];
uint32_t receive_start_us; uint32_t receive_start_us;
uint32_t receive_timeout_usec; uint32_t receive_timeout_msec;
uint32_t last_recv_us; uint32_t last_recv_us;
uint32_t last_parse_us; uint32_t last_parse_us;
@ -286,5 +286,8 @@ private:
// check for double binding // check for double binding
void check_double_bind(void); void check_double_bind(void);
// setup a timeout handler
void setup_timeout(uint32_t timeout_ms);
}; };