AP_RPM: support RPM input on any AUX pin

libraries/AP_RPM/AP_RPM.cpp

@@ -15,6 +15,7 @@
 
 #include "AP_RPM.h"
 #include "RPM_PX4_PWM.h"
+#include "RPM_Pin.h"
 #include "RPM_SITL.h"
 
 extern const AP_HAL::HAL& hal;
@@ -24,7 +25,7 @@ const AP_Param::GroupInfo AP_RPM::var_info[] = {
     // @Param: _TYPE
     // @DisplayName: RPM type
     // @Description: What type of RPM sensor is connected
-    // @Values: 0:None,1:PX4-PWM
+    // @Values: 0:None,1:PX4-PWM,2:AUXPIN
     // @User: Standard
     AP_GROUPINFO("_TYPE",    0, AP_RPM, _type[0], 0),
 
@@ -56,11 +57,18 @@ const AP_Param::GroupInfo AP_RPM::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("_MIN_QUAL", 4, AP_RPM, _quality_min[0], 0.5),
 
+    // @Param: _PIN
+    // @DisplayName: Input pin number
+    // @Description: Which pin to use
+    // @Values: -1:Disabled,50:PixhawkAUX1,51:PixhawkAUX2,52:PixhawkAUX3,53:PixhawkAUX4,54:PixhawkAUX5,55:PixhawkAUX6
+    // @User: Standard
+    AP_GROUPINFO("_PIN",    5, AP_RPM, _pin[0], 54),
+    
 #if RPM_MAX_INSTANCES > 1
     // @Param: 2_TYPE
     // @DisplayName: Second RPM type
     // @Description: What type of RPM sensor is connected
-    // @Values: 0:None,1:PX4-PWM
+    // @Values: 0:None,1:PX4-PWM,2:AUXPIN
     // @User: Advanced
     AP_GROUPINFO("2_TYPE",    10, AP_RPM, _type[1], 0),
 
@@ -72,6 +80,13 @@ const AP_Param::GroupInfo AP_RPM::var_info[] = {
     AP_GROUPINFO("2_SCALING", 11, AP_RPM, _scaling[1], 1.0f),
 #endif
 
+    // @Param: 2_PIN
+    // @DisplayName: RPM2 input pin number
+    // @Description: Which pin to use
+    // @Values: -1:Disabled,50:PixhawkAUX1,51:PixhawkAUX2,52:PixhawkAUX3,53:PixhawkAUX4,54:PixhawkAUX5,55:PixhawkAUX6
+    // @User: Standard
+    AP_GROUPINFO("2_PIN",    12, AP_RPM, _pin[1], -1),
+    
     AP_GROUPEND
 };
 
@@ -102,6 +117,9 @@ void AP_RPM::init(void)
         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]);
         }
 #endif
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL

libraries/AP_RPM/AP_RPM.h

@@ -34,7 +34,8 @@ public:
     // RPM driver types
     enum RPM_Type {
         RPM_TYPE_NONE    = 0,
-        RPM_TYPE_PX4_PWM = 1
+        RPM_TYPE_PX4_PWM = 1,
+        RPM_TYPE_PIN     = 2
     };
 
     // The RPM_State structure is filled in by the backend driver
@@ -47,6 +48,7 @@ public:
 
     // parameters for each instance
     AP_Int8  _type[RPM_MAX_INSTANCES];
+    AP_Int8  _pin[RPM_MAX_INSTANCES];
     AP_Float _scaling[RPM_MAX_INSTANCES];
     AP_Float _maximum[RPM_MAX_INSTANCES];
     AP_Float _minimum[RPM_MAX_INSTANCES];

libraries/AP_RPM/RPM_Backend.h

@@ -31,6 +31,13 @@ public:
     // update the state structure. All backends must implement this.
     virtual void update() = 0;
 
+    int8_t get_pin(void) const {
+        if (state.instance > 1) {
+            return -1;
+        }
+        return ap_rpm._pin[state.instance].get();
+    }
+    
 protected:
 
     AP_RPM &ap_rpm;

libraries/AP_RPM/RPM_Pin.cpp

