ICE: add slew rate to throttle

Signed-off-by: Silvan Fuhrer <[email protected]>
PX4 · Nov 29, 2024 · a6809e7 · a6809e7
1 parent 72a6bb6
commit a6809e7
Show file tree

Hide file tree

Showing 5 changed files with 35 additions and 2 deletions.
diff --git a/msg/InternalCombustionEngineControl.msg b/msg/InternalCombustionEngineControl.msg
@@ -1,7 +1,7 @@
 uint64 timestamp        		# time since system start (microseconds)
 
 bool ignition_on          		# activate/deactivate ignition
-float32 throttle_control		# [0,1] - Motor should idle with 0
+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]
 

diff --git a/src/modules/internal_combustion_engine_control/CMakeLists.txt b/src/modules/internal_combustion_engine_control/CMakeLists.txt
@@ -42,4 +42,5 @@ px4_add_module(
 		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
@@ -89,6 +89,7 @@ void InternalCombustionEngineControl::Run()
 
 		// 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
@@ -221,6 +222,18 @@ void InternalCombustionEngineControl::Run()
 
 	const hrt_abstime now = hrt_absolute_time();
 
+	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(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<uint8_t>(user_request);
 	_internal_combustion_engine_control_pub.publish(ice_control);

diff --git a/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp
@@ -48,6 +48,8 @@
 #include <uORB/topics/rpm.h>
 #include <uORB/topics/actuator_motors.h>
 
+#include <lib/slew_rate/SlewRate.hpp>
+
 namespace internal_combustion_engine_control
 {
 
@@ -99,9 +101,12 @@ class InternalCombustionEngineControl : public ModuleBase<InternalCombustionEngi
 	};
 
 	hrt_abstime _state_start_time{0};
+	hrt_abstime _last_time_run{0};
 	int _starting_retry_cycle{0};
 	bool engine_tried_to_restart{false};
 
+	SlewRate<float> _throttle_control_slew_rate;
+
 	bool isEngineRunning();
 	void instantiateEngineStart();
 	void controlEngineRunning(internal_combustion_engine_control_s &ice_control, float throttle_in);
@@ -117,7 +122,8 @@ class InternalCombustionEngineControl : public ModuleBase<InternalCombustionEngi
 		(ParamInt<px4::params::ICE_STRT_RETRY>) _param_ice_strt_retry,
 		(ParamInt<px4::params::ICE_RETRY_FAULT>) _param_ice_retry_fault,
 		(ParamFloat<px4::params::ICE_STRT_THR>) _param_ice_strt_thr,
-		(ParamInt<px4::params::ICE_STOP_CHOKE>) _param_ice_stop_choke
+		(ParamInt<px4::params::ICE_STOP_CHOKE>) _param_ice_stop_choke,
+		(ParamFloat<px4::params::ICE_THR_SLEW>) _param_ice_thr_slew
 	)
 };
 

diff --git a/src/modules/internal_combustion_engine_control/module.yaml b/src/modules/internal_combustion_engine_control/module.yaml
@@ -95,3 +95,16 @@ parameters:
           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