mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-03-24 10:34:00 +08:00
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 <silvan@auterion.com> Co-authored-by: Pernilla <pernilla@auterion.com>
This commit is contained in:
Silvan Fuhrer
GitHub
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
#
|
||||
|
||||
@@ -114,6 +114,7 @@ set(msg_files
|
||||
HeaterStatus.msg
|
||||
HoverThrustEstimate.msg
|
||||
InputRc.msg
|
||||
InternalCombustionEngineControl.msg
|
||||
InternalCombustionEngineStatus.msg
|
||||
IridiumsbdStatus.msg
|
||||
IrlockReport.msg
|
||||
|
||||
8
msg/InternalCombustionEngineControl.msg
Normal file
8
msg/InternalCombustionEngineControl.msg
Normal file
@@ -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
|
||||
@@ -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
|
||||
|
||||
83
src/lib/mixer_module/functions/FunctionICEngineControl.hpp
Normal file
83
src/lib/mixer_module/functions/FunctionICEngineControl.hpp
Normal file
@@ -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 <uORB/topics/internal_combustion_engine_control.h>
|
||||
|
||||
/**
|
||||
* 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] {};
|
||||
};
|
||||
@@ -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,
|
||||
|
||||
@@ -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"
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
)
|
||||
@@ -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 <px4_platform_common/events.h>
|
||||
|
||||
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<ICESource>(_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<float>((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<uint8_t>(user_request);
|
||||
_internal_combustion_engine_control_pub.publish(ice_control);
|
||||
|
||||
internal_combustion_engine_status_s ice_status;
|
||||
ice_status.state = static_cast<uint8_t>(_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.:
|
||||
|
||||
|
||||
### 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:
|
||||
|
||||
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)
|
||||
<a id="internal_combustion_engine_control_usage"></a>
|
||||
)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
|
||||
@@ -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 <drivers/drv_hrt.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/internal_combustion_engine_control.h>
|
||||
#include <uORB/topics/internal_combustion_engine_status.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/rpm.h>
|
||||
#include <uORB/topics/actuator_motors.h>
|
||||
|
||||
#include <lib/slew_rate/SlewRate.hpp>
|
||||
|
||||
namespace internal_combustion_engine_control
|
||||
{
|
||||
|
||||
class InternalCombustionEngineControl : public ModuleBase<InternalCombustionEngineControl>, 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_s> _internal_combustion_engine_control_pub{ORB_ID(internal_combustion_engine_control)};
|
||||
uORB::Publication<internal_combustion_engine_status_s> _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<float> _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<px4::params::ICE_ON_SOURCE>) _param_ice_on_source,
|
||||
(ParamFloat<px4::params::ICE_CHOKE_ST_DUR>) _param_ice_choke_st_dur,
|
||||
(ParamFloat<px4::params::ICE_STRT_DUR>) _param_ice_strt_dur,
|
||||
(ParamFloat<px4::params::ICE_MIN_RUN_RPM>) _param_ice_min_run_rpm,
|
||||
(ParamInt<px4::params::ICE_STRT_ATTEMPT>) _param_ice_strt_attempts,
|
||||
(ParamInt<px4::params::ICE_RUN_FAULT_D>) _param_ice_running_fault_detection,
|
||||
(ParamFloat<px4::params::ICE_STRT_THR>) _param_ice_strt_thr,
|
||||
(ParamInt<px4::params::ICE_STOP_CHOKE>) _param_ice_stop_choke,
|
||||
(ParamFloat<px4::params::ICE_THR_SLEW>) _param_ice_thr_slew,
|
||||
(ParamFloat<px4::params::ICE_IGN_DELAY>) _param_ice_ign_delay
|
||||
)
|
||||
};
|
||||
|
||||
} // namespace internal_combustion_engine_control
|
||||
5
src/modules/internal_combustion_engine_control/Kconfig
Normal file
5
src/modules/internal_combustion_engine_control/Kconfig
Normal file
@@ -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
|
||||
120
src/modules/internal_combustion_engine_control/module.yaml
Normal file
120
src/modules/internal_combustion_engine_control/module.yaml
Normal file
@@ -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
|
||||
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user