From 68fb838641bf8e3d5c1345b99d71fff1cb8f62ee Mon Sep 17 00:00:00 2001 From: Matthew Kennedy Date: Thu, 7 Jul 2022 20:16:56 -0700 Subject: [PATCH] Lua hooks for gear detection (#4328) * implement * sim has vss * write a test * guard --- firmware/controllers/algo/gear_detector.cpp | 37 +++++++++++++------ firmware/controllers/algo/gear_detector.h | 4 +++ firmware/controllers/lua/lua_hooks.cpp | 13 +++++++ simulator/simulator/efifeatures.h | 2 +- unit_tests/tests/test_gear_detector.cpp | 40 +++++++++++++++++++++ 5 files changed, 85 insertions(+), 11 deletions(-) diff --git a/firmware/controllers/algo/gear_detector.cpp b/firmware/controllers/algo/gear_detector.cpp index 179f144139..ab5412b182 100644 --- a/firmware/controllers/algo/gear_detector.cpp +++ b/firmware/controllers/algo/gear_detector.cpp @@ -77,7 +77,7 @@ size_t GearDetector::determineGearFromRatio(float ratio) const { return currentGear; } -float GearDetector::computeGearboxRatio() const { +float GearDetector::getDriveshaftRpm() const { auto vssKph = Sensor::getOrZero(SensorType::VehicleSpeed); if (vssKph < 5) { @@ -85,6 +85,23 @@ float GearDetector::computeGearboxRatio() const { return 0; } + // Convert to wheel RPM + // km rev 1 hr + // ------ * ------------ * __________ + // hr km 60 min + float wheelRpm = vssKph * engineConfiguration->driveWheelRevPerKm * (1 / 60.0f); + + // Convert to driveshaft RPM + return wheelRpm * engineConfiguration->finalGearRatio; +} + +float GearDetector::computeGearboxRatio() const { + float driveshaftRpm = getDriveshaftRpm(); + + if (driveshaftRpm == 0) { + return 0; + } + float engineRpm; if (Sensor::hasSensor(SensorType::InputShaftSpeed)) { engineRpm = Sensor::getOrZero(SensorType::InputShaftSpeed); @@ -92,18 +109,18 @@ float GearDetector::computeGearboxRatio() const { engineRpm = Sensor::getOrZero(SensorType::Rpm); } - // Convert to wheel RPM - // km rev 1 hr - // ------ * ------------ * __________ - // hr km 60 min - float wheelRpm = vssKph * engineConfiguration->driveWheelRevPerKm * (1 / 60.0f); - - // Convert to driveshaft RPM - auto driveshaftRpm = wheelRpm * engineConfiguration->finalGearRatio; - return engineRpm / driveshaftRpm; } +float GearDetector::getRpmInGear(size_t gear) const { + if (gear <= 0 || gear > engineConfiguration->totalGearsCount) { + return 0; + } + + // Ideal engine RPM is driveshaft speed times gear + return getDriveshaftRpm() * engineConfiguration->gearRatio[gear - 1]; +} + float GearDetector::getGearboxRatio() const { return m_gearboxRatio; } diff --git a/firmware/controllers/algo/gear_detector.h b/firmware/controllers/algo/gear_detector.h index 19927072c5..0761279121 100644 --- a/firmware/controllers/algo/gear_detector.h +++ b/firmware/controllers/algo/gear_detector.h @@ -1,3 +1,4 @@ +#pragma once class GearDetector : public EngineModule { public: @@ -11,8 +12,11 @@ public: size_t determineGearFromRatio(float ratio) const; + float getRpmInGear(size_t gear) const; + private: float computeGearboxRatio() const; + float getDriveshaftRpm() const; float m_gearboxRatio = 0; size_t m_currentGear = 0; diff --git a/firmware/controllers/lua/lua_hooks.cpp b/firmware/controllers/lua/lua_hooks.cpp index 3c5d868cce..7e1e132446 100644 --- a/firmware/controllers/lua/lua_hooks.cpp +++ b/firmware/controllers/lua/lua_hooks.cpp @@ -616,6 +616,19 @@ void configureRusefiLuaHooks(lua_State* l) { return 0; }); +#if EFI_VEHICLE_SPEED + lua_register(l, "getCurrentGear", [](lua_State* l) { + lua_pushinteger(l, engine->module()->getCurrentGear()); + return 1; + }); + + lua_register(l, "getRpmInGear", [](lua_State* l) { + auto idx = luaL_checkinteger(l, 1); + lua_pushinteger(l, engine->module()->getRpmInGear(idx)); + return 1; + }); +#endif // EFI_VEHICLE_SPEED + #if !EFI_UNIT_TEST lua_register(l, "startPwm", lua_startPwm); lua_register(l, "setPwmDuty", lua_setPwmDuty); diff --git a/simulator/simulator/efifeatures.h b/simulator/simulator/efifeatures.h index a5ba935fae..c198ced86d 100644 --- a/simulator/simulator/efifeatures.h +++ b/simulator/simulator/efifeatures.h @@ -138,7 +138,7 @@ #define EFI_TUNER_STUDIO_VERBOSE FALSE #define EFI_FILE_LOGGING TRUE #define EFI_WARNING_LED FALSE -#define EFI_VEHICLE_SPEED FALSE +#define EFI_VEHICLE_SPEED TRUE #define EFI_TCU FALSE #define EFI_SENSOR_CHART TRUE diff --git a/unit_tests/tests/test_gear_detector.cpp b/unit_tests/tests/test_gear_detector.cpp index 5f5072d6cc..53bcc452a1 100644 --- a/unit_tests/tests/test_gear_detector.cpp +++ b/unit_tests/tests/test_gear_detector.cpp @@ -27,6 +27,46 @@ TEST(GearDetector, ComputeGearRatio) { EXPECT_EQ(0, GetGearRatioFor(507, 4.1, 0, 800)); } + +TEST(GearDetector, GetRpmInGear) { + EngineTestHelper eth(TEST_ENGINE); + + engineConfiguration->driveWheelRevPerKm = 507; + engineConfiguration->finalGearRatio = 4.10f; + + // real gears from Volvo racecar + engineConfiguration->totalGearsCount = 5; + engineConfiguration->gearRatio[0] = 3.35f; + engineConfiguration->gearRatio[1] = 1.99f; + engineConfiguration->gearRatio[2] = 1.33f; + engineConfiguration->gearRatio[3] = 1.00f; + engineConfiguration->gearRatio[4] = 0.72f; + + GearDetector dut; + + Sensor::setMockValue(SensorType::VehicleSpeed, 29.45f / 0.6214f); + EXPECT_NEAR(5500, dut.getRpmInGear(1), 1); + Sensor::setMockValue(SensorType::VehicleSpeed, 49.57f / 0.6214f); + EXPECT_NEAR(5500, dut.getRpmInGear(2), 1); + Sensor::setMockValue(SensorType::VehicleSpeed, 74.18f / 0.6214f); + EXPECT_NEAR(5500, dut.getRpmInGear(3), 1); + Sensor::setMockValue(SensorType::VehicleSpeed, 98.65f / 0.6214f); + EXPECT_NEAR(5500, dut.getRpmInGear(4), 1); + Sensor::setMockValue(SensorType::VehicleSpeed, 137.02f / 0.6214f); + EXPECT_NEAR(5500, dut.getRpmInGear(5), 1); + + // Test some invalid cases + EXPECT_FLOAT_EQ(0, dut.getRpmInGear(0)); + EXPECT_FLOAT_EQ(0, dut.getRpmInGear(10)); + + // Zero vehicle speed shouldn't cause a problem + Sensor::setMockValue(SensorType::VehicleSpeed, 0); + EXPECT_FLOAT_EQ(0, dut.getRpmInGear(0)); + EXPECT_FLOAT_EQ(0, dut.getRpmInGear(1)); + EXPECT_FLOAT_EQ(0, dut.getRpmInGear(5)); + EXPECT_FLOAT_EQ(0, dut.getRpmInGear(10)); +} + TEST(GearDetector, DetermineGearSingleSpeed) { EngineTestHelper eth(TEST_ENGINE); GearDetector dut;