From f0b810a81ced184b30ca907a46933cdea661b018 Mon Sep 17 00:00:00 2001 From: Matthew Kennedy Date: Thu, 7 Apr 2022 06:21:11 -0700 Subject: [PATCH] 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 | 2 +- firmware/console/status_loop.cpp | 9 +- firmware/controllers/algo/gear_detector.cpp | 56 +++++++++- firmware/controllers/algo/gear_detector.h | 9 ++ firmware/integration/rusefi_config.txt | 1 + firmware/tunerstudio/rusefi.input | 12 +-- unit_tests/tests/test_gear_detector.cpp | 107 ++++++++++++++++++++ 7 files changed, 182 insertions(+), 14 deletions(-) diff --git a/firmware/console/binary/output_channels.txt b/firmware/console/binary/output_channels.txt index 5319a52944..3fcc2debb0 100644 --- a/firmware/console/binary/output_channels.txt +++ b/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 diff --git a/firmware/console/status_loop.cpp b/firmware/console/status_loop.cpp index 30b40804e5..05f24c26c5 100644 --- a/firmware/console/status_loop.cpp +++ b/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()->getGearboxRatio(); + engine->outputChannels.detectedGear = engine->module()->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(); diff --git a/firmware/controllers/algo/gear_detector.cpp b/firmware/controllers/algo/gear_detector.cpp index db760d6c76..46e82db80f 100644 --- a/firmware/controllers/algo/gear_detector.cpp +++ b/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; +} diff --git a/firmware/controllers/algo/gear_detector.h b/firmware/controllers/algo/gear_detector.h index 7d8d2b3ce2..19927072c5 100644 --- a/firmware/controllers/algo/gear_detector.h +++ b/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]; }; diff --git a/firmware/integration/rusefi_config.txt b/firmware/integration/rusefi_config.txt index 171d307e6e..1da3730049 100644 --- a/firmware/integration/rusefi_config.txt +++ b/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" diff --git a/firmware/tunerstudio/rusefi.input b/firmware/tunerstudio/rusefi.input index 9b29fd0a55..5803442989 100644 --- a/firmware/tunerstudio/rusefi.input +++ b/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 diff --git a/unit_tests/tests/test_gear_detector.cpp b/unit_tests/tests/test_gear_detector.cpp index 214137083e..ca2386f3d0 100644 --- a/unit_tests/tests/test_gear_detector.cpp +++ b/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)); +}