diff --git a/libraries/AP_RPM/AP_RPM.cpp b/libraries/AP_RPM/AP_RPM.cpp index a053d51b8d..2bea2cbd7c 100644 --- a/libraries/AP_RPM/AP_RPM.cpp +++ b/libraries/AP_RPM/AP_RPM.cpp @@ -16,6 +16,7 @@ #include "AP_RPM.h" #include "RPM_Pin.h" #include "RPM_SITL.h" +#include "RPM_EFI.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:PWM,2:AUXPIN + // @Values: 0:None,1:PWM,2:AUXPIN,3:EFI // @User: Standard AP_GROUPINFO("_TYPE", 0, AP_RPM, _type[0], 0), @@ -118,8 +119,15 @@ void AP_RPM::init(void) if (type == RPM_TYPE_PIN) { drivers[i] = new AP_RPM_Pin(*this, i, state[i]); } +#if EFI_ENABLED + if (type == RPM_TYPE_EFI) { + drivers[i] = new AP_RPM_EFI(*this, i, state[i]); + } +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - drivers[i] = new AP_RPM_SITL(*this, i, state[i]); + if (drivers[i] == nullptr) { + drivers[i] = new AP_RPM_SITL(*this, i, state[i]); + } #endif if (drivers[i] != nullptr) { // we loaded a driver for this instance, so it must be diff --git a/libraries/AP_RPM/AP_RPM.h b/libraries/AP_RPM/AP_RPM.h index 3277c3efe2..b7d297a9a7 100644 --- a/libraries/AP_RPM/AP_RPM.h +++ b/libraries/AP_RPM/AP_RPM.h @@ -39,7 +39,8 @@ public: enum RPM_Type { RPM_TYPE_NONE = 0, RPM_TYPE_PWM = 1, - RPM_TYPE_PIN = 2 + RPM_TYPE_PIN = 2, + RPM_TYPE_EFI = 3 }; // The RPM_State structure is filled in by the backend driver diff --git a/libraries/AP_RPM/RPM_EFI.cpp b/libraries/AP_RPM/RPM_EFI.cpp new file mode 100644 index 0000000000..74d338b2ee --- /dev/null +++ b/libraries/AP_RPM/RPM_EFI.cpp @@ -0,0 +1,44 @@ +/* + 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_EFI.h" + +#if EFI_ENABLED +extern const AP_HAL::HAL& hal; + +/* + open the sensor in constructor +*/ +AP_RPM_EFI::AP_RPM_EFI(AP_RPM &_ap_rpm, uint8_t _instance, AP_RPM::RPM_State &_state) : + AP_RPM_Backend(_ap_rpm, _instance, _state) +{ + instance = _instance; +} + +void AP_RPM_EFI::update(void) +{ + AP_EFI *efi = AP::EFI(); + if (efi == nullptr) { + return; + } + state.rate_rpm = efi->get_rpm(); + state.rate_rpm *= ap_rpm._scaling[state.instance]; + state.signal_quality = 0.5f; + state.last_reading_ms = AP_HAL::millis(); +} + +#endif // EFI_ENABLED diff --git a/libraries/AP_RPM/RPM_EFI.h b/libraries/AP_RPM/RPM_EFI.h new file mode 100644 index 0000000000..b30fbfda3c --- /dev/null +++ b/libraries/AP_RPM/RPM_EFI.h @@ -0,0 +1,32 @@ +/* + 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 + +class AP_RPM_EFI : public AP_RPM_Backend +{ +public: + // constructor + AP_RPM_EFI(AP_RPM &ranger, uint8_t instance, AP_RPM::RPM_State &_state); + + // update state + void update(void) override; + +private: + uint8_t instance; +}; diff --git a/libraries/AP_RPM/RPM_SITL.cpp b/libraries/AP_RPM/RPM_SITL.cpp index b464388b7a..18965094cd 100644 --- a/libraries/AP_RPM/RPM_SITL.cpp +++ b/libraries/AP_RPM/RPM_SITL.cpp @@ -43,6 +43,7 @@ void AP_RPM_SITL::update(void) state.rate_rpm *= ap_rpm._scaling[state.instance]; state.signal_quality = 0.5f; state.last_reading_ms = AP_HAL::millis(); + } #endif // CONFIG_HAL_BOARD