@@ -0,0 +1,155 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_HAL/AP_HAL.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"
+#include <stdio.h>
+
+extern const AP_HAL::HAL& hal;
+AP_RPM_Pin::IrqState AP_RPM_Pin::irq_state[RPM_MAX_INSTANCES];
+
+/* 
+   open the sensor in constructor
+*/
+AP_RPM_Pin::AP_RPM_Pin(AP_RPM &_ap_rpm, uint8_t instance, AP_RPM::RPM_State &_state) :
+	AP_RPM_Backend(_ap_rpm, instance, _state)
+{
+}
+
+/*
+  handle interrupt on an instance
+ */
+void AP_RPM_Pin::irq_handler(uint8_t instance)
+{
+    uint32_t now = AP_HAL::micros();
+    uint32_t dt = now - irq_state[instance].last_pulse_us;
+    irq_state[instance].last_pulse_us = now;
+    // we don't accept pulses less than 100us. Using an irq for such
+    // high RPM is too inaccurate, and it is probably just bounce of
+    // the signal which we should ignore
+    if (dt > 100 && dt < 1000*1000) {
+        irq_state[instance].dt_sum += dt;
+        irq_state[instance].dt_count++;
+    }
+}
+
+/*
+  interrupt handler for instance 0
+ */
+int AP_RPM_Pin::irq_handler0(int irq, void *context)
+{
+    irq_handler(0);
+    return 0;
+}
+
+/*
+  interrupt handler for instance 1
+ */
+int AP_RPM_Pin::irq_handler1(int irq, void *context)
+{
+    irq_handler(1);
+    return 0;
+}
+
+void AP_RPM_Pin::update(void)
+{
+    if (last_pin != get_pin()) {
+        last_pin = get_pin();
+        uint32_t gpio = 0;
+        switch (last_pin) {
+        case 50:
+            gpio = GPIO_GPIO0_INPUT;
+            break;
+        case 51:
+            gpio = GPIO_GPIO1_INPUT;
+            break;
+        case 52:
+            gpio = GPIO_GPIO2_INPUT;
+            break;
+        case 53:
+            gpio = GPIO_GPIO3_INPUT;
+            break;
+        case 54:
+            gpio = GPIO_GPIO4_INPUT;
+            break;
+        case 55:
+            gpio = GPIO_GPIO5_INPUT;
+            break;
+        }
+
+        // uninstall old handler if installed
+        if (last_gpio != 0) {
+            stm32_gpiosetevent(last_gpio, false, false, false, nullptr);
+        }
+        irq_state[state.instance].dt_count = 0;
+        irq_state[state.instance].dt_sum = 0;
+
+        last_gpio = gpio;
+
+        if (gpio == 0) {
+            return;
+        }
+        
+        // install interrupt handler on rising edge of pin. This works
+        // 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);
+    }
+
+    if (irq_state[state.instance].dt_count > 0) {
+        float dt_avg;
+
+        // disable interrupts to prevent race with irq_handler
+        irqstate_t istate = irqsave();
+        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);
+
+        const float scaling = ap_rpm._scaling[state.instance];
+        float maximum = ap_rpm._maximum[state.instance];
+        float minimum = ap_rpm._minimum[state.instance];
+        float quality = 0;
+        float rpm = scaling * (1.0e6 / dt_avg) * 60;
+        float filter_value = signal_quality_filter.get();
+
+        state.rate_rpm = signal_quality_filter.apply(rpm);
+        
+        if ((maximum <= 0 || rpm <= maximum) && (rpm >= minimum)) {
+            if (is_zero(filter_value)){
+                quality = 0;
+            } else {
+                quality = 1 - constrain_float((fabsf(rpm-filter_value))/filter_value, 0.0, 1.0);
+                quality = powf(quality, 2.0);
+            }
+            state.last_reading_ms = AP_HAL::millis();
+        } else {
+            quality = 0;
+        }
+        state.signal_quality = (0.1 * quality) + (0.9 * state.signal_quality);
+    }
+    
+    // assume we get readings at at least 1Hz, otherwise reset quality to zero
+    if (AP_HAL::millis() - state.last_reading_ms > 1000) {
+        state.signal_quality = 0;
+        state.rate_rpm = 0;
+    }
+}
+
+#endif // CONFIG_HAL_BOARD

libraries/AP_RPM/RPM_Pin.h

@@ -0,0 +1,45 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include "AP_RPM.h"
+#include "RPM_Backend.h"
+#include <Filter/Filter.h>
+#include <AP_Math/AP_Math.h>
+
+class AP_RPM_Pin : public AP_RPM_Backend
+{
+public:
+    // constructor
+    AP_RPM_Pin(AP_RPM &ranger, uint8_t instance, AP_RPM::RPM_State &_state);
+
+    // update state
+    void update(void);
+
+private:
+    static void irq_handler(uint8_t instance);
+    static int irq_handler0(int irq, void *context);
+    static int irq_handler1(int irq, void *context);
+
+    ModeFilterFloat_Size5 signal_quality_filter {3};
+    uint8_t last_pin = -1;
+    uint32_t last_gpio;
+    struct IrqState {
+        uint32_t last_pulse_us;
+        uint32_t dt_sum;
+        uint32_t dt_count;
+    };
+    static struct IrqState irq_state[RPM_MAX_INSTANCES];
+};