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

---
 libraries/AP_HAL/AP_HAL_Namespace.h |   1 +
 libraries/AP_HAL/GPIO.cpp           | 108 ++++++++++++++++++++++++++++
 libraries/AP_HAL/GPIO.h             |  25 +++++++
 3 files changed, 134 insertions(+)
 create mode 100644 libraries/AP_HAL/GPIO.cpp

diff --git a/libraries/AP_HAL/AP_HAL_Namespace.h b/libraries/AP_HAL/AP_HAL_Namespace.h
index 5ec50d621b..cce205c90f 100644
--- a/libraries/AP_HAL/AP_HAL_Namespace.h
+++ b/libraries/AP_HAL/AP_HAL_Namespace.h
@@ -22,6 +22,7 @@ namespace AP_HAL {
     class AnalogIn;
     class Storage;
     class DigitalSource;
+    class PWMSource;
     class GPIO;
     class RCInput;
     class RCOutput;
diff --git a/libraries/AP_HAL/GPIO.cpp b/libraries/AP_HAL/GPIO.cpp
new file mode 100644
index 0000000000..603b16b58c
--- /dev/null
+++ b/libraries/AP_HAL/GPIO.cpp
@@ -0,0 +1,108 @@
+#include "GPIO.h"
+
+#include <AP_HAL/AP_HAL.h>
+#if defined(HAL_NO_GCS) || defined(HAL_BOOTLOADER_BUILD)
+#define GCS_SEND_TEXT(severity, format, args...)
+#else
+#include <GCS_MAVLink/GCS.h>
+#endif
+
+extern const AP_HAL::HAL& hal;
+
+bool AP_HAL::PWMSource::set_pin(int16_t new_pin, const char *subsystem)
+{
+    if (new_pin == _pin) {
+        // we've already tried to attach to this pin
+        return interrupt_attached;
+    }
+
+    if (interrupt_attached) {
+        if (!hal.gpio->detach_interrupt(_pin)) {
+            GCS_SEND_TEXT(MAV_SEVERITY_WARNING,
+                          "%s: Failed to detach interrupt from %d",
+                          subsystem,
+                          _pin);
+        }
+        interrupt_attached = false;
+    }
+
+    // don't want to try to attach more than once:
+    _pin = new_pin;
+
+    if (_pin <= 0) {
+        // invalid pin
+        return false;
+    }
+
+    // install interrupt handler on rising and falling edge
+    hal.gpio->pinMode(_pin, HAL_GPIO_INPUT);
+    if (!hal.gpio->attach_interrupt(
+            _pin,
+            FUNCTOR_BIND_MEMBER(&AP_HAL::PWMSource::irq_handler,
+                                void,
+                                uint8_t,
+                                bool,
+                                uint32_t),
+            AP_HAL::GPIO::INTERRUPT_BOTH)) {
+        // failed to attach interrupt
+        GCS_SEND_TEXT(MAV_SEVERITY_WARNING,
+                      "%s: Failed to attach interrupt to %d",
+                      subsystem,
+                      _pin);
+        return false;
+    }
+
+    interrupt_attached = true;
+
+    return interrupt_attached;
+}
+
+// interrupt handler for reading pwm value
+void AP_HAL::PWMSource::irq_handler(uint8_t a_pin, bool pin_high, uint32_t timestamp_us)
+{
+    if (pin_high) {
+        _pulse_start_us = timestamp_us;
+        return;
+    }
+    if (_pulse_start_us == 0) {
+        // if we miss interrupts we could get two lows in a row.  If
+        // we miss interrupts we're definitely handing back bad
+        // values anyway....
+        return;
+    }
+    _irq_value_us = timestamp_us - _pulse_start_us;
+    _pulse_start_us = 0;
+
+    // update fields for taking an average reading:
+    _irq_value_us_sum += _irq_value_us;
+    _irq_value_us_count++;
+}
+
+
+uint16_t AP_HAL::PWMSource::get_pwm_us()
+{
+    // disable interrupts and grab state
+    void *irqstate = hal.scheduler->disable_interrupts_save();
+    const uint32_t ret = _irq_value_us;
+    _irq_value_us = 0;
+    hal.scheduler->restore_interrupts(irqstate);
+
+    return ret;
+}
+
+uint16_t AP_HAL::PWMSource::get_pwm_avg_us()
+{
+    // disable interrupts and grab state
+    void *irqstate = hal.scheduler->disable_interrupts_save();
+    uint32_t ret;
+    if (_irq_value_us_count == 0) {
+        ret = 0;
+    } else {
+        ret = _irq_value_us_sum / _irq_value_us_count;
+    }
+    _irq_value_us_sum = 0;
+    _irq_value_us_count = 0;
+    hal.scheduler->restore_interrupts(irqstate);
+
+    return ret;
+}
diff --git a/libraries/AP_HAL/GPIO.h b/libraries/AP_HAL/GPIO.h
index 742076d7ed..03646cd131 100644
--- a/libraries/AP_HAL/GPIO.h
+++ b/libraries/AP_HAL/GPIO.h
@@ -16,6 +16,31 @@ public:
     virtual void    toggle() = 0;
 };
 
+class AP_HAL::PWMSource {
+public:
+
+    bool set_pin(int16_t new_pin, const char *subsystem);
+    int16_t pin() const { return _pin; }  // returns pin this is attached to
+
+    uint16_t get_pwm_us();            // return last measured PWM input
+    uint16_t get_pwm_avg_us();        // return average PWM since last call to get_pwm_avg_us
+
+private:
+    uint16_t _irq_value_us;         // last calculated pwm value (irq copy)
+    uint32_t _pulse_start_us;       // system time of start of pulse
+    int16_t _pin = -1;
+
+    uint32_t _irq_value_us_sum;     // for get_pwm_avg_us
+    uint32_t _irq_value_us_count;   // for get_pwm_avg_us
+
+    bool interrupt_attached;
+
+    // PWM input handling
+    void irq_handler(uint8_t pin,
+                     bool pin_state,
+                     uint32_t timestamp);
+};
+
 class AP_HAL::GPIO {
 public:
     GPIO() {}