From abfd995fff43e9522aa766dbdd0e16816d0a7fbf Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 1 Sep 2020 15:45:15 +1000
Subject: [PATCH] AP_RSSI: create and use new AP_HAL::PWMSource object

---
 libraries/AP_RSSI/AP_RSSI.cpp | 68 +++--------------------------------
 libraries/AP_RSSI/AP_RSSI.h   | 13 ++-----
 2 files changed, 6 insertions(+), 75 deletions(-)

diff --git a/libraries/AP_RSSI/AP_RSSI.cpp b/libraries/AP_RSSI/AP_RSSI.cpp
index 170c830281..878527366c 100644
--- a/libraries/AP_RSSI/AP_RSSI.cpp
+++ b/libraries/AP_RSSI/AP_RSSI.cpp
@@ -185,66 +185,19 @@ 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
-    hal.gpio->pinMode(rssi_analog_pin, HAL_GPIO_INPUT);
-    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",
-                        (unsigned int)rssi_analog_pin);
-        return;
-    }
-}
-
 // read the PWM value from a pin
 float AP_RSSI::read_pwm_pin_rssi()
 {
     // check if pin has changed and configure interrupt handlers if required:
-    check_pwm_pin_rssi();
-
-    if (!pwm_state.last_rssi_analog_pin) {
+    if (!pwm_state.pwm_source.set_pin(rssi_analog_pin, "RSSI")) {
         // disabled (either by configuration or failure to attach interrupt)
         return 0.0f;
     }
 
-    // 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);
+    uint16_t pwm_us = pwm_state.pwm_source.get_pwm_us();
 
     const uint32_t now = AP_HAL::millis();
-    if (irq_value_us == 0) {
+    if (pwm_us == 0) {
         // no reading; check for timeout:
         if (now - pwm_state.last_reading_ms > 1000) {
             // no reading for a second - something is broken
@@ -252,7 +205,7 @@ float AP_RSSI::read_pwm_pin_rssi()
         }
     } else {
         // 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.rssi_value = scale_and_constrain_float_rssi(pwm_us, rssi_channel_low_pwm_value, rssi_channel_high_pwm_value);
         pwm_state.last_reading_ms = now;
     }
 
@@ -294,19 +247,6 @@ float AP_RSSI::scale_and_constrain_float_rssi(float current_rssi_value, float lo
     return constrain_float(rssi_value_scaled, 0.0f, 1.0f);
 }
 
-// interrupt handler for reading pwm value
-void AP_RSSI::irq_handler(uint8_t pin, bool pin_high, uint32_t timestamp_us)
-{
-    if (pin_high) {
-        pwm_state.pulse_start_us = timestamp_us;
-    } else {
-        if (pwm_state.pulse_start_us != 0) {
-            pwm_state.irq_value_us = timestamp_us - pwm_state.pulse_start_us;
-            pwm_state.pulse_start_us = 0;
-        }
-    }
-}
-
 AP_RSSI *AP_RSSI::_singleton = nullptr;
 
 namespace AP {
diff --git a/libraries/AP_RSSI/AP_RSSI.h b/libraries/AP_RSSI/AP_RSSI.h
index c46d4d366c..1404ca5112 100644
--- a/libraries/AP_RSSI/AP_RSSI.h
+++ b/libraries/AP_RSSI/AP_RSSI.h
@@ -77,20 +77,16 @@ private:
 
     // PWM input
     struct PWMState {
-        int8_t last_rssi_analog_pin; // last pin used for reading pwm (used to recognise change in pin assignment)
+        int8_t last_warn_pin; // last pin used for reading pwm (used to recognise change failure 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
+        AP_HAL::PWMSource pwm_source;
     } 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();
 
@@ -102,11 +98,6 @@ private:
 
     // Scale and constrain a float rssi value to 0.0 to 1.0 range
     float scale_and_constrain_float_rssi(float current_rssi_value, float low_rssi_range, float high_rssi_range);
-
-    // PWM input handling
-    void irq_handler(uint8_t pin,
-                     bool pin_state,
-                     uint32_t timestamp);
 };
 
 namespace AP {