Throttle model #63

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"
 }

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 \
 

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<float> 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;
+}

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<float> 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;
+};

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);
+// }

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 \