diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index e47b685a21f7..ef911eda5bd4 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 28804361d1ed..1eee7b8cf65a 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -535,6 +535,12 @@ else payload_deliverer start fi + if param compare -s ICE_EN 1 + then + internal_combustion_engine_control start + echo "****INFO [init] Internal Combustion Engine Control started" + fi + # # Optional board supplied extras: rc.board_extras # diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index f157cf5ae8d7..bf5ffbc6ac36 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -80,3 +80,5 @@ CONFIG_EXAMPLES_HELLO=y CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y CONFIG_EXAMPLES_PX4_SIMPLE_APP=y CONFIG_EXAMPLES_WORK_ITEM=y + +CONFIG_MODULES_INTERNAL_COMBUSTION_ENGINE_CONTROL=y diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index c87e75fb695e..4593afdc0962 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -122,6 +122,7 @@ set(msg_files HomePosition.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 000000000000..180bd3bb2147 --- /dev/null +++ b/msg/InternalCombustionEngineControl.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +bool ignition_on # activate/deactivate ignition +float32 throttle_control # [0,1] - Motor should idle with 0. Includes slew rate if enabled. +float32 choke_control # [0,1] +float32 starter_engine_control # [0,1] + +uint8 user_request # user intent for on/off/keep diff --git a/src/lib/mixer_module/CMakeLists.txt b/src/lib/mixer_module/CMakeLists.txt index 783dfe3ddef9..3a1581118b05 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 000000000000..50652a6289e3 --- /dev/null +++ b/src/lib/mixer_module/functions/FunctionICEngineControl.hpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 (_topic.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 _topic{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 45f56ab54025..56a2d61356e2 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 ff400ca6d59f..c55b72da4e92 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 7543ed388bce..214e511b76d8 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 000000000000..bbb075979eac --- /dev/null +++ b/src/modules/internal_combustion_engine_control/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2024 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 000000000000..7f146dd864e4 --- /dev/null +++ b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp @@ -0,0 +1,356 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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" + +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()); + } + + UserOnOffRequest user_request = UserOnOffRequest::Keep; // todo: keep is not yet doing anything + + 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]; + + switch (_param_ice_on_source.get()) { + case 0: + user_request = UserOnOffRequest::Off; + break; + + case 1: + user_request = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? UserOnOffRequest::On : + UserOnOffRequest::Off; + break; + + case 2: + user_request = UserOnOffRequest::Off; + break; + + case 3: + user_request = manual_control_setpoint.aux1 > 0.5f ? UserOnOffRequest::On : UserOnOffRequest::Off; + break; + + case 4: + user_request = manual_control_setpoint.aux2 > 0.5f ? UserOnOffRequest::On : UserOnOffRequest::Off; + break; + + case 5: + user_request = manual_control_setpoint.aux3 > 0.5f ? UserOnOffRequest::On : UserOnOffRequest::Off; + break; + + case 6: + // TODO: remove hack to have it testable with yaw stick + user_request = manual_control_setpoint.yaw > 0.5f ? UserOnOffRequest::On : UserOnOffRequest::Off; + break; + } + + internal_combustion_engine_control_s ice_control{}; + + switch (_state) { + case State::Stopped: + + controlEngineStop(ice_control); + + if (user_request == UserOnOffRequest::On && !engine_tried_to_restart) { + _state = State::Starting; + instantiateEngineStart(); + controlEngineStartup(ice_control); + PX4_INFO("ICE: Starting"); + } + + break; + + case State::Starting: + + if (user_request == UserOnOffRequest::Off) { + _state = State::Stopped; + controlEngineStop(ice_control); + PX4_INFO("ICE: Abort"); + + } else { + if (isEngineRunning()) { + _state = State::Running; + controlEngineRunning(ice_control, throttle_in); + PX4_INFO("ICE: Starting finished"); + + } else { + controlEngineStartup(ice_control); + + if (_starting_retry_cycle >= _param_ice_strt_retry.get()) { + _state = State::Fault; + PX4_WARN("ICE: Fault"); + + engine_tried_to_restart = true; + } + } + } + + break; + + case State::Running: + + controlEngineRunning(ice_control, throttle_in); + + if (user_request == UserOnOffRequest::Off) { + _state = State::Stopped; + PX4_INFO("ICE: Stop"); + + } else { + if (!isEngineRunning()) { + _state = State::Fault; + PX4_WARN("ICE: Fault detected"); + } + } + + break; + + case State::Fault: + + // do nothing + if (user_request == UserOnOffRequest::Off) { + _state = State::Stopped; + controlEngineStop(ice_control); + PX4_INFO("ICE: Stop"); + + } else { + if (_param_ice_retry_fault.get() && !engine_tried_to_restart) { + _state = State::Starting; + instantiateEngineStart(); + controlEngineStartup(ice_control); + PX4_INFO("ICE: Restarting"); + + } else { + controlEngineFault(ice_control); + } + } + + break; + } + + const hrt_abstime now = hrt_absolute_time(); + + 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(ice_control.throttle_control)) { + ice_control.throttle_control = _throttle_control_slew_rate.update(ice_control.throttle_control, control_interval); + + } else { + _throttle_control_slew_rate.setForcedValue(0.f); + } + + ice_control.timestamp = now; + 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() +{ + rpm_s rpm; + _rpm_sub.copy(&rpm); + + const bool rpm_is_recent = hrt_elapsed_time(&rpm.timestamp) < 2_s; + + const bool use_rpm_feedback_for_running = _param_ice_min_run_rpm.get() > FLT_EPSILON && rpm_is_recent; + bool engine_running = false; + + if (use_rpm_feedback_for_running) { + + if (rpm.rpm_estimate > _param_ice_min_run_rpm.get()) { + engine_running = true; + } + + } else { + // without RPM feedback we assume the engine is running after the starting procedure + engine_running = _starting_retry_cycle > 0; + } + + return engine_running; +} + +void InternalCombustionEngineControl::instantiateEngineStart() +{ + _state_start_time = hrt_absolute_time(); +} + +void InternalCombustionEngineControl::controlEngineRunning(internal_combustion_engine_control_s &ice_control, + float throttle_in) +{ + ice_control.ignition_on = true; + ice_control.choke_control = 0.f; + ice_control.starter_engine_control = 0.f; + ice_control.throttle_control = throttle_in; + + engine_tried_to_restart = false; +} + +void InternalCombustionEngineControl::controlEngineStop(internal_combustion_engine_control_s &ice_control) +{ + ice_control.ignition_on = false; + ice_control.choke_control = _param_ice_stop_choke.get() ? 1.f : 0.f; + ice_control.starter_engine_control = 0.f; + ice_control.throttle_control = 0.f; + + _starting_retry_cycle = 0; + engine_tried_to_restart = false; +} + +void InternalCombustionEngineControl::controlEngineFault(internal_combustion_engine_control_s &ice_control) +{ + ice_control.ignition_on = false; + ice_control.choke_control = 0.f; + ice_control.starter_engine_control = 0.f; + ice_control.throttle_control = 0.f; +} + +void InternalCombustionEngineControl::controlEngineStartup(internal_combustion_engine_control_s &ice_control) +{ + const float choke_duration = _param_ice_choke_st_dur.get() * 1_s; // todo make cold/hot start dependent + const float starter_duration = _param_ice_starting_dur.get() * 1_s; + + if (hrt_elapsed_time(&_state_start_time) < choke_duration) { + // choking engine to pump fuel + ice_control.ignition_on = true; + ice_control.choke_control = 1.f; + ice_control.starter_engine_control = 1.f; + ice_control.throttle_control = _param_ice_strt_thr.get(); + + } else if (hrt_elapsed_time(&_state_start_time) < choke_duration + starter_duration) { + // starting engine + ice_control.ignition_on = true; + ice_control.choke_control = 0.f; + ice_control.starter_engine_control = 1.f; + ice_control.throttle_control = _param_ice_strt_thr.get(); + + } else { + // reset timer to restart procedure if engine is not running + _state_start_time = hrt_absolute_time(); + _starting_retry_cycle++; + PX4_INFO("ICE: Retry %i finished", _starting_retry_cycle); + } +} + +int InternalCombustionEngineControl::print_usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +ICE controls. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("internal_combustion_engine_control", "system"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task"); + 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 000000000000..88ad3d60fe47 --- /dev/null +++ b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 UserOnOffRequest { + Keep, + Off, + On + }; + + hrt_abstime _state_start_time{0}; + hrt_abstime _last_time_run{0}; + int _starting_retry_cycle{0}; + bool engine_tried_to_restart{false}; + + SlewRate _throttle_control_slew_rate; + + bool isEngineRunning(); + void instantiateEngineStart(); + void controlEngineRunning(internal_combustion_engine_control_s &ice_control, float throttle_in); + void controlEngineStop(internal_combustion_engine_control_s &ice_control); + void controlEngineStartup(internal_combustion_engine_control_s &ice_control); + void controlEngineFault(internal_combustion_engine_control_s &ice_control); + + DEFINE_PARAMETERS( + (ParamInt) _param_ice_on_source, + (ParamFloat) _param_ice_choke_st_dur, + (ParamFloat) _param_ice_starting_dur, + (ParamFloat) _param_ice_min_run_rpm, + (ParamInt) _param_ice_strt_retry, + (ParamInt) _param_ice_retry_fault, + (ParamFloat) _param_ice_strt_thr, + (ParamInt) _param_ice_stop_choke, + (ParamFloat) _param_ice_thr_slew + ) +}; + +} // 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 000000000000..c0d0e9049fc7 --- /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 000000000000..70188df02b5b --- /dev/null +++ b/src/modules/internal_combustion_engine_control/module.yaml @@ -0,0 +1,110 @@ +parameters: + - group: ICE + definitions: + + ICE_EN: + description: + short: Enable internal combustion engine + long: TBD + type: boolean + default: true + + ICE_ON_SOURCE: + description: + short: Source of input to activate engine + long: TBD + type: enum + default: 0 + values: + 0: No activation + 1: On arming - disarming + 2: Mavlink? + 3: Aux1 + 4: Aux2 + 5: Aux3 + 6: Aux4 + + ICE_CHOKE_ST_DUR: + description: + short: Duration for choking before startup + long: TBD + type: float + unit: s + min: 0 + max: 10 + decimal: 1 + increment: 0.1 + default: 1 + + ICE_STARTING_DUR: + description: + short: Duration for starting up (excl. choking) + long: TBD + type: float + unit: s + min: 0 + max: 10 + decimal: 1 + increment: 0.1 + default: 2 + + ICE_MIN_RUN_RPM: + description: + short: Minimum RPM for engine to be declared running + long: TBD + type: float + unit: rpm + min: 0 + max: 10000 + decimal: 0 + increment: 1 + default: 1000 + + ICE_STRT_RETRY: + description: + short: Number of retries for starting engine + long: TBD + type: int32 + min: 0 + max: 10 + default: 3 + + ICE_RETRY_FAULT: + description: + short: Try to re-start engine if it stops + long: TBD + type: boolean + default: false + + 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 + long: TBD + type: boolean + default: false + + 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 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 3ff1c321e79b..7aeada00920d 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -80,6 +80,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);