feat(ice_controller): add a FF-PI controlled idle RPM governor (#27650)

Disabled if ICE_IDLE_RPM is 0. 
The idle state is entered when the commanded throttle is close to zero
or the measured RPM drops below the idle threshold.
The idle state is exited if the commanded thrust is increased to above 
the last needed throttle to keep idle. 

Signed-off-by: Silvan <silvan@auterion.com>
Co-authored-by: Silvan <silvan@auterion.com>

boards/auterion/fmu-v6s/default.px4board

@@ -98,3 +98,6 @@ CONFIG_SYSTEMCMDS_WORK_QUEUE=y
 CONFIG_DRIVERS_SERIALPASSTHROUGH=y
 CONFIG_SERIALPASSTHROUGH_BITBANG=y
 CONFIG_WQ_TTY_STACKSIZE=2500
+
+CONFIG_MODULES_INTERNAL_COMBUSTION_ENGINE_CONTROL=y
+CONFIG_DRIVERS_RPM_CAPTURE=y

msg/InternalCombustionEngineStatus.msg

@@ -1,10 +1,15 @@
-uint64 timestamp					# time since system start (microseconds)
+uint64 timestamp					# [us] Time since system start
 
 uint8 STATE_STOPPED = 0					# The engine is not running. This is the default state.
 uint8 STATE_STARTING = 1				# The engine is starting. This is a transient state.
 uint8 STATE_RUNNING = 2					# The engine is running normally.
 uint8 STATE_FAULT = 3					# The engine can no longer function.
-uint8 state
+uint8 state						# [@enum STATE] Engine state
+
+uint8 SUBSTATE_RUN = 0					# The engine is running. This is the default state.
+uint8 SUBSTATE_IDLE = 1					# The engine idle rpm controller is running.
+uint8 SUBSTATE_REST = 2					# The engine is at rest.
+uint8 substate						# [@enum SUBSTATE] Engine substate
 
 uint32 FLAG_GENERAL_ERROR = 1				# General error.
 
@@ -33,32 +38,34 @@ uint32 FLAG_OIL_PRESSURE_ABOVE_NOMINAL = 131072  	# Over-pressure warning
 
 uint32 FLAG_DEBRIS_SUPPORTED = 262144			# Debris warning. This flag is optional
 uint32 FLAG_DEBRIS_DETECTED = 524288  			# Detection of debris warning
-uint32 flags
+uint32 flags						# [@enum FLAG] Engine status flags bitmap
 
-uint8 engine_load_percent				# Engine load estimate, percent, [0, 127]
-uint32 engine_speed_rpm					# Engine speed, revolutions per minute
-float32 spark_dwell_time_ms 				# Spark dwell time, millisecond
-float32 atmospheric_pressure_kpa			# Atmospheric (barometric) pressure, kilopascal
-float32 intake_manifold_pressure_kpa			# Engine intake manifold pressure, kilopascal
-float32 intake_manifold_temperature			# Engine intake manifold temperature, kelvin
-float32 coolant_temperature				# Engine coolant temperature, kelvin
-float32 oil_pressure					# Oil pressure, kilopascal
-float32 oil_temperature					# Oil temperature, kelvin
-float32 fuel_pressure					# Fuel pressure, kilopascal
-float32 fuel_consumption_rate_cm3pm			# Instant fuel consumption estimate, (centimeter^3)/minute
-float32 estimated_consumed_fuel_volume_cm3		# Estimate of the consumed fuel since the start of the engine, centimeter^3
-uint8 throttle_position_percent				# Throttle position, percent
-uint8 ecu_index						# The index of the publishing ECU
+uint8 engine_load_percent				# [%] [@range 0, 127] Engine load estimate
+uint32 engine_speed_rpm					# [rpm] [@range 0, inf] Engine speed
+float32 spark_dwell_time_ms 				# [ms] [@range 0, inf] Spark dwell time
+float32 atmospheric_pressure_kpa			# [kPa] [@range 0, inf] Atmospheric (barometric) pressure
+float32 intake_manifold_pressure_kpa			# [kPa] [@range 0, inf] Engine intake manifold pressure
+float32 intake_manifold_temperature			# [K] [@range 0, inf] Engine intake manifold temperature
+float32 coolant_temperature				# [K] [@range 0, inf] Engine coolant temperature
+float32 oil_pressure					# [kPa] [@range 0, inf] Oil pressure
+float32 oil_temperature					# [K] [@range 0, inf] Oil temperature
+float32 fuel_pressure					# [kPa] [@range 0, inf] Fuel pressure
+float32 fuel_consumption_rate_cm3pm			# [cm^3/min] [@range 0, inf] Instant fuel consumption estimate
+float32 estimated_consumed_fuel_volume_cm3		# [cm^3] [@range 0, inf] Estimate of the consumed fuel since the start of the engine
+uint8 throttle_position_percent				# [%] [@range 0, 100] Throttle position
+uint8 ecu_index						# [-] [@range 0, 255] The index of the publishing ECU
 
 
 uint8 SPARK_PLUG_SINGLE         = 0
 uint8 SPARK_PLUG_FIRST_ACTIVE   = 1
 uint8 SPARK_PLUG_SECOND_ACTIVE  = 2
 uint8 SPARK_PLUG_BOTH_ACTIVE    = 3
-uint8 spark_plug_usage					# Spark plug activity report.
+uint8 spark_plug_usage					# [@enum SPARK_PLUG] Spark plug activity report
 
-float32 ignition_timing_deg				# Cylinder ignition timing, angular degrees of the crankshaft
-float32 injection_time_ms				# Fuel injection time, millisecond
-float32 cylinder_head_temperature			# Cylinder head temperature (CHT), kelvin
-float32 exhaust_gas_temperature				# Exhaust gas temperature (EGT), kelvin
-float32 lambda_coefficient				# Estimated lambda coefficient, dimensionless ratio
+float32 ignition_timing_deg				# [deg] [@range -inf, inf] Cylinder ignition timing, angular degrees of the crankshaft
+float32 injection_time_ms				# [ms] [@range 0, inf] Fuel injection time
+float32 cylinder_head_temperature			# [K] [@range 0, inf] Cylinder head temperature (CHT)
+float32 exhaust_gas_temperature				# [K] [@range 0, inf] Exhaust gas temperature (EGT)
+float32 lambda_coefficient				# [-] [@range 0, inf] Estimated lambda coefficient, dimensionless ratio
+
+float32 pid_idle_rpm_integral				# [-] [@range -1, 1] Integral of the PID for the closed loop idle RPM controller

src/lib/pid/PID.cpp

@@ -34,6 +34,12 @@
 #include "PID.hpp"
 #include "lib/mathlib/math/Functions.hpp"
 
+void PID::setOutputLimit(const float lower_limit, const float upper_limit)
+{
+	_lower_limit_output = lower_limit;
+	_upper_limit_output = upper_limit;
+}
+
 void PID::setGains(const float P, const float I, const float D)
 {
 	_gain_proportional = P;
@@ -44,18 +50,27 @@ void PID::setGains(const float P, const float I, const float D)
 float PID::update(const float feedback, const float dt, const bool update_integral)
 {
 	const float error = _setpoint - feedback;
-	const float output = (_gain_proportional * error) + _integral - (_gain_derivative * updateDerivative(feedback, dt));
+	const float derivative = updateDerivative(feedback, dt);
+	const float output = (_gain_feedforward * _setpoint) + (_gain_proportional * error) + _integral -
+			     (_gain_derivative * derivative);
+	const float output_constrained = math::constrain(output, _lower_limit_output, _upper_limit_output);
 
 	if (update_integral) {
-		updateIntegral(error, dt);
+		updateIntegral(error, output - output_constrained, dt);
 	}
 
 	_last_feedback = feedback;
-	return math::constrain(output, -_limit_output, _limit_output);
+	return output_constrained;
 }
 
-void PID::updateIntegral(float error, const float dt)
+void PID::updateIntegral(float error, const float saturation, const float dt)
 {
+	// Anti-windup: stop integrating in the direction that drives the output
+	// further into saturation (conditional integration)
+	if ((saturation > FLT_EPSILON && error > 0.f) || (saturation < -FLT_EPSILON && error < 0.f)) {
+		error = 0.f;
+	}
+
 	const float integral_new = _integral + _gain_integral * error * dt;
 
 	if (std::isfinite(integral_new)) {

src/lib/pid/PID.hpp

@@ -40,16 +40,18 @@ class PID
 public:
 	PID() = default;
 	virtual ~PID() = default;
-	void setOutputLimit(const float limit) { _limit_output = limit; }
+	void setOutputLimit(const float limit) {setOutputLimit(-limit, limit);}
+	void setOutputLimit(const float lower_limit, const float upper_limit);
 	void setIntegralLimit(const float limit) { _limit_integral = limit; }
 	void setGains(const float P, const float I, const float D);
 	void setSetpoint(const float setpoint) { _setpoint = setpoint; }
+	void setFeedForwardGain(const float feedforward_gain) { _gain_feedforward = feedforward_gain; }
 	float update(const float feedback, const float dt, const bool update_integral = true);
 	float getIntegral() { return _integral; }
 	void resetIntegral() { _integral = 0.f; }
 	void resetDerivative() { _last_feedback = NAN; }
 private:
-	void updateIntegral(float error, const float dt);
+	void updateIntegral(float error, const float saturation, const float dt);
 	float updateDerivative(float feedback, const float dt);
 
 	float _setpoint{0.f}; ///< current setpoint to track
@@ -57,9 +59,11 @@ private:
 	float _last_feedback{NAN};
 
 	// Gains, Limits
+	float _gain_feedforward{0.f};
 	float _gain_proportional{0.f};
 	float _gain_integral{0.f};
 	float _gain_derivative{0.f};
 	float _limit_integral{0.f};
-	float _limit_output{0.f};
+	float _lower_limit_output{0.f};
+	float _upper_limit_output{0.f};
 };

src/lib/pid/PIDTest.cpp

@@ -34,6 +34,11 @@
 #include <gtest/gtest.h>
 #include <PID.hpp>
 
+// Run all PID unit tests:
+//   make tests TESTFILTER=unit-PID
+// Run a subset directly (after building) by filtering gtest cases, e.g.:
+//   ./build/px4_sitl_test/unit-PID --gtest_filter=PIDTest.AntiWindup*
+
 TEST(PIDTest, AllZeroCase)
 {
 	PID pid;
@@ -129,6 +134,52 @@ TEST(PIDTest, InteralOpenLoop)
 	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.01f);
 }
 
+TEST(PIDTest, AntiWindupHighSaturation)
+{
+	// Integrator-only controller whose output saturates before the integral limit.
+	PID pid;
+	pid.setGains(0.f, .1f, 0.f);
+	pid.setIntegralLimit(1.f); // large, so the output limit saturates first
+	pid.setOutputLimit(.05f);
+	pid.setSetpoint(1.f);
+
+	// Output uses the pre-update integral, so the first step is not yet saturated.
+	EXPECT_FLOAT_EQ(pid.update(0.f, 1.f, true), 0.f);
+	EXPECT_FLOAT_EQ(pid.getIntegral(), .1f);
+
+	// Output now saturates high; integrating further up would wind up -> frozen.
+	EXPECT_FLOAT_EQ(pid.update(0.f, 1.f, true), .05f);
+	EXPECT_FLOAT_EQ(pid.getIntegral(), .1f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 1.f, true), .05f);
+	EXPECT_FLOAT_EQ(pid.getIntegral(), .1f);
+
+	// Error reverses: still saturated high, but integrating down unwinds -> allowed.
+	pid.setSetpoint(-1.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 1.f, true), .05f);
+	EXPECT_FLOAT_EQ(pid.getIntegral(), 0.f);
+}
+
+TEST(PIDTest, AntiWindupLowSaturation)
+{
+	PID pid;
+	pid.setGains(0.f, .1f, 0.f);
+	pid.setIntegralLimit(1.f);
+	pid.setOutputLimit(.05f);
+	pid.setSetpoint(-1.f);
+
+	EXPECT_FLOAT_EQ(pid.update(0.f, 1.f, true), 0.f);
+	EXPECT_FLOAT_EQ(pid.getIntegral(), -.1f);
+
+	// Output saturates low; integrating further down would wind up -> frozen.
+	EXPECT_FLOAT_EQ(pid.update(0.f, 1.f, true), -.05f);
+	EXPECT_FLOAT_EQ(pid.getIntegral(), -.1f);
+
+	// Error reverses: still saturated low, but integrating up unwinds -> allowed.
+	pid.setSetpoint(1.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 1.f, true), -.05f);
+	EXPECT_FLOAT_EQ(pid.getIntegral(), 0.f);
+}
+
 TEST(PIDTest, DerivativeOnlyDampsFeedbackMotion)
 {
 	// Derivative-on-measurement: the D term must oppose feedback motion so the

src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp

@@ -93,7 +93,16 @@ void InternalCombustionEngineControl::Run()
 
 		// update parameters from storage
 		updateParams();
+		// Set up slew rate
 		_throttle_control_slew_rate.setSlewRate(_param_ice_thr_slew.get());
+		// Set up idle PID controller
+		const float idle_rpm = _param_ice_idle_rpm.get();
+		_rpm_idle_pid.setSetpoint(idle_rpm);
+		_rpm_idle_pid.setGains(_param_ice_idle_rpm_p.get() * 1e-3f, _param_ice_idle_rpm_i.get() * 1e-3f, 0.f);
+		// Feed-forward maps the RPM setpoint to the base idle throttle, so FF = idle_thr at the setpoint
+		_rpm_idle_pid.setFeedForwardGain(idle_rpm > FLT_EPSILON ? _param_ice_idle_thr_ff.get() / idle_rpm : 0.f);
+		_rpm_idle_pid.setIntegralLimit(1.f);
+		_rpm_idle_pid.setOutputLimit(0.f, 1.f);
 	}
 
 
