Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RSSI: make RSSI work on ChibiOS

Browse Source
This commit is contained in:
Peter Barker 2019-02-01 13:38:13 +11:00 committed by Randy Mackay
parent 6af9be44a7
commit 2550c9922d
2 changed files with 60 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
libraries/AP_RSSI/AP_RSSI.cpp
View File

@ -191,9 +191,11 @@ float AP_RSSI::read_channel_rssi()
return channel_rssi;
}
// read the PWM value from a pin
float AP_RSSI::read_pwm_pin_rssi()
{
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN || CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
// check if pin has changed and initialise gpio event callback
pwm_state.gpio = get_gpio(rssi_analog_pin);
@ -211,9 +213,34 @@ float AP_RSSI::read_pwm_pin_rssi()
pwm_state.last_gpio = pwm_state.gpio;
}
}
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
if (pwm_state.gpio != rssi_analog_pin) {
pwm_state.gpio = rssi_analog_pin;
// remove old gpio event callback if present
if (pwm_state.last_gpio != 0) {
hal.gpio->attach_interrupt(pwm_state.gpio,
nullptr,
HAL_GPIO_INTERRUPT_BOTH);
pwm_state.last_gpio = 0;
}
// install interrupt handler on rising or falling edge of gpio
if (pwm_state.gpio != 0) {
hal.gpio->pinMode(pwm_state.gpio, HAL_GPIO_INPUT);
hal.gpio->attach_interrupt(pwm_state.gpio,
irq_handler_chibios,
HAL_GPIO_INTERRUPT_BOTH);
pwm_state.gpio = pwm_state.gpio;
}
}
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
// disable interrupts temporarily
irqstate_t istate = irqsave();
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
void *istate = hal.scheduler->disable_interrupts_save();
#endif
// check for timeout
float ret;
@ -225,8 +252,12 @@ float AP_RSSI::read_pwm_pin_rssi()
ret = scale_and_constrain_float_rssi(pwm_state.value, rssi_channel_low_pwm_value, rssi_channel_high_pwm_value);
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
// restore interrupts
irqrestore(istate);
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
hal.scheduler->restore_interrupts(istate);
#endif
return ret;
#else
@ -264,6 +295,7 @@ float AP_RSSI::scale_and_constrain_float_rssi(float current_rssi_value, float lo
return constrain_float(rssi_value_scaled, 0.0f, 1.0f);
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
// get gpio id from pin number
uint32_t AP_RSSI::get_gpio(uint8_t pin_number) const
{
@ -289,17 +321,23 @@ uint32_t AP_RSSI::get_gpio(uint8_t pin_number) const
// interrupt handler for reading pwm value
int AP_RSSI::irq_handler(int irq, void *context)
{
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
// sanity check
if (pwm_state.gpio == 0) {
return 0;
}
// capture time
uint64_t now = AP_HAL::micros64();
// read value of pin
bool pin_high = stm32_gpioread(pwm_state.gpio);
irq_handler_calc_pwm(pin_high);
return 0;
}
#endif
void AP_RSSI::irq_handler_calc_pwm(bool pin_high)
{
// capture time
const uint64_t now = AP_HAL::micros64();
// calculate pwm value
if (pin_high) {
@ -311,11 +349,18 @@ int AP_RSSI::irq_handler(int irq, void *context)
pwm_state.pulse_start_us = 0;
pwm_state.last_reading_ms = AP_HAL::millis();
}
#endif
return 0;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
// interrupt handler for reading pwm value
void AP_RSSI::irq_handler_chibios()
{
// read value of pin
bool pin_high = hal.gpio->read(pwm_state.gpio);
irq_handler_calc_pwm(pin_high);
}
#endif
AP_RSSI *AP_RSSI::_s_instance = nullptr;
namespace AP {

8
libraries/AP_RSSI/AP_RSSI.h
View File

@ -96,8 +96,16 @@ private:
float scale_and_constrain_float_rssi(float current_rssi_value, float low_rssi_range, float high_rssi_range);
// PWM input handling
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
uint32_t get_gpio(uint8_t pin_number) const;
static int irq_handler(int irq, void *context);
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
static void irq_handler_chibios(void);
#endif
static void irq_handler_calc_pwm(bool pin_high);
};
namespace AP {