diff --git a/boards/auterion/fmu-v6s/default.px4board b/boards/auterion/fmu-v6s/default.px4board index 35af2a295ec..4164d2a1034 100644 --- a/boards/auterion/fmu-v6s/default.px4board +++ b/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 diff --git a/msg/InternalCombustionEngineStatus.msg b/msg/InternalCombustionEngineStatus.msg index 301eb92a861..aec9fd542aa 100644 --- a/msg/InternalCombustionEngineStatus.msg +++ b/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 diff --git a/src/lib/pid/PID.cpp b/src/lib/pid/PID.cpp index ed3a345cf66..555175215cb 100644 --- a/src/lib/pid/PID.cpp +++ b/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)) { diff --git a/src/lib/pid/PID.hpp b/src/lib/pid/PID.hpp index 526abccf4c8..4264f7027fd 100644 --- a/src/lib/pid/PID.hpp +++ b/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}; }; diff --git a/src/lib/pid/PIDTest.cpp b/src/lib/pid/PIDTest.cpp index 0fcbf459e8f..b164c7dc7c0 100644 --- a/src/lib/pid/PIDTest.cpp +++ b/src/lib/pid/PIDTest.cpp @@ -34,6 +34,11 @@ #include #include +// 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 diff --git a/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp index d74c37122e0..95287e061ff 100644 --- a/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp +++ b/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(_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(_state); + ice_status.substate = static_cast(_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() diff --git a/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp index d78f8ee758d..f97888ce211 100644 --- a/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp +++ b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp @@ -49,6 +49,7 @@ #include #include +#include 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 _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) _param_ice_strt_thr, (ParamInt) _param_ice_stop_choke, (ParamFloat) _param_ice_thr_slew, - (ParamFloat) _param_ice_ign_delay + (ParamFloat) _param_ice_ign_delay, + (ParamFloat) _param_ice_idle_thr_ff, + (ParamFloat) _param_ice_idle_rpm, + (ParamFloat) _param_ice_idle_rpm_p, + (ParamFloat) _param_ice_idle_rpm_i ) }; diff --git a/src/modules/internal_combustion_engine_control/module.yaml b/src/modules/internal_combustion_engine_control/module.yaml index 4e3ca311cd3..6e25698fffe 100644 --- a/src/modules/internal_combustion_engine_control/module.yaml +++ b/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