Gear detection implementation (#4052)

* gear detect improvements

* ui

* ui for gear ratios

* ui

* output channel

* output channels etc

* implement gear detection

* name

* s

* status loop

* missing function

* ui tweaking

* s

firmware/console/binary/output_channels.txt

@@ -324,7 +324,7 @@ uint16_t rpmAcceleration;dRPM;"RPM/s",1, 0, 0, 0, 0
 	uint16_t autoscale fallbackMap;;"kPa", 0.1, 0, 0, 1000, 1
 
 	int8_t autoscale boostControllerClosedLoopPart;@@GAUGE_NAME_BOOST_CLOSED_LOOP@@;"%", 0.5, 0, -50, 50, 1
-	uint8_t unused503;;"", 1, 0, 0, 0, 0
+	uint8_t detectedGear;@@GAUGE_NAME_DETECTED_GEAR@@;"", 1, 0, 0, @@GEARS_COUNT@@, 0
 
 	int16_t autoscale timingCltCorrection;;"deg",{1/@@PACK_MULT_PERCENT@@}, 0, -20, 20, 2
 	int16_t autoscale timingIatCorrection;;"deg",{1/@@PACK_MULT_PERCENT@@}, 0, -20, 20, 2

firmware/console/status_loop.cpp

@@ -546,10 +546,11 @@ static void updateVvtSensors() {
 #endif
 }
 
-static void updateVehicleSpeed(int rpm) {
+static void updateVehicleSpeed() {
 #if EFI_VEHICLE_SPEED
 	engine->outputChannels.vehicleSpeedKph = Sensor::getOrZero(SensorType::VehicleSpeed);
 	engine->outputChannels.speedToRpmRatio = engine->module<GearDetector>()->getGearboxRatio();
+	engine->outputChannels.detectedGear = engine->module<GearDetector>()->getCurrentGear();
 #endif /* EFI_VEHICLE_SPEED */
 }
 
@@ -594,14 +595,14 @@ static void updateMiscSensors() {
 	engine->outputChannels.tCharge = engine->engineState.sd.tCharge;
 }
 
-static void updateSensors(int rpm) {
+static void updateSensors() {
 	updateTempSensors();
 	updateThrottles();
 	updateRawSensors();
 	updateLambda();
 	updateFuelSensors();
 	updateVvtSensors();
-	updateVehicleSpeed(rpm);
+	updateVehicleSpeed();
 	updatePressures();
 	updateMiscSensors();
 }
@@ -724,7 +725,7 @@ void updateTunerStudioState() {
 	auto instantRpm = engine->triggerCentral.triggerState.getInstantRpm();
 	tsOutputChannels->instantRpm = instantRpm;
 
-	updateSensors(rpm);
+	updateSensors();
 	updateFuelInfo();
 	updateIgnition(rpm);
 	updateFlags();

firmware/controllers/algo/gear_detector.cpp

@@ -1,9 +1,55 @@
 #include "pch.h"
 
-void GearDetector::onSlowCallback() {
-	m_gearboxRatio = computeGearboxRatio();
+static constexpr float geometricMean(float x, float y) {
+	return sqrtf(x * y);
+}
 
-	// TODO: solve for which gear this is
+void GearDetector::onConfigurationChange(engine_configuration_s const * /*previousConfig*/) {
+	// Compute gear thresholds between gears
+
+	for (size_t i = 0; i < efi::size(m_gearThresholds); i++) {
+		// Threshold i is the threshold between gears i and i+1
+
+		m_gearThresholds[i] = geometricMean(
+			engineConfiguration->gearRatio[i],
+			engineConfiguration->gearRatio[i + 1]
+		);
+
+		// TODO: validate gears are in correct order
+	}
+}
+
+void GearDetector::onSlowCallback() {
+	float ratio = computeGearboxRatio();
+	m_gearboxRatio = ratio;
+
+	m_currentGear = determineGearFromRatio(ratio);
+}
+
+size_t GearDetector::determineGearFromRatio(float ratio) const {
+	// 1.5x first gear is neutral or clutch slip or something
+	if (ratio > engineConfiguration->gearRatio[0] * 1.5f) {
+		return 0;
+	}
+
+	auto gearCount = engineConfiguration->totalGearsCount;
+
+	// 0.66x top gear is coasting with engine off or something
+	if (ratio < engineConfiguration->gearRatio[gearCount - 1] * 0.66f) {
+		return 0;
+	}
+
+	size_t currentGear = gearCount;
+
+	while (currentGear > 1) {
+		if (ratio < m_gearThresholds[currentGear - 2]) {
+			break;
+		}
+
+		currentGear--;
+	}
+
+	return currentGear;
 }
 
 float GearDetector::computeGearboxRatio() const {
@@ -31,3 +77,7 @@ float GearDetector::computeGearboxRatio() const {
 float GearDetector::getGearboxRatio() const {
 	return m_gearboxRatio;
 }
+
+size_t GearDetector::getCurrentGear() const {
+	return m_currentGear;
+}

firmware/controllers/algo/gear_detector.h

@@ -2,11 +2,20 @@
 class GearDetector : public EngineModule {
 public:
 	void onSlowCallback() override;
+	void onConfigurationChange(engine_configuration_s const * /*previousConfig*/) override;
 
 	float getGearboxRatio() const;
 
+	// Returns 0 for neutral, 1 for 1st, 5 for 5th, etc.
+	size_t getCurrentGear() const;
+
+	size_t determineGearFromRatio(float ratio) const;
+
 private:
 	float computeGearboxRatio() const;
 
 	float m_gearboxRatio = 0;
+	size_t m_currentGear = 0;
+
+	float m_gearThresholds[GEARS_COUNT - 1];
 };

firmware/integration/rusefi_config.txt

@@ -1695,6 +1695,7 @@ end_struct
 #define GAUGE_NAME_TIMING_ADVANCE "timing"
 #define GAUGE_NAME_VVS "Vehicle Speed"
 #define GAUGE_NAME_GEAR_RATIO "Gearbox Ratio"
+#define GAUGE_NAME_DETECTED_GEAR "Detected Gear"
 #define GAUGE_NAME_TURBO_SPEED "Turbocharger Speed"
 #define GAUGE_NAME_VBAT "VBatt"
 #define GAUGE_NAME_TIME "Time"

firmware/tunerstudio/rusefi.input

@@ -1037,7 +1037,6 @@ gaugeCategory = Sensors - Extra 2
    knockLevelGauge   = knockLevel,"Knock level",           "V",        0,      7,      10,     10,     100,   100,   1,   2
    knockCountGauge   = knockCount, "Knock count",          "count",        0, 10000, 0, 0, 10000, 10000, 0, 0
    fuelTankLevelGauge   = fuelTankLevel,"Fuel level",           "%",        0,      100,      10,     20,     100,   100,   1,   1
-   speedToRpmRatioGauge = speedToRpmRatio, @@GAUGE_NAME_GEAR_RATIO@@, "",     0,   100,     0,    0,    100,  100,   4,   4
    wastegatePosGauge = wastegatePositionSensor, @@GAUGE_NAME_WG_POSITION@@,  "%",        0,    100,       0,      0,     100,   100,   1,   1
    idlePosSensGauge  = idlePositionSensor,      @@GAUGE_NAME_IDLE_POSITION@@,       "%",        0,    100,       0,      0,     100,   100,   1,   1
    currentEnginePhaseGauge = currentEnginePhase, "Engine Phase", "deg", 0, 720, 0, 0, 720, 720, 0, 0
@@ -1184,6 +1183,8 @@ gaugeCategory = Sensors - Raw
 gaugeCategory = Transmission
    desiredGearGauge = tcuDesiredGear, @@GAUGE_NAME_DESIRED_GEAR@@, "gear", -1, 10, -1, -1, 10, 10, 0, 0
    currentGearGauge = tcuCurrentGear, @@GAUGE_NAME_CURRENT_GEAR@@, "gear", -1, 10, -1, -1, 10, 10, 0, 0
+   detectedGearGauge = detectedGear, @@GAUGE_NAME_DETECTED_GEAR@@, "gear", 0, @@GEARS_COUNT@@, 0, 0, @@GEARS_COUNT@@, @@GEARS_COUNT@@, 0, 0
+   speedToRpmRatioGauge = speedToRpmRatio, @@GAUGE_NAME_GEAR_RATIO@@, "",     0,   100,     0,    0,    100,  100,   4,   4
 
 gaugeCategory = Knock
    knock1Gauge = knock1, "Knock Cyl 1", "dBv", -60, 10, -60, -60, 10, 10, 0, 0
@@ -1466,10 +1467,10 @@ menuDialog = main
 		subMenu = gearDetection,			"Gear detection", 0
 
 		subMenu = std_separator
-		subMenu = boostDialog,          "Boost Control"
-		subMenu = boostOpenLoopDialog,	"Boost Control Open Loop",          { isBoostControlEnabled && boostType == 0 }
-		subMenu = boostPidDialog,       "Boost Control Closed Loop PID",    { isBoostControlEnabled && boostType == 1 }
-		subMenu = boostTargetDialog,	"Boost Control Closed Loop Target", { isBoostControlEnabled && boostType == 1 }
+		subMenu = boostDialog,          "Boost control"
+		subMenu = boostOpenLoopDialog,	"Boost control open loop",          { isBoostControlEnabled }
+		subMenu = boostPidDialog,       "Boost control PID",    { isBoostControlEnabled && boostType == 1 }
+		subMenu = boostTargetDialog,	"Boost control target", { isBoostControlEnabled && boostType == 1 }
 		subMenu = boostEtbPid,			"ETB-style Wastegate Actuator", { etbFunctions1 == @@etb_function_e_ETB_Wastegate@@ || etbFunctions2 == @@etb_function_e_ETB_Wastegate@@ }
 
 		subMenu = std_separator
@@ -3122,7 +3123,6 @@ cmd_set_engine_type_default					= "@@TS_IO_TEST_COMMAND_char@@\x00\x31\x00\x00"
 	dialog = speedSensorLeft, "", yAxis
 	    panel = speedSensorAnalog, { enableCanVss == 0 }
 		panel = speedSensorCan
-		panel = vssFilter
 
 	dialog = gearDetection, "Gear Detection"
 		field = "Wheel revolutions per kilometer",	driveWheelRevPerKm

unit_tests/tests/test_gear_detector.cpp

@@ -26,3 +26,110 @@ TEST(GearDetector, ComputeGearRatio) {
 	// Idling, car stopped, check no div/0
 	EXPECT_EQ(0, GetGearRatioFor(507, 4.1, 0, 800));
 }
+
+TEST(GearDetector, DetermineGearSingleSpeed) {
+	EngineTestHelper eth(TEST_ENGINE);
+	GearDetector dut;
+
+	engineConfiguration->totalGearsCount = 1;
+	engineConfiguration->gearRatio[0] = 2;
+
+	dut.onConfigurationChange(nullptr);
+
+	// Super high ratios indicate clutch slip or idling in neutral or something
+	EXPECT_EQ(0, dut.determineGearFromRatio(100));
+	EXPECT_EQ(0, dut.determineGearFromRatio(4));
+
+	// Check exactly on the gear
+	EXPECT_EQ(1, dut.determineGearFromRatio(2));
+
+	// Check near the gear
+	EXPECT_EQ(1, dut.determineGearFromRatio(2.1));
+	EXPECT_EQ(1, dut.determineGearFromRatio(1.9));
+
+	// Extremely low ratio suggests stopped engine at speed?
+	EXPECT_EQ(0, dut.determineGearFromRatio(1.0));
+}
+
+TEST(GearDetector, DetermineGear5Speed) {
+	EngineTestHelper eth(TEST_ENGINE);
+	GearDetector dut;
+
+	engineConfiguration->totalGearsCount = 5;
+	engineConfiguration->gearRatio[0] = 3.35;
+	engineConfiguration->gearRatio[1] = 1.99;
+	engineConfiguration->gearRatio[2] = 1.33;
+	engineConfiguration->gearRatio[3] = 1.00;
+	engineConfiguration->gearRatio[4] = 0.72;
+
+	dut.onConfigurationChange(nullptr);
+
+	// Super high ratios indicate clutch slip or idling in neutral or something
+	EXPECT_EQ(0, dut.determineGearFromRatio(100));
+	EXPECT_EQ(0, dut.determineGearFromRatio(6));
+
+	// Check exactly on gears
+	EXPECT_EQ(1, dut.determineGearFromRatio(3.35));
+	EXPECT_EQ(2, dut.determineGearFromRatio(1.99));
+	EXPECT_EQ(3, dut.determineGearFromRatio(1.33));
+	EXPECT_EQ(4, dut.determineGearFromRatio(1.00));
+	EXPECT_EQ(5, dut.determineGearFromRatio(0.72));
+
+	// Check near each gear
+	EXPECT_EQ(1, dut.determineGearFromRatio(3.45));
+	EXPECT_EQ(1, dut.determineGearFromRatio(3.25));
+
+	EXPECT_EQ(2, dut.determineGearFromRatio(2.2));
+	EXPECT_EQ(2, dut.determineGearFromRatio(1.8));
+
+	EXPECT_EQ(3, dut.determineGearFromRatio(1.45));
+	EXPECT_EQ(3, dut.determineGearFromRatio(1.25));
+
+	EXPECT_EQ(4, dut.determineGearFromRatio(1.1));
+	EXPECT_EQ(4, dut.determineGearFromRatio(0.9));
+
+	EXPECT_EQ(5, dut.determineGearFromRatio(0.8));
+	EXPECT_EQ(5, dut.determineGearFromRatio(0.6));
+
+	// Extremely low ratio suggests stopped engine at speed?
+	EXPECT_EQ(0, dut.determineGearFromRatio(0.1));
+}
+
+TEST(GearDetector, DetermineGear8Speed) {
+	EngineTestHelper eth(TEST_ENGINE);
+	GearDetector dut;
+
+	// ZF 8HP 70
+	engineConfiguration->totalGearsCount = 8;
+	engineConfiguration->gearRatio[0] = 4.69;
+	engineConfiguration->gearRatio[1] = 3.13;
+	engineConfiguration->gearRatio[2] = 2.10;
+	engineConfiguration->gearRatio[3] = 1.67;
+	engineConfiguration->gearRatio[4] = 1.28;
+	engineConfiguration->gearRatio[5] = 1;
+	engineConfiguration->gearRatio[6] = 0.84;
+	engineConfiguration->gearRatio[7] = 0.67;
+
+	dut.onConfigurationChange(nullptr);
+
+	// Super high ratios indicate clutch slip or idling in neutral or something
+	EXPECT_EQ(0, dut.determineGearFromRatio(100));
+	EXPECT_EQ(0, dut.determineGearFromRatio(8));
+
+	// Check exactly on gears - only test the ends, the middle works
+	EXPECT_EQ(1, dut.determineGearFromRatio(4.69));
+	EXPECT_EQ(2, dut.determineGearFromRatio(3.13));
+
+	EXPECT_EQ(7, dut.determineGearFromRatio(0.84));
+	EXPECT_EQ(8, dut.determineGearFromRatio(0.67));
+
+	// Check near each gear - only test the ends, the middle works
+	EXPECT_EQ(1, dut.determineGearFromRatio(4.75));
+	EXPECT_EQ(1, dut.determineGearFromRatio(4.3));
+
+	EXPECT_EQ(8, dut.determineGearFromRatio(0.71));
+	EXPECT_EQ(8, dut.determineGearFromRatio(0.6));
+
+	// Extremely low ratio suggests stopped engine at speed?
+	EXPECT_EQ(0, dut.determineGearFromRatio(0.1));
+}