diff --git a/firmware/controllers/trigger/trigger_central.cpp b/firmware/controllers/trigger/trigger_central.cpp index a76e992faf..a7cd1b866f 100644 --- a/firmware/controllers/trigger/trigger_central.cpp +++ b/firmware/controllers/trigger/trigger_central.cpp @@ -115,13 +115,16 @@ static bool vvtWithRealDecoder(vvt_mode_e vvtMode) { && vvtMode != VVT_FIRST_HALF; } -static void syncAndReport(TriggerCentral *tc, int mod, int remainder DECLARE_ENGINE_PARAMETER_SUFFIX) { - bool wasChanged = tc->triggerState.syncSymmetricalCrank(mod, remainder); - if (wasChanged && engineConfiguration->debugMode == DBG_VVT) { +static angle_t syncAndReport(TriggerCentral *tc, int divider, int remainder DECLARE_ENGINE_PARAMETER_SUFFIX) { + angle_t engineCycle = getEngineCycle(engine->getOperationMode(PASS_ENGINE_PARAMETER_SIGNATURE)); + + angle_t offset = tc->triggerState.syncSymmetricalCrank(divider, remainder, engineCycle); + if (offset > 0 && engineConfiguration->debugMode == DBG_VVT) { #if EFI_TUNER_STUDIO tsOutputChannels.debugIntField1++; #endif /* EFI_TUNER_STUDIO */ } + return offset; } static void turnOffAllDebugFields(void *arg) { @@ -140,6 +143,40 @@ static void turnOffAllDebugFields(void *arg) { #endif /* EFI_PROD_CODE */ } +static angle_t adjustCrankPhase(int camIndex DECLARE_ENGINE_PARAMETER_SUFFIX) { + TriggerCentral *tc = &engine->triggerCentral; + operation_mode_e operationMode = engine->getOperationMode(PASS_ENGINE_PARAMETER_SIGNATURE); + + switch (engineConfiguration->vvtMode[camIndex]) { + case VVT_FIRST_HALF: + return syncAndReport(tc, getCrankDivider(operationMode), 1 PASS_ENGINE_PARAMETER_SUFFIX); + case VVT_SECOND_HALF: + return syncAndReport(tc, getCrankDivider(operationMode), 0 PASS_ENGINE_PARAMETER_SUFFIX); + case VVT_MIATA_NB2: + /** + * NB2 is a symmetrical crank, there are four phases total + */ + return syncAndReport(tc, getCrankDivider(operationMode), miataNbIndex PASS_ENGINE_PARAMETER_SUFFIX); + case VVT_NISSAN_VQ: + return syncAndReport(tc, getCrankDivider(operationMode), 0 PASS_ENGINE_PARAMETER_SUFFIX); + default: + case VVT_INACTIVE: + // do nothing + return 0; + } +} + +static angle_t wrapVvt(angle_t vvtPosition) { + // Wrap VVT position in to the range [-360, 360) + while (vvtPosition < -360) { + vvtPosition += 720; + } + while (vvtPosition >= 360) { + vvtPosition -= 720; + } + return vvtPosition; +} + void hwHandleVvtCamSignal(trigger_value_e front, efitick_t nowNt, int index DECLARE_ENGINE_PARAMETER_SUFFIX) { int bankIndex = index / CAMS_PER_BANK; int camIndex = index % CAMS_PER_BANK; @@ -291,45 +328,18 @@ void hwHandleVvtCamSignal(trigger_value_e front, efitick_t nowNt, int index DECL auto vvtPosition = engineConfiguration->vvtOffsets[bankIndex * CAMS_PER_BANK + camIndex] - currentPosition; - // Wrap VVT position in to the range [-360, 360) - while (vvtPosition < -360) { - vvtPosition += 720; - } - while (vvtPosition >= 360) { - vvtPosition -= 720; - } - - tc->vvtPosition[bankIndex][camIndex] = vvtPosition; - if (index != 0) { + // todo: only assign initial position of not first cam once cam was synchronized + tc->vvtPosition[bankIndex][camIndex] = wrapVvt(vvtPosition); // at the moment we use only primary VVT to sync crank phase return; } - operation_mode_e operationMode = engine->getOperationMode(PASS_ENGINE_PARAMETER_SIGNATURE); - - switch (engineConfiguration->vvtMode[camIndex]) { - case VVT_FIRST_HALF: - syncAndReport(tc, getCrankDivider(operationMode), 1 PASS_ENGINE_PARAMETER_SUFFIX); - break; - case VVT_SECOND_HALF: - syncAndReport(tc, getCrankDivider(operationMode), 0 PASS_ENGINE_PARAMETER_SUFFIX); - break; - case VVT_MIATA_NB2: - /** - * NB2 is a symmetrical crank, there are four phases total - */ - syncAndReport(tc, getCrankDivider(operationMode), miataNbIndex PASS_ENGINE_PARAMETER_SUFFIX); - break; - case VVT_NISSAN_VQ: - syncAndReport(tc, getCrankDivider(operationMode), 0 PASS_ENGINE_PARAMETER_SUFFIX); - break; - default: - case VVT_INACTIVE: - // do nothing - break; - } - + angle_t crankOffset = adjustCrankPhase(camIndex PASS_ENGINE_PARAMETER_SUFFIX); + // vvtPosition was calculated against wrong crank zero position. Now that we have adjusted crank position we + // shall adjust vvt position as well + vvtPosition -= crankOffset; + tc->vvtPosition[bankIndex][camIndex] = wrapVvt(vvtPosition); } int triggerReentraint = 0; diff --git a/firmware/controllers/trigger/trigger_decoder.cpp b/firmware/controllers/trigger/trigger_decoder.cpp index 255d924cee..bc9cc27139 100644 --- a/firmware/controllers/trigger/trigger_decoder.cpp +++ b/firmware/controllers/trigger/trigger_decoder.cpp @@ -376,9 +376,9 @@ void TriggerCentral::validateCamVvtCounters() { } } -bool TriggerState::syncSymmetricalCrank(int divider, int remainder) { +angle_t TriggerState::syncSymmetricalCrank(int divider, int remainder, angle_t engineCycle) { efiAssert(OBD_PCM_Processor_Fault, remainder < divider, "syncSymmetricalCrank", false); - bool isSync = false; + angle_t totalShift = 0; while (getTotalRevolutionCounter() % divider != remainder) { /** * we are here if we've detected the cam sensor within the wrong crank phase @@ -386,9 +386,9 @@ bool TriggerState::syncSymmetricalCrank(int divider, int remainder) { * virtual crank-based trigger */ incrementTotalEventCounter(); - isSync = true; + totalShift += engineCycle / divider; } - return isSync; + return totalShift; } void TriggerState::incrementTotalEventCounter() { diff --git a/firmware/controllers/trigger/trigger_decoder.h b/firmware/controllers/trigger/trigger_decoder.h index a05feccaa7..18f205f215 100644 --- a/firmware/controllers/trigger/trigger_decoder.h +++ b/firmware/controllers/trigger/trigger_decoder.h @@ -90,7 +90,7 @@ public: * this is important for crank-based virtual trigger and VVT magic */ void incrementTotalEventCounter(); - bool syncSymmetricalCrank(int divider, int remainder); + angle_t syncSymmetricalCrank(int divider, int remainder, angle_t engineCycle); efitime_t getTotalEventCounter() const; diff --git a/unit_tests/tests/trigger/test_cam_vvt_input.cpp b/unit_tests/tests/trigger/test_cam_vvt_input.cpp index 58df051699..6f4a27096e 100644 --- a/unit_tests/tests/trigger/test_cam_vvt_input.cpp +++ b/unit_tests/tests/trigger/test_cam_vvt_input.cpp @@ -117,7 +117,7 @@ TEST(trigger, testCamInput) { // asserting that error code has cleared ASSERT_EQ(0, unitTestWarningCodeState.recentWarnings.getCount()) << "warningCounter#testCamInput #3"; - ASSERT_NEAR(360 - 181, engine->triggerCentral.getVVTPosition(0, 0), EPS3D); + ASSERT_NEAR(-181, engine->triggerCentral.getVVTPosition(0, 0), EPS3D); } TEST(trigger, testNB2CamInput) { @@ -172,7 +172,7 @@ TEST(trigger, testNB2CamInput) { eth.moveTimeForwardUs(MS2US( 30)); hwHandleVvtCamSignal(TV_RISE, getTimeNowNt(), 0 PASS_ENGINE_PARAMETER_SUFFIX); - EXPECT_NEAR(148.4f, engine->triggerCentral.getVVTPosition(0, 0), EPS3D); + EXPECT_NEAR(-211.59f, engine->triggerCentral.getVVTPosition(0, 0), EPS2D); // actually position based on VVT! ASSERT_EQ(totalRevolutionCountBeforeVvtSync + 2, engine->triggerCentral.triggerState.getTotalRevolutionCounter()); diff --git a/unit_tests/tests/trigger/test_quad_cam.cpp b/unit_tests/tests/trigger/test_quad_cam.cpp index d60092455c..5b8668f237 100644 --- a/unit_tests/tests/trigger/test_quad_cam.cpp +++ b/unit_tests/tests/trigger/test_quad_cam.cpp @@ -64,7 +64,7 @@ TEST(trigger, testQuadCam) { float basePos = -80.2f; // All four cams should now have the same position - EXPECT_NEAR(basePos, engine->triggerCentral.getVVTPosition(firstBank, firstCam), EPS3D); + EXPECT_NEAR(360 + basePos, engine->triggerCentral.getVVTPosition(firstBank, firstCam), EPS3D); EXPECT_NEAR(basePos, engine->triggerCentral.getVVTPosition(firstBank, secondCam), EPS3D); EXPECT_NEAR(basePos, engine->triggerCentral.getVVTPosition(secondBank, firstCam), EPS3D); EXPECT_NEAR(basePos, engine->triggerCentral.getVVTPosition(secondBank, secondCam), EPS3D); 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 996e0d1937..1e0d891c86 100644 --- a/unit_tests/tests/trigger/test_real_cranking_nissan_vq40.cpp +++ b/unit_tests/tests/trigger/test_real_cranking_nissan_vq40.cpp @@ -15,8 +15,16 @@ TEST(realCrankingVQ40, normalCranking) { reader.open("tests/trigger/resources/nissan_vq40_cranking-1.csv", indeces); WITH_ENGINE_TEST_HELPER (HELLEN_121_NISSAN); + bool hasSeenFirstVvt = false; + while (reader.haveMore()) { reader.processLine(ð); + float vvt1 = engine->triggerCentral.getVVTPosition(/*bankIndex*/0, /*camIndex*/0); + + if (vvt1 != 0 && !hasSeenFirstVvt) { + EXPECT_NEAR(vvt1, -38.69, 1); + hasSeenFirstVvt = true; + } } EXPECT_NEAR(engine->triggerCentral.getVVTPosition(/*bankIndex*/0, /*camIndex*/0), -46.817, 1e-2);