Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_RSSI: convert to HAL attach_interrupt 

AP_RSSI: add error reporting for attaching of interrupts

AP_RSSI: use detach_interrupt method
This commit is contained in:
Peter Barker 2018-08-13 10:53:03 +10:00 committed by Randy Mackay
parent f2b8c9e501
commit d9f393f42e
2 changed files with 82 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

141
libraries/AP_RSSI/AP_RSSI.cpp
View File

@ -15,7 +15,9 @@
#include <AP_RSSI/AP_RSSI.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <GCS_MAVLink/GCS.h>
#include <RC_Channel/RC_Channel.h>
#include <utility>
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include <board_config.h>
@ -41,8 +43,6 @@ extern const AP_HAL::HAL& hal;
#define BOARD_RSSI_ANA_PIN_HIGH 5.0f
#endif
AP_RSSI::PWMState AP_RSSI::pwm_state;
const AP_Param::GroupInfo AP_RSSI::var_info[] = {
// @Param: TYPE
@ -203,44 +203,78 @@ float AP_RSSI::read_channel_rssi()
return channel_rssi;
}
void AP_RSSI::check_pwm_pin_rssi()
{
if (rssi_analog_pin == pwm_state.last_rssi_analog_pin) {
return;
}
// detach last one
if (pwm_state.last_rssi_analog_pin) {
if (!hal.gpio->detach_interrupt(pwm_state.last_rssi_analog_pin)) {
gcs().send_text(MAV_SEVERITY_WARNING,
"RSSI: Failed to detach from pin %u",
pwm_state.last_rssi_analog_pin);
// ignore this failure or the user may be stuck
}
}
pwm_state.last_rssi_analog_pin = rssi_analog_pin;
if (!rssi_analog_pin) {
// don't need to install handler
return;
}
// install interrupt handler on rising and falling edge
if (!hal.gpio->attach_interrupt(
rssi_analog_pin,
FUNCTOR_BIND_MEMBER(&AP_RSSI::irq_handler,
void,
uint8_t,
bool,
uint32_t),
AP_HAL::GPIO::INTERRUPT_BOTH)) {
// failed to attach interrupt
gcs().send_text(MAV_SEVERITY_WARNING,
"RSSI: Failed to attach to pin %u",
rssi_analog_pin);
return;
}
}
// 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
// check if pin has changed and initialise gpio event callback
pwm_state.gpio = get_gpio(rssi_analog_pin);
if (pwm_state.gpio != pwm_state.last_gpio) {
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN || CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
// check if pin has changed and configure interrupt handlers if required:
check_pwm_pin_rssi();
// remove old gpio event callback if present
if (pwm_state.last_gpio != 0) {
stm32_gpiosetevent(pwm_state.last_gpio, false, false, false, nullptr);
pwm_state.last_gpio = 0;
if (!pwm_state.last_rssi_analog_pin) {
// disabled (either by configuration or failure to attach interrupt)
return 0.0f;
}
// install interrupt handler on rising or falling edge of gpio
if (pwm_state.gpio != 0) {
stm32_gpiosetevent(pwm_state.gpio, true, true, false, irq_handler);
pwm_state.last_gpio = pwm_state.gpio;
}
}
// disable interrupts and grab state
void *irqstate = hal.scheduler->disable_interrupts_save();
const uint32_t irq_value_us = pwm_state.irq_value_us;
pwm_state.irq_value_us = 0;
hal.scheduler->restore_interrupts(irqstate);
// disable interrupts temporarily
irqstate_t istate = irqsave();
// check for timeout
float ret;
if ((pwm_state.last_reading_ms == 0) || (AP_HAL::millis() - pwm_state.last_reading_ms > 1000)) {
pwm_state.value = 0;
ret = 0;
const uint32_t now = AP_HAL::millis();
if (irq_value_us == 0) {
// no reading; check for timeout:
if (now - pwm_state.last_reading_ms > 1000) {
// no reading for a second - something is broken
pwm_state.rssi_value = 0.0f;
}
} else {
// convert pwm value to rssi value
ret = scale_and_constrain_float_rssi(pwm_state.value, rssi_channel_low_pwm_value, rssi_channel_high_pwm_value);
// a new reading - convert pwm value to rssi value
pwm_state.rssi_value = scale_and_constrain_float_rssi(irq_value_us, rssi_channel_low_pwm_value, rssi_channel_high_pwm_value);
pwm_state.last_reading_ms = now;
}
// restore interrupts
irqrestore(istate);
return ret;
return pwm_state.rssi_value;
#else
return 0.0f;
#endif
@ -276,56 +310,19 @@ float AP_RSSI::scale_and_constrain_float_rssi(float current_rssi_value, float lo
return constrain_float(rssi_value_scaled, 0.0f, 1.0f);
}
// get gpio id from pin number
uint32_t AP_RSSI::get_gpio(uint8_t pin_number) const
{
#ifdef GPIO_GPIO0_INPUT
switch (pin_number) {
case 50:
return GPIO_GPIO0_INPUT;
case 51:
return GPIO_GPIO1_INPUT;
case 52:
return GPIO_GPIO2_INPUT;
case 53:
return GPIO_GPIO3_INPUT;
case 54:
return GPIO_GPIO4_INPUT;
case 55:
return GPIO_GPIO5_INPUT;
}
#endif
return 0;
}
// interrupt handler for reading pwm value
int AP_RSSI::irq_handler(int irq, void *context)
void AP_RSSI::irq_handler(uint8_t pin, bool pin_high, uint32_t timestamp_us)
{
#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);
// calculate pwm value
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN || CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
if (pin_high) {
pwm_state.pulse_start_us = now;
pwm_state.pulse_start_us = timestamp_us;
} else {
if (pwm_state.pulse_start_us != 0) {
pwm_state.value = now - pwm_state.pulse_start_us;
}
pwm_state.irq_value_us = timestamp_us - pwm_state.pulse_start_us;
pwm_state.pulse_start_us = 0;
pwm_state.last_reading_ms = AP_HAL::millis();
}
}
#endif
return 0;
}
AP_RSSI *AP_RSSI::_s_instance = nullptr;

19
libraries/AP_RSSI/AP_RSSI.h
View File

@ -75,17 +75,21 @@ private:
AP_HAL::AnalogSource *rssi_analog_source;
// PWM input
static struct PWMState {
uint32_t gpio; // gpio pin used for reading pwm
uint32_t last_gpio; // last gpio pin used for reading pwm (used to recognise change in pin assignment)
uint32_t value; // last calculated pwm value
struct PWMState {
int8_t last_rssi_analog_pin; // last pin used for reading pwm (used to recognise change in pin assignment)
uint32_t last_reading_ms; // system time of last read (used for health reporting)
uint64_t pulse_start_us; // system time of start of pulse
float rssi_value; // last calculated RSSI value
// the following two members are updated by the interrupt handler
uint32_t irq_value_us; // last calculated pwm value (irq copy)
uint32_t pulse_start_us; // system time of start of pulse
} pwm_state;
// read the RSSI value from an analog pin - returns float in range 0.0 to 1.0
float read_pin_rssi();
// check if pin has changed and configure interrupt handlers if required
void check_pwm_pin_rssi();
// read the RSSI value from a PWM value on a RC channel
float read_channel_rssi();
@ -96,8 +100,9 @@ private:
float scale_and_constrain_float_rssi(float current_rssi_value, float low_rssi_range, float high_rssi_range);
// PWM input handling
uint32_t get_gpio(uint8_t pin_number) const;
static int irq_handler(int irq, void *context);
void irq_handler(uint8_t pin,
bool pin_state,
uint32_t timestamp);
};
namespace AP {