From c4217f329b6c50305dfd1d52eab1e3c1224ceeb4 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 6 Apr 2020 13:54:00 +0100 Subject: [PATCH] AP_RPM: allow harmonic notch driver to appear as RPM values. --- libraries/AP_RPM/AP_RPM.cpp | 10 ++++-- libraries/AP_RPM/AP_RPM.h | 3 +- libraries/AP_RPM/RPM_HarmonicNotch.cpp | 42 ++++++++++++++++++++++++++ libraries/AP_RPM/RPM_HarmonicNotch.h | 31 +++++++++++++++++++ 4 files changed, 83 insertions(+), 3 deletions(-) create mode 100644 libraries/AP_RPM/RPM_HarmonicNotch.cpp create mode 100644 libraries/AP_RPM/RPM_HarmonicNotch.h diff --git a/libraries/AP_RPM/AP_RPM.cpp b/libraries/AP_RPM/AP_RPM.cpp index ca124888a5..8a412cb3ee 100644 --- a/libraries/AP_RPM/AP_RPM.cpp +++ b/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]); diff --git a/libraries/AP_RPM/AP_RPM.h b/libraries/AP_RPM/AP_RPM.h index 76150d75f4..aae1242b0a 100644 --- a/libraries/AP_RPM/AP_RPM.h +++ b/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 diff --git a/libraries/AP_RPM/RPM_HarmonicNotch.cpp b/libraries/AP_RPM/RPM_HarmonicNotch.cpp new file mode 100644 index 0000000000..7c64c922c9 --- /dev/null +++ b/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 . + */ + +#include +#include + +#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(); + } +} + diff --git a/libraries/AP_RPM/RPM_HarmonicNotch.h b/libraries/AP_RPM/RPM_HarmonicNotch.h new file mode 100644 index 0000000000..4817d441a4 --- /dev/null +++ b/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 . + */ +#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; +};