Browse Source

AP_Radio: remove use of never-set AP_HAL_PX4

master
Peter Barker 6 years ago committed by Andrew Tridgell
parent
commit
5e15dc6967
  1. 4
      libraries/AP_Radio/AP_Radio_cc2500.cpp
  2. 40
      libraries/AP_Radio/AP_Radio_cypress.cpp
  3. 16
      libraries/AP_Radio/AP_Radio_cypress.h

4
libraries/AP_Radio/AP_Radio_cc2500.cpp

@ -282,9 +282,7 @@ void AP_Radio_cc2500::radio_init(void) @@ -282,9 +282,7 @@ void AP_Radio_cc2500::radio_init(void)
hal.scheduler->delay_microseconds(10*1000);
// 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
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
hal.gpio->attach_interrupt(HAL_GPIO_RADIO_IRQ, trigger_irq_radio_event, AP_HAL::GPIO::INTERRUPT_RISING);
#endif

40
libraries/AP_Radio/AP_Radio_cypress.cpp

@ -3,9 +3,6 @@ @@ -3,9 +3,6 @@
#if HAL_RCINPUT_WITH_AP_RADIO
#include <AP_Math/AP_Math.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#include <board_config.h>
#endif
#include "AP_Radio_cypress.h"
#include <utility>
#include <stdio.h>
@ -292,15 +289,7 @@ bool AP_Radio_cypress::reset(void) @@ -292,15 +289,7 @@ bool AP_Radio_cypress::reset(void)
/*
to reset radio hold reset high for 0.5s, then low for 0.5s
*/
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
stm32_configgpio(CYRF_RESET_PIN);
stm32_gpiowrite(CYRF_RESET_PIN, 1);
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 defined(HAL_GPIO_RADIO_RESET)
#if defined(HAL_GPIO_RADIO_RESET)
hal.gpio->write(HAL_GPIO_RADIO_RESET, 1);
hal.scheduler->delay(500);
hal.gpio->write(HAL_GPIO_RADIO_RESET, 0);
@ -643,9 +632,7 @@ void AP_Radio_cypress::radio_init(void) @@ -643,9 +632,7 @@ void AP_Radio_cypress::radio_init(void)
start_receive();
// 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
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
hal.gpio->attach_interrupt(HAL_GPIO_RADIO_IRQ, trigger_irq_radio_event, AP_HAL::GPIO::INTERRUPT_RISING);
#endif
}
@ -945,9 +932,7 @@ void AP_Radio_cypress::dsm2_start_sync(void) @@ -945,9 +932,7 @@ void AP_Radio_cypress::dsm2_start_sync(void)
*/
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
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
chVTSet(&timeout_vt, chTimeMS2I(timeout_ms), trigger_timeout_event, nullptr);
#endif
}
@ -1201,27 +1186,10 @@ void AP_Radio_cypress::irq_timeout(void) @@ -1201,27 +1186,10 @@ void AP_Radio_cypress::irq_timeout(void)
}
/*
called on rising edge of radio IRQ pin
*/
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
int AP_Radio_cypress::irq_radio_trampoline(int irq, void *context)
{
radio_instance->irq_handler();
return 0;
}
#endif
/*
called on HRT timeout
*/
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
int AP_Radio_cypress::irq_timeout_trampoline(int irq, void *context)
{
radio_instance->irq_timeout();
return 0;
}
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
void AP_Radio_cypress::irq_handler_thd(void *arg)
{
_irq_handler_ctx = chThdGetSelfX();

16
libraries/AP_Radio/AP_Radio_cypress.h

@ -24,11 +24,7 @@ @@ -24,11 +24,7 @@
*/
#include "AP_Radio_backend.h"
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#include <nuttx/arch.h>
#include <systemlib/systemlib.h>
#include <drivers/drv_hrt.h>
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#include "hal.h"
#endif
#include "telem_structure.h"
@ -114,10 +110,7 @@ private: @@ -114,10 +110,7 @@ private:
static const config cyrf_bind_config[];
static const config cyrf_transfer_config[];
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
sem_t irq_sem;
struct hrt_call wait_call;
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
virtual_timer_t timeout_vt;
static thread_t *_irq_handler_ctx;
#endif
@ -136,10 +129,7 @@ private: @@ -136,10 +129,7 @@ private:
// trampoline functions to take us from static IRQ function to class functions
#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
#if 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);

Loading…
Cancel
Save