From c2adc26051ae64b5a8a67bc1edaf8b9482f4d55c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 26 Mar 2018 17:19:13 +1100
Subject: [PATCH] AP_RPM: support RPM pin input on ChibiOS

---
 libraries/AP_RPM/AP_RPM.cpp  | 23 +++++++++++++++--------
 libraries/AP_RPM/RPM_Pin.cpp | 34 +++++++++++++++++++++++++++++-----
 libraries/AP_RPM/RPM_Pin.h   |  8 +++++++-
 3 files changed, 51 insertions(+), 14 deletions(-)

diff --git a/libraries/AP_RPM/AP_RPM.cpp b/libraries/AP_RPM/AP_RPM.cpp
index d63652e970..fb53426a81 100644
--- a/libraries/AP_RPM/AP_RPM.cpp
+++ b/libraries/AP_RPM/AP_RPM.cpp
@@ -111,17 +111,24 @@ void AP_RPM::init(void)
     }
     for (uint8_t i=0; i<RPM_MAX_INSTANCES; i++) {
 #if (CONFIG_HAL_BOARD == HAL_BOARD_PX4) || ((CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN) && (!defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) && !defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52)))
-        uint8_t type = _type[num_instances];
-        uint8_t instance = num_instances;
+        {
+            uint8_t type = _type[num_instances];
+            uint8_t instance = num_instances;
 
-        if (type == RPM_TYPE_PX4_PWM) {
-            state[instance].instance = instance;
-            drivers[instance] = new AP_RPM_PX4_PWM(*this, instance, state[instance]);
-        } else if (type == RPM_TYPE_PIN) {
-            state[instance].instance = instance;
-            drivers[instance] = new AP_RPM_Pin(*this, instance, state[instance]);
+            if (type == RPM_TYPE_PX4_PWM) {
+                state[instance].instance = instance;
+                drivers[instance] = new AP_RPM_PX4_PWM(*this, instance, state[instance]);
+            }
         }
 #endif
+        {
+            uint8_t type = _type[num_instances];
+            uint8_t instance = num_instances;
+            if (type == RPM_TYPE_PIN) {
+                state[instance].instance = instance;
+                drivers[instance] = new AP_RPM_Pin(*this, instance, state[instance]);
+            }
+        }
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
         uint8_t instance = num_instances;
         state[instance].instance = instance;
diff --git a/libraries/AP_RPM/RPM_Pin.cpp b/libraries/AP_RPM/RPM_Pin.cpp
index 9e975a1b63..27c3f89b67 100644
--- a/libraries/AP_RPM/RPM_Pin.cpp
+++ b/libraries/AP_RPM/RPM_Pin.cpp
@@ -14,11 +14,13 @@
  */
 
 #include <AP_HAL/AP_HAL.h>
+#include "RPM_Pin.h"
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <board_config.h>
-#include "RPM_Pin.h"
+#endif
+
 #include <stdio.h>
 
 extern const AP_HAL::HAL& hal;
@@ -49,6 +51,7 @@ void AP_RPM_Pin::irq_handler(uint8_t instance)
     }
 }
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
 /*
   interrupt handler for instance 0
  */
@@ -66,11 +69,30 @@ int AP_RPM_Pin::irq_handler1(int irq, void *context)
     irq_handler(1);
     return 0;
 }
+#else // other HALs
+/*
+  interrupt handler for instance 0
+ */
+void AP_RPM_Pin::irq_handler0(void)
+{
+    irq_handler(0);
+}
+
+/*
+  interrupt handler for instance 1
+ */
+void AP_RPM_Pin::irq_handler1(void)
+{
+    irq_handler(1);
+}
+#endif
 
 void AP_RPM_Pin::update(void)
 {
     if (last_pin != get_pin()) {
         last_pin = get_pin();
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
         uint32_t gpio = 0;
 
 #ifdef GPIO_GPIO0_INPUT
@@ -113,17 +135,21 @@ void AP_RPM_Pin::update(void)
         // for either polarity of pulse, as all we want is the period
         stm32_gpiosetevent(gpio, true, false, false,
                            state.instance==0?irq_handler0:irq_handler1);
+#else // other HALs
+        hal.gpio->attach_interrupt(last_pin, state.instance==0?irq_handler0:irq_handler1,
+                                   HAL_GPIO_INTERRUPT_RISING);
+#endif
     }
 
     if (irq_state[state.instance].dt_count > 0) {
         float dt_avg;
 
         // disable interrupts to prevent race with irq_handler
-        irqstate_t istate = irqsave();
+        void *irqstate = hal.scheduler->disable_interrupts_save();
         dt_avg = irq_state[state.instance].dt_sum / irq_state[state.instance].dt_count;
         irq_state[state.instance].dt_count = 0;
         irq_state[state.instance].dt_sum = 0;
-        irqrestore(istate);
+        hal.scheduler->restore_interrupts(irqstate);
 
         const float scaling = ap_rpm._scaling[state.instance];
         float maximum = ap_rpm._maximum[state.instance];
@@ -154,5 +180,3 @@ void AP_RPM_Pin::update(void)
         state.rate_rpm = 0;
     }
 }
-
-#endif // CONFIG_HAL_BOARD
diff --git a/libraries/AP_RPM/RPM_Pin.h b/libraries/AP_RPM/RPM_Pin.h
index 6ffd3e8a20..edf386359c 100644
--- a/libraries/AP_RPM/RPM_Pin.h
+++ b/libraries/AP_RPM/RPM_Pin.h
@@ -15,6 +15,7 @@
 #pragma once
 
 #include "AP_RPM.h"
+
 #include "RPM_Backend.h"
 #include <Filter/Filter.h>
 #include <AP_Math/AP_Math.h>
@@ -30,9 +31,14 @@ public:
 
 private:
     static void irq_handler(uint8_t instance);
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
     static int irq_handler0(int irq, void *context);
     static int irq_handler1(int irq, void *context);
-
+#else
+    static void irq_handler0(void);
+    static void irq_handler1(void);
+#endif
+    
     ModeFilterFloat_Size5 signal_quality_filter {3};
     uint8_t last_pin = -1;
     uint32_t last_gpio;