VVT position of first cam is off on first cycle fix #2987

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;

firmware/controllers/trigger/trigger_decoder.cpp

@@ -376,19 +376,19 @@ 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
 		 * let's increase the trigger event counter, that would adjust the state of
 		 * virtual crank-based trigger
 		 */
 		incrementTotalEventCounter();
-		isSync = true;
+		totalShift += engineCycle / divider;
 	}
-	return isSync;
+	return totalShift;
 }
 
 void TriggerState::incrementTotalEventCounter() {

firmware/controllers/trigger/trigger_decoder.h

@@ -90,7 +90,7 @@ class TriggerState : public trigger_state_s {
 	 * 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;
 

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());
 

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);

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(&eth);
+		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);