diff --git a/firmware/controllers/trigger/trigger_central.cpp b/firmware/controllers/trigger/trigger_central.cpp index f2b6eab0ba..4a053b82b7 100644 --- a/firmware/controllers/trigger/trigger_central.cpp +++ b/firmware/controllers/trigger/trigger_central.cpp @@ -302,10 +302,20 @@ void hwHandleVvtCamSignal(bool isRising, efitick_t nowNt, int index) { return; } + auto vvtPosition = engineConfiguration->vvtOffsets[bankIndex * CAMS_PER_BANK + camIndex] - currentPosition; + switch(engineConfiguration->vvtMode[camIndex]) { case VVT_TOYOTA_3_TOOTH: { - // Consider the tooth in the first 1/3 of the engine phase - bool inRange = angleFromPrimarySyncPoint > 0 && angleFromPrimarySyncPoint < (720 / 3); + // Only consider teeth with a sensible VVT angle - this is guaranteed to work so long + // as the range we check is < 120 deg + + // Wrap to +-180 deg + vvtPosition = wrapVvt(vvtPosition, 360); + + // Check whether the position is + // - within range (no resync, just report VVT position) + // - within range but 360 deg out (engine needs resync) + bool inRange = vvtPosition > -37 && vvtPosition < 80; if (!inRange) { return; @@ -319,8 +329,6 @@ void hwHandleVvtCamSignal(bool isRising, efitick_t nowNt, int index) { tc->triggerState.vvtCounter++; - auto vvtPosition = engineConfiguration->vvtOffsets[bankIndex * CAMS_PER_BANK + camIndex] - currentPosition; - // Only do engine sync using one cam, other cams just provide VVT position. if (index == engineConfiguration->engineSyncCam) { angle_t crankOffset = adjustCrankPhase(camIndex); @@ -333,6 +341,8 @@ void hwHandleVvtCamSignal(bool isRising, efitick_t nowNt, int index) { switch(engineConfiguration->vvtMode[camIndex]) { case VVT_HONDA_K_INTAKE: // honda K has four tooth in VVT intake trigger, so we just wrap each of those to 720 / 4 + case VVT_TOYOTA_3_TOOTH: + // Toyota 3 tooth vvt angle comes pre-wrapped, so ensure the crankOffset doesn't break it vvtPosition = wrapVvt(vvtPosition, 180); break; default: diff --git a/unit_tests/tests/tests.mk b/unit_tests/tests/tests.mk index b15b45e998..800287fb01 100644 --- a/unit_tests/tests/tests.mk +++ b/unit_tests/tests/tests.mk @@ -20,7 +20,7 @@ TESTS_SRC_CPP = \ tests/trigger/test_real_gm_24x.cpp \ tests/trigger/test_real_k24a2.cpp \ tests/trigger/test_real_k20.cpp \ - tests/trigger/test_real_toyota_3_tooth_cam.cpp \ + tests/trigger/test_toyota_3_tooth_cam.cpp \ tests/trigger/test_map_cam.cpp \ tests/trigger/test_rpm_multiplier.cpp \ tests/trigger/test_quad_cam.cpp \ diff --git a/unit_tests/tests/trigger/test_real_toyota_3_tooth_cam.cpp b/unit_tests/tests/trigger/test_real_toyota_3_tooth_cam.cpp deleted file mode 100644 index 8109377a43..0000000000 --- a/unit_tests/tests/trigger/test_real_toyota_3_tooth_cam.cpp +++ /dev/null @@ -1,42 +0,0 @@ -#include "pch.h" -#include "logicdata_csv_reader.h" - -TEST(realToyota3ToothCam, running) { - CsvReader reader(1, /* vvtCount */ 1); - - reader.open("tests/trigger/resources/toyota_3_tooth_cam.csv"); - EngineTestHelper eth(engine_type_e::TEST_ENGINE); - engineConfiguration->isFasterEngineSpinUpEnabled = true; - engineConfiguration->alwaysInstantRpm = true; - - engineConfiguration->vvtMode[0] = VVT_TOYOTA_3_TOOTH; - engineConfiguration->vvtOffsets[0] = 152.583f; - - engineConfiguration->trigger.customTotalToothCount = 36; - engineConfiguration->trigger.customSkippedToothCount = 2; - eth.setTriggerType(trigger_type_e::TT_TOOTHED_WHEEL); - - bool hasSeenFirstVvt = false; - - while (reader.haveMore()) { - reader.processLine(ð); - float vvt1 = engine->triggerCentral.getVVTPosition(/*bankIndex*/0, /*camIndex*/0); - - if (vvt1 != 0) { - if (!hasSeenFirstVvt) { - EXPECT_NEAR(vvt1, 0, /*precision*/1); - hasSeenFirstVvt = true; - } - - // cam position should never be reported outside of correct range - EXPECT_TRUE(vvt1 > -3 && vvt1 < 3); - } - } - - EXPECT_NEAR(engine->triggerCentral.getVVTPosition(/*bankIndex*/0, /*camIndex*/0), 0, 1); - ASSERT_EQ(3078, round(Sensor::getOrZero(SensorType::Rpm))); - - // TODO: why warnings? - ASSERT_EQ(1, eth.recentWarnings()->getCount()); - ASSERT_EQ(ObdCode::CUSTOM_PRIMARY_TOO_MANY_TEETH, eth.recentWarnings()->get(0).Code); -} diff --git a/unit_tests/tests/trigger/test_toyota_3_tooth_cam.cpp b/unit_tests/tests/trigger/test_toyota_3_tooth_cam.cpp new file mode 100644 index 0000000000..8712b03a6d --- /dev/null +++ b/unit_tests/tests/trigger/test_toyota_3_tooth_cam.cpp @@ -0,0 +1,144 @@ +#include "pch.h" +#include "logicdata_csv_reader.h" + +TEST(Toyota3ToothCam, RealEngineRunning) { + CsvReader reader(1, /* vvtCount */ 1); + + reader.open("tests/trigger/resources/toyota_3_tooth_cam.csv"); + EngineTestHelper eth(engine_type_e::TEST_ENGINE); + engineConfiguration->isFasterEngineSpinUpEnabled = true; + engineConfiguration->alwaysInstantRpm = true; + + engineConfiguration->vvtMode[0] = VVT_TOYOTA_3_TOOTH; + engineConfiguration->vvtOffsets[0] = 152.58; + + engineConfiguration->trigger.customTotalToothCount = 36; + engineConfiguration->trigger.customSkippedToothCount = 2; + eth.setTriggerType(trigger_type_e::TT_TOOTHED_WHEEL); + + bool hasSeenFirstVvt = false; + + while (reader.haveMore()) { + reader.processLine(ð); + float vvt1 = engine->triggerCentral.getVVTPosition(/*bankIndex*/0, /*camIndex*/0); + + if (vvt1 != 0) { + if (!hasSeenFirstVvt) { + EXPECT_NEAR(vvt1, 0, /*precision*/1); + hasSeenFirstVvt = true; + } + + // cam position should never be reported outside of correct range + EXPECT_TRUE(vvt1 > -3 && vvt1 < 3); + } + } + + EXPECT_EQ(getTriggerCentral()->triggerState.camResyncCounter, 0); + + EXPECT_NEAR(engine->triggerCentral.getVVTPosition(/*bankIndex*/0, /*camIndex*/0), 0, 1); + ASSERT_EQ(3078, round(Sensor::getOrZero(SensorType::Rpm))); + + // TODO: why warnings? + ASSERT_EQ(1, eth.recentWarnings()->getCount()); + ASSERT_EQ(ObdCode::CUSTOM_PRIMARY_TOO_MANY_TEETH, eth.recentWarnings()->get(0).Code); +} + +static void test3tooth(size_t revsBeforeVvt, size_t teethBeforeVvt, bool expectSync, int expectCamResyncCounter) { + EngineTestHelper eth(engine_type_e::TEST_ENGINE); + engineConfiguration->vvtMode[0] = VVT_TOYOTA_3_TOOTH; + engineConfiguration->skippedWheelOnCam = false; + engineConfiguration->trigger.customTotalToothCount = 12; + engineConfiguration->trigger.customSkippedToothCount = 1; + eth.setTriggerType(trigger_type_e::TT_TOOTHED_WHEEL); + + engineConfiguration->vvtOffsets[0] = 35; + + ASSERT_EQ(0, round(Sensor::getOrZero(SensorType::Rpm))); + + // Get the crank sync'd + for (int i = 0; i < 4; i++) { + eth.fireFall(1); + eth.fireRise(9); + } + + ASSERT_EQ(0, round(Sensor::getOrZero(SensorType::Rpm))); + + // skipped tooth - should sync here + eth.fireFall(1); + eth.fireRise(19); + + ASSERT_EQ(500, round(Sensor::getOrZero(SensorType::Rpm))); + + // We should be at TDC + ASSERT_EQ(0, getTriggerCentral()->currentEngineDecodedPhase); + + // Do some number of revolutions before firing the cam tooth + for (size_t i = 0; i < revsBeforeVvt; i++) { + for (size_t j = 0; i < 10; i++) + { + eth.fireFall(1); + eth.fireRise(9); + } + + eth.fireFall(1); + eth.fireRise(19); + } + + // Bump ahead by some more teeth + for (size_t i = 0; i < teethBeforeVvt; i++) { + eth.fireFall(1); + eth.fireRise(9); + } + auto expectedPhase = 360 * revsBeforeVvt + 30 * teethBeforeVvt; + ASSERT_EQ(expectedPhase, getTriggerCentral()->currentEngineDecodedPhase); + + // No full sync yet, no VVT has arrived + EXPECT_EQ(false, getTriggerCentral()->triggerState.hasSynchronizedPhase()); + + // Now fire a cam tooth - this shouldn't change engine phase, + // should set VVT position + hwHandleVvtCamSignal(true, getTimeNowNt(), 0); + + EXPECT_EQ(expectCamResyncCounter, getTriggerCentral()->triggerState.camResyncCounter); + EXPECT_EQ(expectSync, getTriggerCentral()->triggerState.hasSynchronizedPhase()); + + if (expectSync) { + EXPECT_EQ(5, getTriggerCentral()->getVVTPosition(0, 0)); + + // Bump ahead by one tooth + eth.fireFall(1); + eth.fireRise(9); + ASSERT_EQ(60, getTriggerCentral()->currentEngineDecodedPhase); + } else { + EXPECT_EQ(0, getTriggerCentral()->getVVTPosition(0, 0)); + } +} + +// The cam has 3 teeth, which could arrive at one of 6 possible crank phases. +// 0 revs or 1 revs +// +0, +120, or +240 deg +// Test all 6 cases! + +TEST(Toyota3ToothCam, TestFirstToothInPhase) { + test3tooth(0, 1, true, 0); +} + +TEST(Toyota3ToothCam, TestFirstToothOutOfPhase) { + test3tooth(1, 1, true, 1); +} + +TEST(Toyota3ToothCam, TestSecondToothInPhase) { + test3tooth(0, 5, false, 0); +} + +TEST(Toyota3ToothCam, TestSecondToothOutOfPhase) { + test3tooth(1, 5, false, 0); +} + +TEST(Toyota3ToothCam, TestThirdToothInPhase) { + test3tooth(0, 9, false, 0); +} + +TEST(Toyota3ToothCam, TestThirdToothOutOfPhase) { + test3tooth(1, 9, false, 0); +}