AP_RPM: allow harmonic notch driver to appear as RPM values.

libraries/AP_RPM/AP_RPM.cpp

@@ -17,6 +17,7 @@
 #include "RPM_Pin.h"
 #include "RPM_SITL.h"
 #include "RPM_EFI.h"
+#include "RPM_HarmonicNotch.h"
 
 extern const AP_HAL::HAL& hal;
 
@@ -25,7 +26,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:PWM,2:AUXPIN,3:EFI
+    // @Values: 0:None,1:PWM,2:AUXPIN,3:EFI,4:Harmonic Notch
     // @User: Standard
     AP_GROUPINFO("_TYPE",    0, AP_RPM, _type[0], 0),
 
@@ -68,7 +69,7 @@ const AP_Param::GroupInfo AP_RPM::var_info[] = {
     // @Param: 2_TYPE
     // @DisplayName: Second RPM type
     // @Description: What type of RPM sensor is connected
-    // @Values: 0:None,1:PWM,2:AUXPIN
+    // @Values: 0:None,1:PWM,2:AUXPIN,3:EFI,4:Harmonic Notch
     // @User: Advanced
     AP_GROUPINFO("2_TYPE",    10, AP_RPM, _type[1], 0),
 
@@ -125,6 +126,11 @@ void AP_RPM::init(void)
             drivers[i] = new AP_RPM_EFI(*this, i, state[i]);
         }
 #endif
+        // include harmonic notch last
+        // this makes whatever process is driving the dynamic notch appear as an RPM value
+        if (type == RPM_TYPE_HNTCH) {
+            drivers[i] = new AP_RPM_HarmonicNotch(*this, i, state[i]);
+        }
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
         if (drivers[i] == nullptr) {
             drivers[i] = new AP_RPM_SITL(*this, i, state[i]);

libraries/AP_RPM/AP_RPM.h

@@ -40,7 +40,8 @@ public:
         RPM_TYPE_NONE    = 0,
         RPM_TYPE_PWM     = 1,
         RPM_TYPE_PIN     = 2,
-        RPM_TYPE_EFI     = 3
+        RPM_TYPE_EFI     = 3,
+        RPM_TYPE_HNTCH   = 4
     };
 
     // The RPM_State structure is filled in by the backend driver

libraries/AP_RPM/RPM_HarmonicNotch.cpp

@@ -0,0 +1,42 @@
+/*
+   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>
+#include <AP_InertialSensor/AP_InertialSensor.h>
+
+#include "RPM_HarmonicNotch.h"
+
+extern const AP_HAL::HAL& hal;
+
+/* 
+   open the sensor in constructor
+*/
+AP_RPM_HarmonicNotch::AP_RPM_HarmonicNotch(AP_RPM &_ap_rpm, uint8_t _instance, AP_RPM::RPM_State &_state) :
+    AP_RPM_Backend(_ap_rpm, _instance, _state)
+{
+    instance = _instance;
+}
+
+void AP_RPM_HarmonicNotch::update(void)
+{
+    AP_InertialSensor& ins = AP::ins();
+    if (ins.get_gyro_harmonic_notch_tracking_mode() != HarmonicNotchDynamicMode::Fixed) {
+        state.rate_rpm = ins.get_gyro_dynamic_notch_center_freq_hz() * 60.0f;
+        state.rate_rpm *= ap_rpm._scaling[state.instance];
+        state.signal_quality = 0.5f;
+        state.last_reading_ms = AP_HAL::millis();
+    }
+}
+

libraries/AP_RPM/RPM_HarmonicNotch.h

@@ -0,0 +1,31 @@
+/*
+   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"
+
+class AP_RPM_HarmonicNotch : public AP_RPM_Backend
+{
+public:
+    // constructor
+    AP_RPM_HarmonicNotch(AP_RPM &ranger, uint8_t instance, AP_RPM::RPM_State &_state);
+
+    // update state
+    void update(void) override;
+
+private:
+    uint8_t instance;
+};