From bd2b5a3dba058c0ad827e34d757b6a5afb7eb1b0 Mon Sep 17 00:00:00 2001
From: Matthew Kennedy <matthewkennedy@outlook.com>
Date: Sat, 9 Apr 2022 05:59:07 -0700
Subject: [PATCH] gear detection validation (#4053)

* gear detection validation

* test validation because why not
---
 firmware/controllers/algo/gear_detector.cpp | 41 +++++++++++++++++----
 unit_tests/tests/test_gear_detector.cpp     | 31 ++++++++++++++++
 2 files changed, 64 insertions(+), 8 deletions(-)

diff --git a/firmware/controllers/algo/gear_detector.cpp b/firmware/controllers/algo/gear_detector.cpp
index 46e82db80f..244bde89fb 100644
--- a/firmware/controllers/algo/gear_detector.cpp
+++ b/firmware/controllers/algo/gear_detector.cpp
@@ -7,15 +7,36 @@ static constexpr float geometricMean(float x, float y) {
 void GearDetector::onConfigurationChange(engine_configuration_s const * /*previousConfig*/) {
 	// Compute gear thresholds between gears
 
-	for (size_t i = 0; i < efi::size(m_gearThresholds); i++) {
+	uint8_t gearCount = engineConfiguration->totalGearsCount;
+
+	if (gearCount == 0) {
+		// No gears, nothing to do here.
+		return;
+	}
+
+	if (gearCount > GEARS_COUNT) {
+		firmwareError(OBD_PCM_Processor_Fault, "too many gears");
+		return;
+	}
+
+	// validate gears
+	for (size_t i = 0; i < gearCount; i++) {
+		if (engineConfiguration->gearRatio[i] <= 0) {
+			firmwareError(OBD_PCM_Processor_Fault, "Invalid gear ratio for #%d", i + 1);
+			return;
+		}
+	}
+
+	for (int i = 0; i < gearCount - 1; i++) {
 		// Threshold i is the threshold between gears i and i+1
+		float gearI        = engineConfiguration->gearRatio[i];
+		float gearIplusOne = engineConfiguration->gearRatio[i + 1];
 
-		m_gearThresholds[i] = geometricMean(
-			engineConfiguration->gearRatio[i],
-			engineConfiguration->gearRatio[i + 1]
-		);
+		if (gearI <= gearIplusOne) {
+			firmwareError(OBD_PCM_Processor_Fault, "Invalid gear ordering near gear #%d", i + 1);
+		}
 
-		// TODO: validate gears are in correct order
+		m_gearThresholds[i] = geometricMean(gearI, gearIplusOne);
 	}
 }
 
@@ -27,13 +48,17 @@ void GearDetector::onSlowCallback() {
 }
 
 size_t GearDetector::determineGearFromRatio(float ratio) const {
+	auto gearCount = engineConfiguration->totalGearsCount;
+	if (gearCount == 0) {
+		// No gears, we only have neutral.
+		return 0;
+	}
+
 	// 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;
diff --git a/unit_tests/tests/test_gear_detector.cpp b/unit_tests/tests/test_gear_detector.cpp
index ca2386f3d0..5f5072d6cc 100644
--- a/unit_tests/tests/test_gear_detector.cpp
+++ b/unit_tests/tests/test_gear_detector.cpp
@@ -133,3 +133,34 @@ TEST(GearDetector, DetermineGear8Speed) {
 	// Extremely low ratio suggests stopped engine at speed?
 	EXPECT_EQ(0, dut.determineGearFromRatio(0.1));
 }
+
+TEST(GearDetector, ParameterValidation) {
+	EngineTestHelper eth(TEST_ENGINE);
+	GearDetector dut;
+
+	// Defaults should work
+	EXPECT_NO_FATAL_ERROR(dut.onConfigurationChange(nullptr));
+
+	// Invalid gear count
+	engineConfiguration->totalGearsCount = 25;
+	EXPECT_FATAL_ERROR(dut.onConfigurationChange(nullptr));
+
+	// Valid gears
+	engineConfiguration->totalGearsCount = 2;
+	engineConfiguration->gearRatio[0] = 3;
+	engineConfiguration->gearRatio[1] = 2;
+	EXPECT_NO_FATAL_ERROR(dut.onConfigurationChange(nullptr));
+
+	// Invalid gear ratio
+	engineConfiguration->gearRatio[1] = 0;
+	EXPECT_FATAL_ERROR(dut.onConfigurationChange(nullptr));
+
+	// Out of order gear ratios
+	engineConfiguration->gearRatio[0] = 2;
+	engineConfiguration->gearRatio[1] = 3;
+	EXPECT_FATAL_ERROR(dut.onConfigurationChange(nullptr));
+
+	// No gears at all is a valid configuration
+	engineConfiguration->totalGearsCount = 0;
+	EXPECT_NO_FATAL_ERROR(dut.onConfigurationChange(nullptr));
+}