From 13453f705effd7a3bf64336190c6ffa2b102aa82 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 29 Aug 2022 19:34:58 +1000 Subject: [PATCH] AP_RPM: added AP_Generator support useful for notch filtering of generator vibration --- libraries/AP_RPM/AP_RPM.cpp | 6 +++++ libraries/AP_RPM/AP_RPM.h | 1 + libraries/AP_RPM/AP_RPM_Params.cpp | 2 +- libraries/AP_RPM/RPM_Generator.cpp | 38 ++++++++++++++++++++++++++++++ libraries/AP_RPM/RPM_Generator.h | 34 ++++++++++++++++++++++++++ 5 files changed, 80 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_RPM/RPM_Generator.cpp create mode 100644 libraries/AP_RPM/RPM_Generator.h diff --git a/libraries/AP_RPM/AP_RPM.cpp b/libraries/AP_RPM/AP_RPM.cpp index dc45508d2e..9134e1afcb 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_Generator.h" #include "RPM_HarmonicNotch.h" #include "RPM_ESC_Telem.h" @@ -79,6 +80,11 @@ void AP_RPM::init(void) case RPM_TYPE_EFI: drivers[i] = new AP_RPM_EFI(*this, i, state[i]); break; +#endif +#if HAL_GENERATOR_ENABLED + case RPM_TYPE_GENERATOR: + drivers[i] = new AP_RPM_Generator(*this, i, state[i]); + break; #endif // include harmonic notch last // this makes whatever process is driving the dynamic notch appear as an RPM value diff --git a/libraries/AP_RPM/AP_RPM.h b/libraries/AP_RPM/AP_RPM.h index 68d48cb51f..e54f857dc0 100644 --- a/libraries/AP_RPM/AP_RPM.h +++ b/libraries/AP_RPM/AP_RPM.h @@ -42,6 +42,7 @@ public: RPM_TYPE_EFI = 3, RPM_TYPE_HNTCH = 4, RPM_TYPE_ESC_TELEM = 5, + RPM_TYPE_GENERATOR = 6, #if CONFIG_HAL_BOARD == HAL_BOARD_SITL RPM_TYPE_SITL = 10, #endif diff --git a/libraries/AP_RPM/AP_RPM_Params.cpp b/libraries/AP_RPM/AP_RPM_Params.cpp index b99c1f03fe..8a3419594e 100644 --- a/libraries/AP_RPM/AP_RPM_Params.cpp +++ b/libraries/AP_RPM/AP_RPM_Params.cpp @@ -20,7 +20,7 @@ const AP_Param::GroupInfo AP_RPM_Params::var_info[] = { // @Param: TYPE // @DisplayName: RPM type // @Description: What type of RPM sensor is connected - // @Values: 0:None,1:Not Used,2:GPIO,3:EFI,4:Harmonic Notch,5:ESC Telemetry Motors Bitmask + // @Values: 0:None,1:Not Used,2:GPIO,3:EFI,4:Harmonic Notch,5:ESC Telemetry Motors Bitmask,6:Generator // @User: Standard AP_GROUPINFO_FLAGS("TYPE", 1, AP_RPM_Params, type, 0, AP_PARAM_FLAG_ENABLE), // Note, 1 was previously for type = PWM. This has been removed from docs to make setup less confusing for users. diff --git a/libraries/AP_RPM/RPM_Generator.cpp b/libraries/AP_RPM/RPM_Generator.cpp new file mode 100644 index 0000000000..50e2d3627d --- /dev/null +++ b/libraries/AP_RPM/RPM_Generator.cpp @@ -0,0 +1,38 @@ +/* + 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 "RPM_Generator.h" + +#if HAL_GENERATOR_ENABLED +extern const AP_HAL::HAL& hal; + +void AP_RPM_Generator::update(void) +{ + const auto *generator = AP::generator(); + if (generator == nullptr) { + return; + } + if (generator->healthy()) { + state.rate_rpm = generator->get_rpm(); + state.rate_rpm *= ap_rpm._params[state.instance].scaling; + state.signal_quality = 0.5; + } else { + state.signal_quality = 0; + } +} + +#endif // HAL_GENERATOR_ENABLED diff --git a/libraries/AP_RPM/RPM_Generator.h b/libraries/AP_RPM/RPM_Generator.h new file mode 100644 index 0000000000..82e8e0ff8d --- /dev/null +++ b/libraries/AP_RPM/RPM_Generator.h @@ -0,0 +1,34 @@ +/* + 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 + +#if HAL_GENERATOR_ENABLED + +class AP_RPM_Generator : public AP_RPM_Backend +{ +public: + // constructor + using AP_RPM_Backend::AP_RPM_Backend; + + // update state + void update(void) override; +}; + +#endif // HAL_GENERATOR_ENABLED +