From 9e2cfabb8a3c0e936bd0b01c7622d5993c495e91 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 2 Apr 2017 13:57:59 +1000 Subject: [PATCH] AP_RPM: support RPM input on any AUX pin --- libraries/AP_RPM/AP_RPM.cpp | 22 ++++- libraries/AP_RPM/AP_RPM.h | 4 +- libraries/AP_RPM/RPM_Backend.h | 7 ++ libraries/AP_RPM/RPM_Pin.cpp | 155 +++++++++++++++++++++++++++++++++ libraries/AP_RPM/RPM_Pin.h | 45 ++++++++++ 5 files changed, 230 insertions(+), 3 deletions(-) create mode 100644 libraries/AP_RPM/RPM_Pin.cpp create mode 100644 libraries/AP_RPM/RPM_Pin.h 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]; +};