From fcb989744305620a5552ff1fb9b4b12ed68e5eba Mon Sep 17 00:00:00 2001 From: Matthew Kennedy Date: Fri, 12 Aug 2022 05:08:23 -0700 Subject: [PATCH] resolve VVT phase using every tooth, not just sync point (#4434) * resolve VVT phase using every tooth, not just sync point * fix tests * comment * changelog --- firmware/CHANGELOG.md | 1 + .../controllers/trigger/trigger_central.cpp | 32 +++++++++++++++---- .../controllers/trigger/trigger_central.h | 7 ++-- .../tests/trigger/test_cam_vvt_input.cpp | 2 +- .../test_real_cranking_nissan_vq40.cpp | 6 ++-- 5 files changed, 35 insertions(+), 13 deletions(-) diff --git a/firmware/CHANGELOG.md b/firmware/CHANGELOG.md index 47c7205826..aabefa6a52 100644 --- a/firmware/CHANGELOG.md +++ b/firmware/CHANGELOG.md @@ -35,6 +35,7 @@ Release template (copy/paste this for new release): - Additional CAN messages #4401 - Option to invert VVT control (exhaust cams, etc) #4424 - Raw Battery gauge + - More accurate/stable VVT angle calculation #4433 ### Fixed - Lua CAN reception fixed for 11-bit IDs where the frame would be received, but a corrupt ID was passed to the handler function. #4321 diff --git a/firmware/controllers/trigger/trigger_central.cpp b/firmware/controllers/trigger/trigger_central.cpp index 2a758b8c64..1bc18fe36d 100644 --- a/firmware/controllers/trigger/trigger_central.cpp +++ b/firmware/controllers/trigger/trigger_central.cpp @@ -79,7 +79,18 @@ expected TriggerCentral::getCurrentEnginePhase(efitick_t nowNt) const { return unexpected; } - return m_syncPointTimer.getElapsedUs(nowNt) / oneDegreeUs; + float elapsed; + float toothPhase; + + { + // under lock to avoid mismatched tooth phase and time + chibios_rt::CriticalSectionLocker csl; + + elapsed = m_lastToothTimer.getElapsedUs(nowNt); + toothPhase = m_lastToothPhaseFromSyncPoint; + } + + return toothPhase + elapsed / oneDegreeUs; } /** @@ -685,19 +696,28 @@ void TriggerCentral::handleShaftSignal(trigger_event_e signal, efitick_t timesta int crankDivider = getCrankDivider(triggerShape.getOperationMode()); int crankInternalIndex = triggerState.getTotalRevolutionCounter() % crankDivider; int triggerIndexForListeners = decodeResult.Value.CurrentIndex + (crankInternalIndex * triggerShape.getSize()); - if (triggerIndexForListeners == 0) { - m_syncPointTimer.reset(timestamp); - } reportEventToWaveChart(signal, triggerIndexForListeners); - // Compute the current engine absolute phase, 0 means currently at #1 TDC - auto currentPhase = engine->triggerCentral.triggerFormDetails.eventAngles[triggerIndexForListeners] - tdcPosition(); + // Look up this tooth's angle from the sync point. If this tooth is the sync point, we'll get 0 here. + auto currentPhaseFromSyncPoint = engine->triggerCentral.triggerFormDetails.eventAngles[triggerIndexForListeners]; + + // Adjust so currentPhase is in engine-space angle, not trigger-space angle + auto currentPhase = currentPhaseFromSyncPoint - tdcPosition(); wrapAngle(currentPhase, "currentEnginePhase", CUSTOM_ERR_6555); #if EFI_TUNER_STUDIO engine->outputChannels.currentEnginePhase = currentPhase; #endif // EFI_TUNER_STUDIO + // Record precise time and phase of the engine. This is used for VVT decode. + { + // under lock to avoid mismatched tooth phase and time + chibios_rt::CriticalSectionLocker csl; + + m_lastToothTimer.reset(timestamp); + m_lastToothPhaseFromSyncPoint = currentPhaseFromSyncPoint; + } + #if TRIGGER_EXTREME_LOGGING efiPrintf("trigger %d %d %d", triggerIndexForListeners, getRevolutionCounter(), (int)getTimeNowUs()); #endif /* TRIGGER_EXTREME_LOGGING */ diff --git a/firmware/controllers/trigger/trigger_central.h b/firmware/controllers/trigger/trigger_central.h index 39fefdcfc3..2e4daf547c 100644 --- a/firmware/controllers/trigger/trigger_central.h +++ b/firmware/controllers/trigger/trigger_central.h @@ -140,9 +140,10 @@ public: private: void decodeMapCam(efitick_t nowNt, float currentPhase); - // Keep track of the last time we saw the sync tooth go by (trigger index 0) - // not TDC point - Timer m_syncPointTimer; + // Time since the last tooth + Timer m_lastToothTimer; + // Phase of the last tooth relative to the sync point + float m_lastToothPhaseFromSyncPoint; }; void triggerInfo(void); diff --git a/unit_tests/tests/trigger/test_cam_vvt_input.cpp b/unit_tests/tests/trigger/test_cam_vvt_input.cpp index 19f7be9bd8..30c06bac66 100644 --- a/unit_tests/tests/trigger/test_cam_vvt_input.cpp +++ b/unit_tests/tests/trigger/test_cam_vvt_input.cpp @@ -173,7 +173,7 @@ TEST(trigger, testNB2CamInput) { eth.moveTimeForwardUs(MS2US( 30)); hwHandleVvtCamSignal(TV_RISE, getTimeNowNt(), 0); - EXPECT_NEAR(288.0f, engine->triggerCentral.getVVTPosition(0, 0), EPS2D); + EXPECT_NEAR(290.5f, engine->triggerCentral.getVVTPosition(0, 0), EPS2D); // actually position based on VVT! ASSERT_EQ(totalRevolutionCountBeforeVvtSync + 3, engine->triggerCentral.triggerState.getTotalRevolutionCounter()); diff --git a/unit_tests/tests/trigger/test_real_cranking_nissan_vq40.cpp b/unit_tests/tests/trigger/test_real_cranking_nissan_vq40.cpp index 98c9462fc6..ead37219d6 100644 --- a/unit_tests/tests/trigger/test_real_cranking_nissan_vq40.cpp +++ b/unit_tests/tests/trigger/test_real_cranking_nissan_vq40.cpp @@ -23,13 +23,13 @@ TEST(realCrankingVQ40, normalCranking) { float vvt1 = engine->triggerCentral.getVVTPosition(/*bankIndex*/0, /*camIndex*/0); if (vvt1 != 0 && !hasSeenFirstVvt) { - EXPECT_NEAR(vvt1, -33.204, 1); + EXPECT_NEAR(vvt1, -45.56, 1); hasSeenFirstVvt = true; } } - EXPECT_NEAR(engine->triggerCentral.getVVTPosition(/*bankIndex*/0, /*camIndex*/0), -46.817, 1e-2); - EXPECT_NEAR(engine->triggerCentral.getVVTPosition(/*bankIndex*/1, /*camIndex*/0), -47.411, 1e-2); + EXPECT_NEAR(engine->triggerCentral.getVVTPosition(/*bankIndex*/0, /*camIndex*/0), -45.67, 1e-2); + EXPECT_NEAR(engine->triggerCentral.getVVTPosition(/*bankIndex*/1, /*camIndex*/0), -45.47, 1e-2); ASSERT_EQ(241, round(Sensor::getOrZero(SensorType::Rpm)))<< reader.lineIndex(); // TODO: why warnings?