@@ -110,6 +119,8 @@ void InternalCombustionEngineControl::Run()
 
 	const hrt_abstime now = hrt_absolute_time();
 
+	rpmSubUpdate(now);
+
 	switch (static_cast<ICESource>(_param_ice_on_source.get())) {
 	case ICESource::ArmingState: {
 			_user_request = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? UserOnOffRequest::On :
@@ -166,11 +177,11 @@ void InternalCombustionEngineControl::Run()
 
 			} else {
 
-				switch (_starting_sub_state) {
+				switch (_sub_state) {
 				case SubState::Rest: {
 						if (isStartingPermitted(now)) {
 							_state_start_time = now;
-							_starting_sub_state = SubState::Run;
+							_sub_state = SubState::Run;
 						}
 					}
 					break;
@@ -179,8 +190,9 @@ void InternalCombustionEngineControl::Run()
 				default: {
 						controlEngineStartup(now);
 
-						if (isEngineRunning(now)) {
+						if (_is_engine_running) {
 							_state = State::Running;
+							_rpm_idle_pid.resetIntegral();
 							PX4_INFO("ICE: Starting finished");
 
 						} else {
@@ -191,7 +203,7 @@ void InternalCombustionEngineControl::Run()
 
 							} else if (!isStartingPermitted(now)) {
 								controlEngineStop();
-								_starting_sub_state = SubState::Rest;
+								_sub_state = SubState::Rest;
 							}
 						}
 
@@ -205,21 +217,43 @@ void InternalCombustionEngineControl::Run()
 		break;
 
 	case State::Running: {
-			controlEngineRunning(throttle_in);
+
+			// enter idle state if the RPM governor is enabled and either the throttle is zero or the RPM is below the idle setpoint
+			const bool rpm_governor_enabled = _param_ice_idle_rpm.get() > FLT_EPSILON;
+			const bool zero_throttle = throttle_in < .02f || !PX4_ISFINITE(throttle_in);
+			const bool rpm_below_min = _rpm_estimate < _param_ice_idle_rpm.get();
+
+			if (_sub_state != SubState::Idle && rpm_governor_enabled && (zero_throttle || rpm_below_min)) {
+				_sub_state = SubState::Idle;
+				// Seed the PID timestamp so the first idle step gets a near-zero dt instead
+				// of a stale-timestamp spike in the integrator
+				_timestamp_last_idle_throttle_update = now;
+			}
+
+			if (_sub_state == SubState::Idle && throttle_in > _idle_throttle) {
+				_sub_state = SubState::Run;
+			}
+
+			if (_sub_state == SubState::Idle) {
+				controlEngineIdle(now);
+
+			} else {
+				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()) {
+			} else if (!_is_engine_running && _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,
+				events::send(events::ID("internal_combustion_engine_running_fault"), events::Log::Critical,
 					     "IC engine fault detected");
 			}
 		}
@@ -270,22 +304,36 @@ void InternalCombustionEngineControl::publishControl(const hrt_abstime now)
 
 	internal_combustion_engine_status_s ice_status;
 	ice_status.state = static_cast<uint8_t>(_state);
+	ice_status.substate = static_cast<uint8_t>(_sub_state);
 	ice_status.timestamp = now;
+	ice_status.pid_idle_rpm_integral = _rpm_idle_pid.getIntegral();
 	_internal_combustion_engine_status_pub.publish(ice_status);
 }
 
-bool InternalCombustionEngineControl::isEngineRunning(const hrt_abstime now)
+void InternalCombustionEngineControl::rpmSubUpdate(const hrt_abstime now)
 {
 	rpm_s rpm;
 
-	if (_rpm_sub.copy(&rpm)) {
-		const hrt_abstime rpm_timestamp = rpm.timestamp;
+	if (_rpm_sub.update(&rpm)) {
+		_rpm_timestamp = rpm.timestamp;
+		_rpm_estimate = rpm.rpm_estimate;
 
-		return (_param_ice_min_run_rpm.get() > FLT_EPSILON && (now < rpm_timestamp + 2_s)
-			&& rpm.rpm_estimate > _param_ice_min_run_rpm.get());
+		_is_engine_running =
+			(_param_ice_min_run_rpm.get() > FLT_EPSILON &&
+			 (now < _rpm_timestamp + 2_s) && _rpm_estimate > _param_ice_min_run_rpm.get());
 	}
+}
 
-	return false;
+void InternalCombustionEngineControl::controlEngineIdle(const hrt_abstime now)
+{
+	const float dt = math::min((now - _timestamp_last_idle_throttle_update) * 1e-6f, 1.f);
+	_idle_throttle = _rpm_idle_pid.update(_rpm_estimate, dt, true);
+	_timestamp_last_idle_throttle_update = now;
+
+	_ignition_on = true;
+	_choke_control = 0.f;
+	_starter_engine_control = 0.f;
+	_throttle_control = _idle_throttle;
 }
 
 void InternalCombustionEngineControl::controlEngineRunning(float throttle_in)
@@ -294,7 +342,6 @@ void InternalCombustionEngineControl::controlEngineRunning(float throttle_in)
 	_choke_control = 0.f;
 	_starter_engine_control = 0.f;
 	_throttle_control = throttle_in;
-
 }
 
 void InternalCombustionEngineControl::controlEngineStop()

src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp

@@ -49,6 +49,7 @@
 #include <uORB/topics/actuator_motors.h>
 
 #include <lib/slew_rate/SlewRate.hpp>
+#include <lib/pid/PID.hpp>
 
 namespace internal_combustion_engine_control
 {
@@ -98,6 +99,7 @@ private:
 
 	enum class SubState {
 		Run,
+		Idle,
 		Rest
 	};
 
@@ -115,16 +117,25 @@ private:
 
 	hrt_abstime _state_start_time{0};
 	hrt_abstime _last_time_run{0};
+	hrt_abstime _rpm_timestamp{0};
 
 	bool _ignition_on{false};
+	bool _is_engine_running{false};
+	bool _idle_control_active{false};
 	float _choke_control{1.f};
 	float _starter_engine_control{0.f};
 	float _throttle_control{0.f};
+	float _idle_throttle{0.f};
+	float _rpm_estimate{0.f};
+	hrt_abstime _timestamp_last_idle_throttle_update{0};
 
 	SlewRate<float> _throttle_control_slew_rate;
+	SubState _sub_state{SubState::Run};
+	PID _rpm_idle_pid; // Output in [0,1]
 
-	bool isEngineRunning(const hrt_abstime now);
+	void rpmSubUpdate(const hrt_abstime now);
 	void controlEngineRunning(float throttle_in);
+	void controlEngineIdle(const hrt_abstime now);
 	void controlEngineStop();
 	void controlEngineStartup(const hrt_abstime now);
 	void controlEngineFault();
@@ -135,7 +146,6 @@ private:
 	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
@@ -155,7 +165,11 @@ private:
 		(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
+		(ParamFloat<px4::params::ICE_IGN_DELAY>) _param_ice_ign_delay,
+		(ParamFloat<px4::params::ICE_IDLE_THR_FF>) _param_ice_idle_thr_ff,
+		(ParamFloat<px4::params::ICE_IDLE_RPM>) _param_ice_idle_rpm,
+		(ParamFloat<px4::params::ICE_IDLE_RPM_P>) _param_ice_idle_rpm_p,
+		(ParamFloat<px4::params::ICE_IDLE_RPM_I>) _param_ice_idle_rpm_i
 	)
 };
 

src/modules/internal_combustion_engine_control/module.yaml

@@ -119,3 +119,52 @@ parameters:
         decimal: 1
         increment: 0.1
         default: 0
+
+      ICE_IDLE_RPM:
+        description:
+          short: Idle RPM setpoint for the engine.
+          long: |
+              Idle RPM setpoint for the engine. Applies a controller to prevent the RPM do drop below this value.
+        type: float
+        unit: rpm
+        min: 0
+        max: 10000
+        decimal: 0
+        increment: 1
+        default: 0.0
+
+      ICE_IDLE_RPM_P:
+        description:
+          short: Proportional gain for idle RPM control
+          long: |
+            Ratio between RPM error devided by 1000 to how much normalized output gets added to correct for it.
+        type: float
+        decimal: 3
+        increment: 0.1
+        min: 0
+        max: 10
+        default: 0.0
+
+      ICE_IDLE_RPM_I:
+        description:
+          short: Integral gain for idle RPM control
+          long: |
+            Ratio between integrated RPM error devided by 1000 to how much normalized output gets added to correct for it.
+        type: float
+        decimal: 3
+        increment: 0.1
+        min: 0
+        max: 10
+        default: 0.0
+
+      ICE_IDLE_THR_FF:
+        description:
+          short: Idle RPM throttle for feed-forward
+          long: |
+            Used as feed-forward. Should match approximately the throttle required to maintain the ICE_IDLE_RPM.
+        type: float
+        decimal: 2
+        increment: 0.01
+        min: 0
+        max: 1
+        default: 0.0