diff --git a/libraries/AP_RPM/AP_RPM.cpp b/libraries/AP_RPM/AP_RPM.cpp
new file mode 100644
index 0000000000..3d17826c7f
--- /dev/null
+++ b/libraries/AP_RPM/AP_RPM.cpp
@@ -0,0 +1,119 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ 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 "AP_RPM.h"
+#include "RPM_PX4_PWM.h"
+
+extern const AP_HAL::HAL& hal;
+
+// table of user settable parameters
+const AP_Param::GroupInfo AP_RPM::var_info[] PROGMEM = {
+ // @Param: _TYPE
+ // @DisplayName: RPM type
+ // @Description: What type of RPM sensor is connected
+ // @Values: 0:None,1:PX4-PWM
+ AP_GROUPINFO("_TYPE", 0, AP_RPM, _type[0], 0),
+
+ // @Param: _SCALING
+ // @DisplayName: RPM scaling
+ // @Description: Scaling factor between sensor reading and RPM.
+ // @Increment: 0.001
+ AP_GROUPINFO("_SCALING", 1, AP_RPM, _scaling[0], 1.0f),
+
+#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
+ AP_GROUPINFO("2_TYPE", 10, AP_RPM, _type[1], 0),
+
+ // @Param: 2_SCALING
+ // @DisplayName: RPM scaling
+ // @Description: Scaling factor between sensor reading and RPM.
+ // @Increment: 0.001
+ AP_GROUPINFO("2_SCALING", 11, AP_RPM, _scaling[1], 1.0f),
+#endif
+
+ AP_GROUPEND
+};
+
+AP_RPM::AP_RPM(void) :
+ num_instances(0)
+{
+ AP_Param::setup_object_defaults(this, var_info);
+
+ // init state and drivers
+ memset(state,0,sizeof(state));
+ memset(drivers,0,sizeof(drivers));
+}
+
+/*
+ initialise the AP_RPM class.
+ */
+void AP_RPM::init(void)
+{
+ if (num_instances != 0) {
+ // init called a 2nd time?
+ return;
+ }
+ for (uint8_t i=0; iupdate();
+ }
+ }
+}
+
+/*
+ check if an instance is healthy
+ */
+bool AP_RPM::healthy(uint8_t instance) const
+{
+ if (instance >= num_instances) {
+ return false;
+ }
+ // assume we get readings at at least 1Hz
+ if (hal.scheduler->millis() - state[instance].last_reading_ms > 1000) {
+ return false;
+ }
+ return true;
+}
diff --git a/libraries/AP_RPM/AP_RPM.h b/libraries/AP_RPM/AP_RPM.h
new file mode 100644
index 0000000000..cbfebb7ec9
--- /dev/null
+++ b/libraries/AP_RPM/AP_RPM.h
@@ -0,0 +1,87 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ 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 .
+ */
+
+#ifndef __RPM_H__
+#define __RPM_H__
+
+#include
+#include
+#include
+#include
+
+// Maximum number of RPM measurement instances available on this platform
+#define RPM_MAX_INSTANCES 2
+
+class AP_RPM_Backend;
+
+class AP_RPM
+{
+public:
+ friend class AP_RPM_Backend;
+
+ AP_RPM(void);
+
+ // RPM driver types
+ enum RPM_Type {
+ RPM_TYPE_NONE = 0,
+ RPM_TYPE_PX4_PWM = 1
+ };
+
+ // The RPM_State structure is filled in by the backend driver
+ struct RPM_State {
+ uint8_t instance; // the instance number of this RPM
+ float rate_rpm; // measured rate in revs per minute
+ uint32_t last_reading_ms; // time of last reading
+ };
+
+ // parameters for each instance
+ AP_Int8 _type[RPM_MAX_INSTANCES];
+ AP_Float _scaling[RPM_MAX_INSTANCES];
+
+ static const struct AP_Param::GroupInfo var_info[];
+
+ // Return the number of rpm sensor instances
+ uint8_t num_sensors(void) const {
+ return num_instances;
+ }
+
+ // detect and initialise any available rpm sensors
+ void init(void);
+
+ // update state of all rpm sensors. Should be called from main loop
+ void update(void);
+
+ /*
+ return RPM for a sensor. Return -1 if not healthy
+ */
+ float get_rpm(uint8_t instance) const {
+ if (!healthy(instance)) {
+ return -1;
+ }
+ return state[instance].rate_rpm;
+ }
+
+ bool healthy(uint8_t instance) const;
+
+private:
+ RPM_State state[RPM_MAX_INSTANCES];
+ AP_RPM_Backend *drivers[RPM_MAX_INSTANCES];
+ uint8_t num_instances:2;
+
+ void detect_instance(uint8_t instance);
+ void update_instance(uint8_t instance);
+};
+#endif // __RPM_H__
diff --git a/libraries/AP_RPM/RPM_Backend.cpp b/libraries/AP_RPM/RPM_Backend.cpp
new file mode 100644
index 0000000000..cec72b74cf
--- /dev/null
+++ b/libraries/AP_RPM/RPM_Backend.cpp
@@ -0,0 +1,29 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ 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
+#include
+#include "AP_RPM.h"
+#include "RPM_Backend.h"
+
+/*
+ base class constructor.
+*/
+AP_RPM_Backend::AP_RPM_Backend(AP_RPM &_ap_rpm, uint8_t instance, AP_RPM::RPM_State &_state) :
+ ap_rpm(_ap_rpm),
+ state(_state)
+{
+}
diff --git a/libraries/AP_RPM/RPM_Backend.h b/libraries/AP_RPM/RPM_Backend.h
new file mode 100644
index 0000000000..57241bfa44
--- /dev/null
+++ b/libraries/AP_RPM/RPM_Backend.h
@@ -0,0 +1,42 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ 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 .
+ */
+
+#ifndef __AP_RPM_BACKEND_H__
+#define __AP_RPM_BACKEND_H__
+
+#include
+#include
+#include "AP_RPM.h"
+
+class AP_RPM_Backend
+{
+public:
+ // constructor. This incorporates initialisation as well.
+ AP_RPM_Backend(AP_RPM &_ap_rpm, uint8_t instance, AP_RPM::RPM_State &_state);
+
+ // we declare a virtual destructor so that RPM drivers can
+ // override with a custom destructor if need be
+ virtual ~AP_RPM_Backend(void) {}
+
+ // update the state structure. All backends must implement this.
+ virtual void update() = 0;
+
+protected:
+
+ AP_RPM &ap_rpm;
+ AP_RPM::RPM_State &state;
+};
+#endif // __AP_RPM_BACKEND_H__
diff --git a/libraries/AP_RPM/RPM_PX4_PWM.cpp b/libraries/AP_RPM/RPM_PX4_PWM.cpp
new file mode 100644
index 0000000000..4ee145eef8
--- /dev/null
+++ b/libraries/AP_RPM/RPM_PX4_PWM.cpp
@@ -0,0 +1,94 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ 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 "RPM_PX4_PWM.h"
+
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+extern const AP_HAL::HAL& hal;
+
+/*
+ open the sensor in constructor
+*/
+AP_RPM_PX4_PWM::AP_RPM_PX4_PWM(AP_RPM &_ap_rpm, uint8_t instance, AP_RPM::RPM_State &_state) :
+ AP_RPM_Backend(_ap_rpm, instance, _state)
+{
+ _fd = open(PWMIN0_DEVICE_PATH, O_RDONLY);
+ if (_fd == -1) {
+ hal.console->printf("Unable to open %s\n", PWMIN0_DEVICE_PATH);
+ return;
+ }
+
+ // keep a queue of 5 samples to reduce noise by averaging
+ if (ioctl(_fd, SENSORIOCSQUEUEDEPTH, 5) != 0) {
+ hal.console->printf("Failed to setup RPM queue\n");
+ close(_fd);
+ _fd = -1;
+ return;
+ }
+}
+
+/*
+ close the file descriptor
+*/
+AP_RPM_PX4_PWM::~AP_RPM_PX4_PWM()
+{
+ if (_fd != -1) {
+ close(_fd);
+ _fd = -1;
+ }
+}
+
+void AP_RPM_PX4_PWM::update(void)
+{
+ if (_fd == -1) {
+ return;
+ }
+
+ struct pwm_input_s pwm;
+ uint16_t count = 0;
+ const float scaling = ap_rpm._scaling[state.instance];
+ float sum = 0;
+
+ while (::read(_fd, &pwm, sizeof(pwm)) == sizeof(pwm)) {
+ // the px4 pwm_input driver reports the period in microseconds
+ if (pwm.period > 0) {
+ sum += (1.0e6f * 60) / pwm.period;
+ count++;
+ }
+ }
+
+ if (count != 0) {
+ state.rate_rpm = scaling * sum / count;
+ state.last_reading_ms = hal.scheduler->millis();
+ }
+}
+
+#endif // CONFIG_HAL_BOARD
diff --git a/libraries/AP_RPM/RPM_PX4_PWM.h b/libraries/AP_RPM/RPM_PX4_PWM.h
new file mode 100644
index 0000000000..87d9446047
--- /dev/null
+++ b/libraries/AP_RPM/RPM_PX4_PWM.h
@@ -0,0 +1,40 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ 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 .
+ */
+
+#ifndef AP_RPM_PX4_PWM_H
+#define AP_RPM_PX4_PWM_H
+
+#include "AP_RPM.h"
+#include "RPM_Backend.h"
+
+class AP_RPM_PX4_PWM : public AP_RPM_Backend
+{
+public:
+ // constructor
+ AP_RPM_PX4_PWM(AP_RPM &ranger, uint8_t instance, AP_RPM::RPM_State &_state);
+
+ // destructor
+ ~AP_RPM_PX4_PWM(void);
+
+ // update state
+ void update(void);
+
+private:
+ int _fd = -1;
+ uint64_t _last_timestamp = 0;
+};
+
+#endif // AP_RPM_PX4_PWM_H