From 3c129aefa15edc9fcdea3fdeaa5cbce9c5abdab3 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 18 Feb 2025 17:15:32 +0100 Subject: [PATCH] Add IC engine control module (#24055) Internal combustion engine control module. New actuator functions and RPM based start/restart logic. Not enabled by default. --------- Signed-off-by: Silvan Fuhrer Co-authored-by: Pernilla --- ROMFS/px4fmu_common/init.d-posix/rcS | 5 + ROMFS/px4fmu_common/init.d/rcS | 5 + msg/CMakeLists.txt | 1 + msg/InternalCombustionEngineControl.msg | 8 + src/lib/mixer_module/CMakeLists.txt | 1 + .../functions/FunctionICEngineControl.hpp | 83 ++++ src/lib/mixer_module/mixer_module.cpp | 1 + src/lib/mixer_module/mixer_module.hpp | 1 + src/lib/mixer_module/output_functions.yaml | 5 + .../CMakeLists.txt | 46 ++ .../InternalCombustionEngineControl.cpp | 397 ++++++++++++++++++ .../InternalCombustionEngineControl.hpp | 159 +++++++ .../Kconfig | 5 + .../module.yaml | 120 ++++++ src/modules/logger/logged_topics.cpp | 1 + 15 files changed, 838 insertions(+) create mode 100644 msg/InternalCombustionEngineControl.msg create mode 100644 src/lib/mixer_module/functions/FunctionICEngineControl.hpp create mode 100644 src/modules/internal_combustion_engine_control/CMakeLists.txt create mode 100644 src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp create mode 100644 src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp create mode 100644 src/modules/internal_combustion_engine_control/Kconfig create mode 100644 src/modules/internal_combustion_engine_control/module.yaml diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index ef2907deb7..c53dbbd46a 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -337,6 +337,11 @@ then payload_deliverer start fi +if param compare -s ICE_EN 1 +then + internal_combustion_engine_control start +fi + #user defined mavlink streams for instances can be in PATH . px4-rc.mavlink diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 807a26ed03..b850985fd8 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -555,6 +555,11 @@ else payload_deliverer start fi + if param compare -s ICE_EN 1 + then + internal_combustion_engine_control start + fi + # # Optional board supplied extras: rc.board_extras # diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index af7a98cf54..5844caafec 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -114,6 +114,7 @@ set(msg_files HeaterStatus.msg HoverThrustEstimate.msg InputRc.msg + InternalCombustionEngineControl.msg InternalCombustionEngineStatus.msg IridiumsbdStatus.msg IrlockReport.msg diff --git a/msg/InternalCombustionEngineControl.msg b/msg/InternalCombustionEngineControl.msg new file mode 100644 index 0000000000..08b4198e01 --- /dev/null +++ b/msg/InternalCombustionEngineControl.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +bool ignition_on # activate/deactivate ignition (Spark Plug) +float32 throttle_control # [0,1] - Motor should idle with 0. Includes slew rate if enabled. +float32 choke_control # [0,1] - 1 fully closes the air inlet. +float32 starter_engine_control # [0,1] - control value for electric starter motor. + +uint8 user_request # user intent for the ICE being on/off diff --git a/src/lib/mixer_module/CMakeLists.txt b/src/lib/mixer_module/CMakeLists.txt index 783dfe3dde..3a1581118b 100644 --- a/src/lib/mixer_module/CMakeLists.txt +++ b/src/lib/mixer_module/CMakeLists.txt @@ -48,6 +48,7 @@ px4_add_library(mixer_module functions/FunctionConstantMax.hpp functions/FunctionConstantMin.hpp functions/FunctionGimbal.hpp + functions/FunctionICEngineControl.hpp functions/FunctionLandingGear.hpp functions/FunctionManualRC.hpp functions/FunctionMotors.hpp diff --git a/src/lib/mixer_module/functions/FunctionICEngineControl.hpp b/src/lib/mixer_module/functions/FunctionICEngineControl.hpp new file mode 100644 index 0000000000..0609f5cc94 --- /dev/null +++ b/src/lib/mixer_module/functions/FunctionICEngineControl.hpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "FunctionProviderBase.hpp" + +#include + +/** + * Functions: ICE... + */ +class FunctionICEControl : public FunctionProviderBase +{ +public: + FunctionICEControl() + { + resetAllToDisarmedValue(); + } + + static FunctionProviderBase *allocate(const Context &context) { return new FunctionICEControl(); } + + void update() override + { + internal_combustion_engine_control_s internal_combustion_engine_control; + + // map [0, 1] to [-1, 1] which is the interface for non-motor PWM channels + if (_internal_combustion_engine_control_sub.update(&internal_combustion_engine_control)) { + _data[0] = internal_combustion_engine_control.ignition_on * 2.f - 1.f; + _data[1] = internal_combustion_engine_control.throttle_control * 2.f - 1.f; + _data[2] = internal_combustion_engine_control.choke_control * 2.f - 1.f; + _data[3] = internal_combustion_engine_control.starter_engine_control * 2.f - 1.f; + } + } + + float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::IC_Engine_Ignition]; } + +private: + static constexpr int num_data_points = 4; + + void resetAllToDisarmedValue() + { + for (int i = 0; i < num_data_points; ++i) { + _data[i] = NAN; + } + } + + static_assert(num_data_points == (int)OutputFunction::IC_Engine_Starter - (int)OutputFunction::IC_Engine_Ignition + 1, + "number of functions mismatch"); + + uORB::Subscription _internal_combustion_engine_control_sub{ORB_ID(internal_combustion_engine_control)}; + float _data[num_data_points] {}; +}; diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 45f56ab540..56a2d61356 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -64,6 +64,7 @@ static const FunctionProvider all_function_providers[] = { {OutputFunction::Gripper, &FunctionGripper::allocate}, {OutputFunction::RC_Roll, OutputFunction::RC_AUXMax, &FunctionManualRC::allocate}, {OutputFunction::Gimbal_Roll, OutputFunction::Gimbal_Yaw, &FunctionGimbal::allocate}, + {OutputFunction::IC_Engine_Ignition, OutputFunction::IC_Engine_Starter, &FunctionICEControl::allocate}, }; MixingOutput::MixingOutput(const char *param_prefix, uint8_t max_num_outputs, OutputModuleInterface &interface, diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index ff400ca6d5..c55b72da4e 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -40,6 +40,7 @@ #include "functions/FunctionConstantMin.hpp" #include "functions/FunctionGimbal.hpp" #include "functions/FunctionGripper.hpp" +#include "functions/FunctionICEngineControl.hpp" #include "functions/FunctionLandingGear.hpp" #include "functions/FunctionLandingGearWheel.hpp" #include "functions/FunctionManualRC.hpp" diff --git a/src/lib/mixer_module/output_functions.yaml b/src/lib/mixer_module/output_functions.yaml index a9965a5454..52543add97 100644 --- a/src/lib/mixer_module/output_functions.yaml +++ b/src/lib/mixer_module/output_functions.yaml @@ -38,6 +38,11 @@ functions: Landing_Gear_Wheel: 440 + IC_Engine_Ignition: 450 + IC_Engine_Throttle: 451 + IC_Engine_Choke: 452 + IC_Engine_Starter: 453 + # Add your own here: #MyCustomFunction: 10000 diff --git a/src/modules/internal_combustion_engine_control/CMakeLists.txt b/src/modules/internal_combustion_engine_control/CMakeLists.txt new file mode 100644 index 0000000000..15df6ca368 --- /dev/null +++ b/src/modules/internal_combustion_engine_control/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE modules__internal_combustion_engine_control + MAIN internal_combustion_engine_control + COMPILE_FLAGS + #-DDEBUG_BUILD + SRCS + InternalCombustionEngineControl.cpp + InternalCombustionEngineControl.hpp + MODULE_CONFIG + module.yaml + DEPENDS + px4_work_queue + SlewRate +) diff --git a/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp new file mode 100644 index 0000000000..f4ab6a05aa --- /dev/null +++ b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp @@ -0,0 +1,397 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "InternalCombustionEngineControl.hpp" + +#include + +using namespace time_literals; + +namespace internal_combustion_engine_control +{ + +InternalCombustionEngineControl::InternalCombustionEngineControl() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) +{ + _internal_combustion_engine_control_pub.advertise(); + _internal_combustion_engine_status_pub.advertise(); +} + +InternalCombustionEngineControl::~InternalCombustionEngineControl() +{ + +} + +int InternalCombustionEngineControl::task_spawn(int argc, char *argv[]) +{ + InternalCombustionEngineControl *obj = new InternalCombustionEngineControl(); + + if (!obj) { + PX4_ERR("alloc failed"); + return -1; + } + + _object.store(obj); + _task_id = task_id_is_work_queue; + + /* Schedule a cycle to start things. */ + obj->start(); + + return 0; +} + +void InternalCombustionEngineControl::start() +{ + ScheduleOnInterval(20_ms); // 50 Hz +} + +void InternalCombustionEngineControl::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + } + + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); + _throttle_control_slew_rate.setSlewRate(_param_ice_thr_slew.get()); + } + + + manual_control_setpoint_s manual_control_setpoint; + _manual_control_setpoint_sub.copy(&manual_control_setpoint); + + vehicle_status_s vehicle_status; + _vehicle_status_sub.copy(&vehicle_status); + + actuator_motors_s actuator_motors; + _actuator_motors.copy(&actuator_motors); + + const float throttle_in = actuator_motors.control[0]; + + const hrt_abstime now = hrt_absolute_time(); + + UserOnOffRequest user_request = UserOnOffRequest::Off; + + switch (static_cast(_param_ice_on_source.get())) { + case ICESource::ArmingState: { + if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + user_request = UserOnOffRequest::On; + } + } + break; + + case ICESource::Aux1: { + if (manual_control_setpoint.aux1 > 0.5f) { + user_request = UserOnOffRequest::On; + } + } + break; + + case ICESource::Aux2: { + if (manual_control_setpoint.aux2 > 0.5f) { + user_request = UserOnOffRequest::On; + } + } + break; + } + + switch (_state) { + case State::Stopped: { + controlEngineStop(); + + if (user_request == UserOnOffRequest::On && !maximumAttemptsReached()) { + + _state = State::Starting; + _state_start_time = now; + PX4_INFO("ICE: Starting"); + } + } + break; + + case State::Starting: { + + if (user_request == UserOnOffRequest::Off) { + _state = State::Stopped; + _starting_retry_cycle = 0; + PX4_INFO("ICE: Stopped"); + + } else { + + switch (_starting_sub_state) { + case SubState::Rest: { + if (isStartingPermitted(now)) { + _state_start_time = now; + _starting_sub_state = SubState::Run; + } + } + break; + + case SubState::Run: + default: { + controlEngineStartup(now); + + if (isEngineRunning(now)) { + _state = State::Running; + PX4_INFO("ICE: Starting finished"); + + } else { + + if (maximumAttemptsReached()) { + _state = State::Fault; + PX4_WARN("ICE: Fault"); + + } else if (!isStartingPermitted(now)) { + controlEngineStop(); + _starting_sub_state = SubState::Rest; + } + } + + break; + } + + } + } + + } + break; + + case State::Running: { + controlEngineRunning(throttle_in); + + if (user_request == UserOnOffRequest::Off) { + _state = State::Stopped; + _starting_retry_cycle = 0; + PX4_INFO("ICE: Stopped"); + + } else if (!isEngineRunning(now) && _param_ice_running_fault_detection.get()) { + // without RPM feedback we assume the engine is running after the + // starting procedure but only switch state if fault detection is enabled + _state = State::Starting; + _state_start_time = now; + _starting_retry_cycle = 0; + PX4_WARN("ICE: Running Fault detected"); + events::send(events::ID("internal_combustion_engine_control_fault"), events::Log::Critical, + "IC engine fault detected"); + } + } + + break; + + case State::Fault: { + + // do nothing + if (user_request == UserOnOffRequest::Off) { + _state = State::Stopped; + _starting_retry_cycle = 0; + PX4_INFO("ICE: Stopped"); + + } else { + controlEngineFault(); + } + } + + + break; + } + + const float control_interval = math::constrain(static_cast((now - _last_time_run) * 1e-6f), 0.01f, 0.1f); + + _last_time_run = now; + + // slew rate limit throttle control if it's finite, otherwise just pass it through (0 throttle = NAN = disarmed) + if (PX4_ISFINITE(_throttle_control)) { + _throttle_control = _throttle_control_slew_rate.update(_throttle_control, control_interval); + + } else { + _throttle_control_slew_rate.setForcedValue(0.f); + } + + publishControl(now, user_request); +} + +void InternalCombustionEngineControl::publishControl(const hrt_abstime now, const UserOnOffRequest user_request) +{ + internal_combustion_engine_control_s ice_control{}; + ice_control.timestamp = now; + ice_control.choke_control = _choke_control; + ice_control.ignition_on = _ignition_on; + ice_control.starter_engine_control = _starter_engine_control; + ice_control.throttle_control = _throttle_control; + ice_control.user_request = static_cast(user_request); + _internal_combustion_engine_control_pub.publish(ice_control); + + internal_combustion_engine_status_s ice_status; + ice_status.state = static_cast(_state); + ice_status.timestamp = now; + _internal_combustion_engine_status_pub.publish(ice_status); +} + +bool InternalCombustionEngineControl::isEngineRunning(const hrt_abstime now) +{ + rpm_s rpm; + + if (_rpm_sub.copy(&rpm)) { + const hrt_abstime rpm_timestamp = rpm.timestamp; + + return (_param_ice_min_run_rpm.get() > FLT_EPSILON && (now < rpm_timestamp + 2_s) + && rpm.rpm_estimate > _param_ice_min_run_rpm.get()); + } + + return false; +} + +void InternalCombustionEngineControl::controlEngineRunning(float throttle_in) +{ + _ignition_on = true; + _choke_control = 0.f; + _starter_engine_control = 0.f; + _throttle_control = throttle_in; + +} + +void InternalCombustionEngineControl::controlEngineStop() +{ + _ignition_on = false; + _choke_control = _param_ice_stop_choke.get() ? 1.f : 0.f; + _starter_engine_control = 0.f; + _throttle_control = 0.f; +} + +void InternalCombustionEngineControl::controlEngineFault() +{ + _ignition_on = false; + _choke_control = _param_ice_stop_choke.get() ? 1.f : 0.f; + _starter_engine_control = 0.f; + _throttle_control = 0.f; +} + +void InternalCombustionEngineControl::controlEngineStartup(const hrt_abstime now) +{ + float ignition_delay = 0.f; + float choke_duration = 0.f; + const float starter_duration = _param_ice_strt_dur.get(); + + if (_starting_retry_cycle == 0) { + ignition_delay = math::max(_param_ice_ign_delay.get(), 0.f); + + if (_param_ice_choke_st_dur.get() > FLT_EPSILON) { + choke_duration = _param_ice_choke_st_dur.get(); + } + } + + _ignition_on = true; + _throttle_control = _param_ice_strt_thr.get(); + _choke_control = now < _state_start_time + (choke_duration + ignition_delay) * 1_s ? 1.f : 0.f; + _starter_engine_control = now > _state_start_time + (ignition_delay * 1_s) ? 1.f : 0.f; + const hrt_abstime cycle_timeout_duration = (ignition_delay + choke_duration + starter_duration) * 1_s; + + if (now > _state_start_time + cycle_timeout_duration) { + // start resting timer if engine is not running + _starting_rest_time = now; + _starting_retry_cycle++; + PX4_INFO("ICE: starting attempt %i finished", _starting_retry_cycle); + } +} + +bool InternalCombustionEngineControl::isStartingPermitted(const hrt_abstime now) +{ + return now > _starting_rest_time + DELAY_BEFORE_RESTARTING * 1_s; +} + +bool InternalCombustionEngineControl::maximumAttemptsReached() +{ + // First and only attempt + if (_param_ice_strt_attempts.get() == 0) { + return _starting_retry_cycle > 0; + } + + return _starting_retry_cycle >= _param_ice_strt_attempts.get(); +} + +int InternalCombustionEngineControl::print_usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +The module controls internal combustion engine (ICE) features including: +ignition (on/off),throttle and choke level, starter engine delay, and user request. +The module publishes [InternalCombustionEngineControl.msg](../msg_docs/InternalCombustionEngineControl.md). +The architecture is as shown below.: +![Architecture](../../assets/diagrams/ice_control_diagram.png) + +### Enabling +This feature is not enabled by default needs to be configured in the +build target for your board together with the rpm capture driver: +CONFIG_MODULES_INTERNAL_COMBUSTION_ENGINE_CONTROL=y +CONFIG_DRIVERS_RPM_CAPTURE=y + +Additionally, to enable the module: +- set [ICE_EN](https://docs.px4.io/main/en/advanced_config/parameter_reference.html#ICE_EN) +to true and adjust the other module parameters ICE_ according to your needs. +- set [RPM_CAP_ENABLE](https://docs.px4.io/main/en/advanced_config/parameter_reference.html#RPM_CAP_ENABLE) to true. + +### Implementation +The ICE is implemented with a (4) state machine: +![Architecture](../../assets/diagrams/ice_control_state_machine.png) +The state machine: +- checks if [Rpm.msg](../msg_docs/Rpm.md) is updated to know if the engine is running +- allows for user inputs from +- AUX{N} +- Arming state in [VehicleStatus.msg(../msg_docs/VehicleStatus.md) + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("internal_combustion_engine_control", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +extern "C" __EXPORT int internal_combustion_engine_control_main(int argc, char *argv[]) +{ + return InternalCombustionEngineControl::main(argc, argv); +} + +} // namespace internal_combustion_engine_control diff --git a/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp new file mode 100644 index 0000000000..01884e4967 --- /dev/null +++ b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp @@ -0,0 +1,159 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace internal_combustion_engine_control +{ + +class InternalCombustionEngineControl : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + InternalCombustionEngineControl(); + ~InternalCombustionEngineControl() override; + + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]) + { + return print_usage("unknown command"); + } + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + void start(); + +private: + void Run() override; + + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _rpm_sub{ORB_ID(rpm)}; + uORB::Subscription _actuator_motors{ORB_ID(actuator_motors)}; + + uORB::Publication _internal_combustion_engine_control_pub{ORB_ID(internal_combustion_engine_control)}; + uORB::Publication _internal_combustion_engine_status_pub{ORB_ID(internal_combustion_engine_status)}; + + + // has to mirror internal_combustion_engine_status_s::State + enum class State { + Stopped, + Starting, + Running, + Fault + } _state{State::Stopped}; + + enum class SubState { + Run, + Rest + }; + + enum class UserOnOffRequest { + Off, + On + }; + + enum class ICESource { + ArmingState, + Aux1, + Aux2, + }; + + hrt_abstime _state_start_time{0}; + hrt_abstime _last_time_run{0}; + + bool _ignition_on{false}; + float _choke_control{1.f}; + float _starter_engine_control{0.f}; + float _throttle_control{0.f}; + + SlewRate _throttle_control_slew_rate; + + bool isEngineRunning(const hrt_abstime now); + void controlEngineRunning(float throttle_in); + void controlEngineStop(); + void controlEngineStartup(const hrt_abstime now); + void controlEngineFault(); + bool maximumAttemptsReached(); + void publishControl(const hrt_abstime now, const UserOnOffRequest user_request); + + // Starting state specifics + static constexpr float DELAY_BEFORE_RESTARTING{1.f}; + int _starting_retry_cycle{0}; + hrt_abstime _starting_rest_time{0}; + SubState _starting_sub_state{SubState::Run}; + + /** + * @brief Currently the ICE starting state is permitted after resting + * DELAY_BEFORE_RESTARTING s to reduce wear on the starter motor. + * @param now current time + * @return if true, otherwise false + */ + bool isStartingPermitted(const hrt_abstime now); + + DEFINE_PARAMETERS( + (ParamInt) _param_ice_on_source, + (ParamFloat) _param_ice_choke_st_dur, + (ParamFloat) _param_ice_strt_dur, + (ParamFloat) _param_ice_min_run_rpm, + (ParamInt) _param_ice_strt_attempts, + (ParamInt) _param_ice_running_fault_detection, + (ParamFloat) _param_ice_strt_thr, + (ParamInt) _param_ice_stop_choke, + (ParamFloat) _param_ice_thr_slew, + (ParamFloat) _param_ice_ign_delay + ) +}; + +} // namespace internal_combustion_engine_control diff --git a/src/modules/internal_combustion_engine_control/Kconfig b/src/modules/internal_combustion_engine_control/Kconfig new file mode 100644 index 0000000000..c0d0e9049f --- /dev/null +++ b/src/modules/internal_combustion_engine_control/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_INTERNAL_COMBUSTION_ENGINE_CONTROL + bool "internal_combustion_engine_control" + default n + ---help--- + Enable support for internal_combustion_engine_control diff --git a/src/modules/internal_combustion_engine_control/module.yaml b/src/modules/internal_combustion_engine_control/module.yaml new file mode 100644 index 0000000000..517fc89fee --- /dev/null +++ b/src/modules/internal_combustion_engine_control/module.yaml @@ -0,0 +1,120 @@ +module_name: Internal Combustion Engine Control + +parameters: + - group: ICE + definitions: + + ICE_EN: + description: + short: Enable internal combustion engine + type: boolean + default: true + + ICE_ON_SOURCE: + description: + short: Engine start/stop input source + type: enum + default: 0 + values: + 0: On arming - disarming + 1: Aux1 + 2: Aux2 + + ICE_CHOKE_ST_DUR: + description: + short: Duration of choking during startup + type: float + unit: s + min: 0 + max: 10 + decimal: 1 + increment: 0.1 + default: 5 + + ICE_STRT_DUR: + description: + short: Duration of single starting attempt (excl. choking) + long: | + Maximum expected time for startup before declaring timeout. + type: float + unit: s + min: 0 + max: 10 + decimal: 1 + increment: 0.1 + default: 5 + + ICE_MIN_RUN_RPM: + description: + short: Minimum RPM for engine to be declared running + type: float + unit: rpm + min: 0 + max: 10000 + decimal: 0 + increment: 1 + default: 2000 + + ICE_STRT_ATTEMPT: + description: + short: Number attempts for starting the engine + long: | + Number of accepted attempts for starting the engine before declaring a fault. + type: int32 + min: 0 + max: 10 + default: 3 + + ICE_RUN_FAULT_D: + description: + short: Fault detection if it stops in running state + long: | + Enables restart if a fault is detected during the running state. Otherwise + commands continues in running state until given an user request off. + type: boolean + default: true + + ICE_STRT_THR: + description: + short: Throttle value for starting engine + long: | + During the choking and the starting phase, the throttle value is set to this value. + type: float + unit: norm + min: 0 + max: 1 + decimal: 0 + increment: 0.01 + default: 0.1 + + ICE_STOP_CHOKE: + description: + short: Apply choke when stopping engine + type: boolean + default: true + + ICE_THR_SLEW: + description: + short: Throttle slew rate + long: | + Maximum rate of change of throttle value per second. + type: float + unit: 1/s + min: 0 + max: 1 + decimal: 2 + increment: 0.01 + default: 0.5 + + ICE_IGN_DELAY: + description: + short: Cold-start delay after ignition before engaging starter + long: | + In case that the ignition takes a moment to be up and running, this parameter can be set to account for that. + type: float + unit: s + min: 0 + max: 10 + decimal: 1 + increment: 0.1 + default: 0 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 3114d83bb1..4882061d1d 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -79,6 +79,7 @@ void LoggedTopics::add_default_topics() add_topic("home_position"); add_topic("hover_thrust_estimate", 100); add_topic("input_rc", 500); + add_optional_topic("internal_combustion_engine_control", 10); add_optional_topic("internal_combustion_engine_status", 10); add_optional_topic("iridiumsbd_status", 1000); add_optional_topic("irlock_report", 1000);