diff --git a/libraries/AP_RPM/AP_RPM.cpp b/libraries/AP_RPM/AP_RPM.cpp
index aca574a392..8bd1bf605b 100644
--- a/libraries/AP_RPM/AP_RPM.cpp
+++ b/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
diff --git a/libraries/AP_RPM/AP_RPM.h b/libraries/AP_RPM/AP_RPM.h
index d19ab38045..59158942d9 100644
--- a/libraries/AP_RPM/AP_RPM.h
+++ b/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];
diff --git a/libraries/AP_RPM/RPM_Backend.h b/libraries/AP_RPM/RPM_Backend.h
index 11df1c1e21..d267b38b46 100644
--- a/libraries/AP_RPM/RPM_Backend.h
+++ b/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;
diff --git a/libraries/AP_RPM/RPM_Pin.cpp b/libraries/AP_RPM/RPM_Pin.cpp
new file mode 100644
index 0000000000..520a5fc038
--- /dev/null
+++ b/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 .
+ */
+
+#include
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#include
+#include
+#include "RPM_Pin.h"
+#include
+
+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
diff --git a/libraries/AP_RPM/RPM_Pin.h b/libraries/AP_RPM/RPM_Pin.h
new file mode 100644
index 0000000000..6ffd3e8a20
--- /dev/null
+++ b/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 .
+ */
+#pragma once
+
+#include "AP_RPM.h"
+#include "RPM_Backend.h"
+#include
+#include
+
+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];
+};