diff --git a/firmware/controllers/algo/engine_configuration.cpp b/firmware/controllers/algo/engine_configuration.cpp index f8c7b50e78..9250760057 100644 --- a/firmware/controllers/algo/engine_configuration.cpp +++ b/firmware/controllers/algo/engine_configuration.cpp @@ -622,6 +622,8 @@ static void setDefaultEngineConfiguration() { // https://github.com/rusefi/rusefi/issues/4030 engineConfiguration->mapErrorDetectionTooHigh = 410; + setLinearCurve(config->throttleEstimateEffectiveAreaBins, 0, 100); + #endif // EFI_ENGINE_CONTROL #include "default_script.lua" } diff --git a/firmware/controllers/math/math.mk b/firmware/controllers/math/math.mk index 3982378c95..cbd543c72c 100644 --- a/firmware/controllers/math/math.mk +++ b/firmware/controllers/math/math.mk @@ -4,4 +4,5 @@ CONTROLLERS_MATH_SRC_CPP = $(PROJECT_DIR)/controllers/math/engine_math.cpp \ $(PROJECT_DIR)/controllers/math/speed_density.cpp \ $(PROJECT_DIR)/controllers/math/closed_loop_fuel.cpp \ $(PROJECT_DIR)/controllers/math/closed_loop_fuel_cell.cpp \ + $(PROJECT_DIR)/controllers/math/throttle_model.cpp \ diff --git a/firmware/controllers/math/throttle_model.cpp b/firmware/controllers/math/throttle_model.cpp new file mode 100644 index 0000000000..67d7d01ed6 --- /dev/null +++ b/firmware/controllers/math/throttle_model.cpp @@ -0,0 +1,130 @@ +#include "pch.h" + +#include "throttle_model.h" + +static const float pressureRatioCorrectionBins[] = { 0.53125, 0.546875, 0.5625, 0.578125, 0.59375, 0.609375, 0.625, 0.640625, 0.65625, 0.671875, 0.6875, 0.703125, 0.71875, 0.734375, 0.750, 0.765625, 0.78125, 0.796875, 0.8125, 0.828125, 0.84375, 0.859375, 0.875, 0.890625, 0.90625, 0.921875, 0.9375, 0.953125 }; +static const float pressureRatioCorrectionValues[] = { 1, 0.9993, 0.998, 0.995, 0.991, 0.986, 0.979, 0.972, 0.963, 0.953, 0.942, 0.930, 0.916, 0.901, 0.884, 0.866, 0.845, 0.824, 0.800, 0.774, 0.745, 0.714, 0.679, 0.642, 0.600, 0.553, 0.449, 0.449 }; +static float pressureRatioFlowCorrection(float pr) { + if (pr < 0.531) { + return 1.0; + } + + // float x = pr; + // float x2 = x * x; + // float x3 = x2 * x; + // return -6.9786 * x3 + 11.597 * x2 - 6.7227 * x + 2.3509; + + return interpolate2d(pr, pressureRatioCorrectionBins, pressureRatioCorrectionValues); +} + +static float flowCorrections(float pressureRatio, float p_up, float iat) { + // PR correction + float prCorrectionFactor = pressureRatioFlowCorrection(pressureRatio); + + // Inlet density correction + float tempCorrection = sqrt(273 / (iat + 273)); + float pressureCorrection = p_up / 101.325; + float densityCorrection = tempCorrection * pressureCorrection; + + return prCorrectionFactor * densityCorrection; +} + +float ThrottleModelBase::partThrottleFlow(float tps, float flowCorrection) const { + return effectiveArea(tps) * flowCorrection; +} + +float ThrottleModelBase::partThrottleFlow(float tps, float pressureRatio, float p_up, float iat) const { + return partThrottleFlow(tps, flowCorrections(pressureRatio, p_up, iat)); +} + +class InverseFlowSolver : public NewtonsMethodSolver { +public: + InverseFlowSolver(const ThrottleModelBase* model, float target, float pressureRatio, float p_up, float iat) + : m_model(*model) + , m_flowCorrection(flowCorrections(pressureRatio, p_up, iat)) + , m_target(target) + { + } + +private: + const ThrottleModelBase& m_model; + const float m_flowCorrection; + const float m_target; + + float fx(float x) override { + // Return 0 when the estimate equals the target, positive when estimate too large + return m_model.partThrottleFlow(x, m_flowCorrection) - m_target; + } + + float dfx(float x) override { + // The marginal flow per angle (dFlow/dTPS) is not trivially differentiable, + // but it is continuous, so we can use a finite difference approximation over some + // "small" step size (0.1 degree ~= 0 for throttle purposes) + // Too small a step may provoke numerical instability. + return (fx(x + 0.1) - fx(x - 0.1)) / 0.2; + } +}; + +// Find the throttle position that gives the specified flow +float ThrottleModelBase::inversePartThrottleFlow(float maxEngineFlow, float flow, float pressureRatio, float p_up, float iat) const { + // TODO: handle near-WOT flow gracefully + + // If over 95% of the wide open flow, return wide open + if (flow > 0.95f * maxEngineFlow) { + return 100; + } + + InverseFlowSolver solver(this, flow, pressureRatio, p_up, iat); + return solver.solve(50, 0.1).value_or(0); +} + +float ThrottleModelBase::estimateThrottleFlow(float tip, float tps, float map, float iat) { + // What output flow do we get at 0.95 PR? + constexpr float crossoverPr = 0.95f; + float p95Flow = maxEngineFlow(tip * crossoverPr); + + // Maximum flow if the throttle was removed + float maximumPossibleFlow = maxEngineFlow(tip); + + // What throttle position gives us that flow at 0.95 PR? + float throttleAngle95Pr = inversePartThrottleFlow(maximumPossibleFlow, p95Flow, crossoverPr, tip, iat); + + if (tps > throttleAngle95Pr) { + // "WOT" model + + // Linearly interpolate between the P95 point and wide open, where the engine flows its max + return interpolateClamped(throttleAngle95Pr, p95Flow, 100, maximumPossibleFlow, tps); + } else { + float pressureRatio = map / tip; + + return partThrottleFlow(tps, pressureRatio, tip, iat); + } +} + +expected ThrottleModelBase::estimateThrottleFlow(float map, float tps) { + // Inputs + auto iat = Sensor::get(SensorType::Iat); + + // Use TIP sensor + // or use Baro sensor if no TIP + // or use 101.325kPa (std atmosphere) if no Baro + // TODO: have a real TIP sensor + auto tip = Sensor::hasSensor(SensorType::AuxLinear1) ? Sensor::get(SensorType::AuxLinear1) : + Sensor::hasSensor(SensorType::BarometricPressure) ? Sensor::get(SensorType::BarometricPressure) : + SensorResult(101.325f); + + if (!tip || !iat) { + return unexpected; + } + + return estimateThrottleFlow(tip.Value, tps, map, iat.Value); +} + +float ThrottleModel::effectiveArea(float tps) const { + return interpolate2d(tps, config->throttleEstimateEffectiveAreaBins, config->throttleEstimateEffectiveAreaValues); +} + +float ThrottleModel::maxEngineFlow(float map) const { + // TODO: implement this for real by consulting VE table, etc + return 0.5f; +} diff --git a/firmware/controllers/math/throttle_model.h b/firmware/controllers/math/throttle_model.h new file mode 100644 index 0000000000..42c0060226 --- /dev/null +++ b/firmware/controllers/math/throttle_model.h @@ -0,0 +1,25 @@ +#pragma once + +struct ThrottleModelBase { +public: + float estimateThrottleFlow(float tip, float tps, float map, float iat); + expected estimateThrottleFlow(float map, float tps); + + float partThrottleFlow(float tps, float flowCorrection) const; + float partThrottleFlow(float tps, float pressureRatio, float p_up, float iat) const; + +protected: + // Given some TPS, what is the normalized choked flow in kg/s? + virtual float effectiveArea(float tps) const = 0; + + // Given some MAP, what is the most the engine can pull through a wide open throttle, in kg/s? + virtual float maxEngineFlow(float map) const = 0; + +private: + float inversePartThrottleFlow(float maxEngineFlow, float flow, float pressureRatio, float p_up, float iat) const; +}; + +struct ThrottleModel : public ThrottleModelBase { + float effectiveArea(float tps) const override; + float maxEngineFlow(float map) const override; +}; diff --git a/unit_tests/tests/test_throttle_model.cpp b/unit_tests/tests/test_throttle_model.cpp new file mode 100644 index 0000000000..f0c9d940a1 --- /dev/null +++ b/unit_tests/tests/test_throttle_model.cpp @@ -0,0 +1,43 @@ +#include "pch.h" +#include "throttle_model.h" + +// From CFD modeled 70mm throttle +static const float throttle70mmFlowBins[] = { 2, 5, 10, 20, 30, 40, 60, 80, 85, 90, 91 }; +static const float throttle70mmFlowValues[] = { 0.000095, 0.002, 0.0107, 0.045, 0.103, 0.185, 0.438, 0.74, 0.77, 0.775, 0.775 }; + +class MockThrottleModel : public ThrottleModelBase { +public: + float effectiveArea(float tps) const override { + return interpolate2d(tps, throttle70mmFlowBins, throttle70mmFlowValues); + } + + MOCK_METHOD(float, maxEngineFlow, (float map), (const, override)); +}; + +TEST(ThrottleModel, PartThrottle) { + MockThrottleModel model; + + EXPECT_CALL(model, maxEngineFlow(::testing::_)).WillRepeatedly([](float map) { return map / 100 * 0.5f; }); + + // Vary throttle at constant PR/temp + // 100kPa inlet + // 45 kPa MAP + // 0C IAT + EXPECT_NEAR(0.01056, model.estimateThrottleFlow(100, 10, 45, 0), 1e-4); + EXPECT_NEAR(0.04441, model.estimateThrottleFlow(100, 20, 45, 0), 1e-4); + EXPECT_NEAR(0.10165, model.estimateThrottleFlow(100, 30, 45, 0), 1e-4); + EXPECT_NEAR(0.18258, model.estimateThrottleFlow(100, 40, 45, 0), 1e-4); + + // Vary inlet pressure + EXPECT_NEAR(1.0 * 0.04441, model.estimateThrottleFlow(100, 20, 45, 0), 1e-4); + EXPECT_NEAR(1.5 * 0.04441, model.estimateThrottleFlow(150, 20, 45, 0), 1e-4); +} + +// TEST(ThrottleModel, WideOpen) { +// MockThrottleModel model; + +// // Engine can only flow 0.5kg/s at 100kPa +// EXPECT_CALL(model, maxEngineFlow(::testing::_)).WillRepeatedly([](float map) { return map / 100 * 0.5f; }); + +// EXPECT_NEAR(0.5f, model.estimateThrottleFlow(95, 90, 98, 0), 1e-4); +// } diff --git a/unit_tests/tests/tests.mk b/unit_tests/tests/tests.mk index 17716b5efa..f59e651f32 100644 --- a/unit_tests/tests/tests.mk +++ b/unit_tests/tests/tests.mk @@ -53,6 +53,7 @@ TESTS_SRC_CPP = \ tests/test_start_stop.cpp \ tests/test_hardware_reinit.cpp \ tests/test_engine_math.cpp \ + tests/test_throttle_model.cpp \ tests/test_fasterEngineSpinningUp.cpp \ tests/test_dwell_corner_case_issue_796.cpp \ tests/test_idle_controller.cpp \