From d9f393f42eb2963d0c3e73b12c3420956555524f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 13 Aug 2018 10:53:03 +1000 Subject: [PATCH] AP_RSSI: convert to HAL attach_interrupt AP_RSSI: add error reporting for attaching of interrupts AP_RSSI: use detach_interrupt method --- libraries/AP_RSSI/AP_RSSI.cpp | 141 +++++++++++++++++----------------- libraries/AP_RSSI/AP_RSSI.h | 21 +++-- 2 files changed, 82 insertions(+), 80 deletions(-) diff --git a/libraries/AP_RSSI/AP_RSSI.cpp b/libraries/AP_RSSI/AP_RSSI.cpp index 5b8941f38c..16a0f21bac 100644 --- a/libraries/AP_RSSI/AP_RSSI.cpp +++ b/libraries/AP_RSSI/AP_RSSI.cpp @@ -15,7 +15,9 @@ #include #include +#include #include + #include #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #include @@ -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; - } - - // 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; - } + if (!pwm_state.last_rssi_analog_pin) { + // disabled (either by configuration or failure to attach interrupt) + return 0.0f; } - // disable interrupts temporarily - irqstate_t istate = irqsave(); + // 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); - // 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.pulse_start_us = 0; - pwm_state.last_reading_ms = AP_HAL::millis(); } #endif - - return 0; } AP_RSSI *AP_RSSI::_s_instance = nullptr; diff --git a/libraries/AP_RSSI/AP_RSSI.h b/libraries/AP_RSSI/AP_RSSI.h index d53a1c3f2d..dd501f857d 100644 --- a/libraries/AP_RSSI/AP_RSSI.h +++ b/libraries/AP_RSSI/AP_RSSI.h @@ -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 - 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 + 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) + 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 {