Injector scheduled by angle, not tooth index (#4208)

* trigger decoder returns a result

* TriggerFormDetails

* s

* don't reach out and touch the engine

* injection doesn't care about teeth

* fix up existing tests

* oh look, we can actually test this logic now without trying to depend on trigger shape!!!

* fix a real bug

* simplify

firmware/controllers/algo/event_registry.h

@@ -24,7 +24,7 @@ public:
 	InjectionEvent();
 
 	// Call this every decoded trigger tooth.  It will schedule any relevant events for this injector.
-	void onTriggerTooth(size_t toothIndex, int rpm, efitick_t nowNt);
+	void onTriggerTooth(int rpm, efitick_t nowNt, float currentPhase, float nextPhase);
 
 	/**
 	 * This is a performance optimization for IM_SIMULTANEOUS fuel strategy.
@@ -34,7 +34,8 @@ public:
 	InjectorOutputPin *outputs[MAX_WIRES_COUNT];
 	uint8_t ownIndex = 0;
 	uint8_t cylinderNumber = 0;
-	event_trigger_position_s injectionStart;
+
+	float injectionStartAngle = 0;
 
 	scheduling_s signalTimerUp;
 	scheduling_s endOfInjectionEvent;
@@ -62,7 +63,7 @@ public:
 	void invalidate();
 
 	// Call this every trigger tooth.  It will schedule all required injector events.
-	void onTriggerTooth(size_t toothIndex, int rpm, efitick_t nowNt);
+	void onTriggerTooth(int rpm, efitick_t nowNt, float currentPhase, float nextPhase);
 
 	/**
 	 * this method schedules all fuel events for an engine cycle

firmware/controllers/engine_cycle/fuel_schedule.cpp

@@ -138,9 +138,12 @@ bool FuelSchedule::addFuelEventsForCylinder(int i ) {
 
 	efiAssert(CUSTOM_ERR_ASSERT, !cisnan(openingAngle), "findAngle#3", false);
 	assertAngleRange(openingAngle, "findAngle#a33", CUSTOM_ERR_6544);
-	ev->injectionStart.setAngle(openingAngle);
+
+	wrapAngle2(openingAngle, "addFuel#2", CUSTOM_ERR_6555, getEngineCycle(engine->triggerCentral.triggerShape.getOperationMode()));
+	ev->injectionStartAngle = openingAngle;
+
 #if EFI_UNIT_TEST
-	printf("registerInjectionEvent openingAngle=%.2f trgIndex=%d inj %d\r\n", openingAngle, ev->injectionStart.triggerEventIndex, injectorIndex);
+	printf("registerInjectionEvent openingAngle=%.2f inj %d\r\n", openingAngle, injectorIndex);
 #endif
 	return true;
 }
@@ -159,14 +162,14 @@ void FuelSchedule::addFuelEvents() {
 	isReady = true;
 }
 
-void FuelSchedule::onTriggerTooth(size_t toothIndex, int rpm, efitick_t nowNt) {
+void FuelSchedule::onTriggerTooth(int rpm, efitick_t nowNt, float currentPhase, float nextPhase) {
 	// Wait for schedule to be built - this happens the first time we get RPM
 	if (!isReady) {
 		return;
 	}
 
 	for (size_t i = 0; i < engineConfiguration->specs.cylindersCount; i++) {
-		elements[i].onTriggerTooth(toothIndex, rpm, nowNt);
+		elements[i].onTriggerTooth(rpm, nowNt, currentPhase, nextPhase);
 	}
 }
 

firmware/controllers/engine_cycle/main_trigger_callback.cpp

@@ -156,11 +156,30 @@ void turnInjectionPinLow(InjectionEvent *event) {
 	engine->injectionEvents.addFuelEventsForCylinder(event->ownIndex);
 }
 
-void InjectionEvent::onTriggerTooth(size_t trgEventIndex, int rpm, efitick_t nowNt) {
+static bool isPhaseInRange(float test, float current, float next) {
-	uint32_t eventIndex = injectionStart.triggerEventIndex;
+	bool afterCurrent = test >= current;
-// right after trigger change we are still using old & invalid fuel schedule. good news is we do not change trigger on the fly in real life
+	bool beforeNext = test < next;
-//		efiAssertVoid(CUSTOM_ERR_ASSERT_VOID, eventIndex < engine->triggerShape.getLength(), "handleFuel/event sch index");
+
-	if (eventIndex != trgEventIndex) {
+	if (next > current) {
+		// we're not near the end of the cycle, comparison is simple
+		// 0            |------------------------|       720
+		//            next                    current
+		return afterCurrent && beforeNext;
+	} else {
+		// we're near the end of the cycle so we have to check the wraparound
+		// 0 -----------|                        |------ 720
+		//            next                    current
+		// Check whether test is after current (ie, between current tooth and end of cycle)
+		// or if test if before next (ie, between start of cycle and next tooth)
+		return afterCurrent || beforeNext;
+	}
+}
+
+void InjectionEvent::onTriggerTooth(int rpm, efitick_t nowNt, float currentPhase, float nextPhase) {
+	auto eventAngle = injectionStartAngle;
+
+	// Determine whether our angle is going to happen before (or near) the next tooth
+	if (!isPhaseInRange(eventAngle, currentPhase, nextPhase)) {
 		return;
 	}
 
@@ -174,8 +193,7 @@ void InjectionEvent::onTriggerTooth(size_t trgEventIndex, int rpm, efitick_t now
 
 #if EFI_PRINTF_FUEL_DETAILS
 	if (printFuelDebug) {
-		printf("fuel index=%d injectionDuration=%.2fms adjusted=%.2fms\n",
+		printf("fuel injectionDuration=%.2fms adjusted=%.2fms\n",
-		  eventIndex,
 		  engine->injectionDuration,
 		  injectionDuration);
 	}
@@ -261,12 +279,17 @@ void InjectionEvent::onTriggerTooth(size_t trgEventIndex, int rpm, efitick_t now
 		endAction = { &turnInjectionPinLow, this };
 	}
 
-	efitick_t startTime = scheduleByAngle(&signalTimerUp, nowNt, injectionStart.angleOffsetFromTriggerEvent, startAction);
+	float angleFromNow = eventAngle - currentPhase;
+	if (angleFromNow < 0) {
+		angleFromNow += engine->engineCycle;
+	}
+
+	efitick_t startTime = scheduleByAngle(&signalTimerUp, nowNt, angleFromNow, startAction);
 	efitick_t turnOffTime = startTime + US2NT((int)durationUs);
 	engine->executor.scheduleByTimestampNt("inj", &endOfInjectionEvent, turnOffTime, endAction);
 
 #if EFI_UNIT_TEST
-		printf("scheduling injection angle=%.2f/delay=%.2f injectionDuration=%.2f\r\n", injectionStart.angleOffsetFromTriggerEvent, NT2US(startTime - nowNt), injectionDuration);
+		printf("scheduling injection angle=%.2f/delay=%.2f injectionDuration=%.2f\r\n", angleFromNow, NT2US(startTime - nowNt), injectionDuration);
 #endif
 #if EFI_DEFAILED_LOGGING
 	efiPrintf("handleFuel pin=%s eventIndex %d duration=%.2fms %d", outputs[0]->name,
@@ -278,7 +301,7 @@ void InjectionEvent::onTriggerTooth(size_t trgEventIndex, int rpm, efitick_t now
 #endif /* EFI_DEFAILED_LOGGING */
 }
 
-static void handleFuel(const bool limitedFuel, uint32_t trgEventIndex, int rpm, efitick_t nowNt) {
+static void handleFuel(const bool limitedFuel, uint32_t trgEventIndex, int rpm, efitick_t nowNt, float currentPhase, float nextPhase) {
 	ScopePerf perf(PE::HandleFuel);
 	
 	efiAssertVoid(CUSTOM_STACK_6627, getCurrentRemainingStack() > 128, "lowstck#3");
@@ -307,7 +330,7 @@ static void handleFuel(const bool limitedFuel, uint32_t trgEventIndex, int rpm,
 	}
 #endif /* FUEL_MATH_EXTREME_LOGGING */
 
-	fs->onTriggerTooth(trgEventIndex, rpm, nowNt);
+	fs->onTriggerTooth(rpm, nowNt, currentPhase, nextPhase);
 }
 
 #if EFI_PROD_CODE
@@ -342,7 +365,7 @@ bool noFiringUntilVvtSync(vvt_mode_e vvtMode) {
  * This is the main trigger event handler.
  * Both injection and ignition are controlled from this method.
  */
-void mainTriggerCallback(uint32_t trgEventIndex, efitick_t edgeTimestamp) {
+void mainTriggerCallback(uint32_t trgEventIndex, efitick_t edgeTimestamp, float currentPhase, float nextPhase) {
 	ScopePerf perf(PE::MainTriggerCallback);
 
 #if ! HW_CHECK_MODE
@@ -404,7 +427,7 @@ void mainTriggerCallback(uint32_t trgEventIndex, efitick_t edgeTimestamp) {
 	 * For fuel we schedule start of injection based on trigger angle, and then inject for
 	 * specified duration of time
 	 */
-	handleFuel(limitedFuel, trgEventIndex, rpm, edgeTimestamp);
+	handleFuel(limitedFuel, trgEventIndex, rpm, edgeTimestamp, currentPhase, nextPhase);
 
 	engine->module<TriggerScheduler>()->scheduleEventsUntilNextTriggerTooth(
 		rpm, trgEventIndex, edgeTimestamp);

firmware/controllers/engine_cycle/main_trigger_callback.h

@@ -13,7 +13,7 @@
 
 void initMainEventListener();
 
-void mainTriggerCallback(uint32_t trgEventIndex, efitick_t edgeTimestamp);
+void mainTriggerCallback(uint32_t trgEventIndex, efitick_t edgeTimestamp, float currentPhase, float nextPhase);
 bool noFiringUntilVvtSync(vvt_mode_e vvtMode);
 
 void startSimultaniousInjection(void* = nullptr);

firmware/controllers/trigger/trigger_central.cpp

@@ -718,12 +718,24 @@ void TriggerCentral::handleShaftSignal(trigger_event_e signal, efitick_t timesta
 		waTriggerEventListener(signal, triggerIndexForListeners, timestamp);
 #endif
 
+		// TODO: is this logic to compute next trigger tooth angle correct?
+		auto nextToothIndex = triggerIndexForListeners;
+		float nextPhase = 0;
+
+		do {
+			// I don't love this.
+			nextToothIndex = (nextToothIndex + 1) % engine->engineCycleEventCount;
+			nextPhase = engine->triggerCentral.triggerFormDetails.eventAngles[nextToothIndex] - tdcPosition();
+			wrapAngle(nextPhase, "nextEnginePhase", CUSTOM_ERR_6555);
+		} while (nextPhase == currentPhase);
+
 		// Handle ignition and injection
-		mainTriggerCallback(triggerIndexForListeners, timestamp);
+		mainTriggerCallback(triggerIndexForListeners, timestamp, currentPhase, nextPhase);
 
 		// Decode the MAP based "cam" sensor
 		decodeMapCam(timestamp, currentPhase);
 	} else {
+		// We don't have sync, but report to the wave chart anyway as index 0.
 		reportEventToWaveChart(signal, 0);
 	}
 }

unit_tests/tests/trigger/test_injection_scheduling.cpp

@@ -6,7 +6,7 @@ using ::testing::_;
 using ::testing::StrictMock;
 using ::testing::InSequence;
 
-TEST(injectionScheduling, NormalDutyCycle) {
+TEST(injectionScheduling, InjectionIsScheduled) {
 	StrictMock<MockExecutor> mockExec;
 
 	EngineTestHelper eth(TEST_ENGINE);
@@ -24,17 +24,136 @@ TEST(injectionScheduling, NormalDutyCycle) {
 	EXPECT_CALL(im, getInjectionDuration(_)).WillOnce(Return(20.0f));
 	engine->module<InjectorModel>().set(&im);
 
+	engine->rpmCalculator.oneDegreeUs = 100;
+
 	{
 		InSequence is;
 
 		// Should schedule one normal injection:
-		// rising edge now
+		// rising edge 5 degrees from now
-		EXPECT_CALL(mockExec, scheduleByTimestampNt(testing::NotNull(), &event.signalTimerUp, nowNt + 0, _));
+		float nt5deg = USF2NT(engine->rpmCalculator.oneDegreeUs * 5);
-		// falling edge 10ms later
+		efitick_t startTime = nowNt + nt5deg;
-		EXPECT_CALL(mockExec, scheduleByTimestampNt(testing::NotNull(), &event.endOfInjectionEvent, nowNt + MS2NT(20), _));
+		EXPECT_CALL(mockExec, scheduleByTimestampNt(testing::NotNull(), &event.signalTimerUp, startTime, _));
+		// falling edge 20ms later
+		EXPECT_CALL(mockExec, scheduleByTimestampNt(testing::NotNull(), &event.endOfInjectionEvent, startTime + MS2NT(20), _));
 	}
 
+	
+	// Event scheduled at 125 degrees
+	event.injectionStartAngle = 125;
+
+	// We are at 120 degrees now, next tooth 130
+	event.onTriggerTooth(1000, nowNt, 120, 130);
+}
+
+TEST(injectionScheduling, InjectionIsScheduledBeforeWraparound) {
+	StrictMock<MockExecutor> mockExec;
+
+	EngineTestHelper eth(TEST_ENGINE);
+	engine->executor.setMockExecutor(&mockExec);
+
+	efitick_t nowNt = 1000000;
+
+	InjectionEvent event;
+	InjectorOutputPin pin;
+	pin.injectorIndex = 0;
+	event.outputs[0] = &pin;
+
+	// Injection duration of 20ms
+	MockInjectorModel2 im;
+	EXPECT_CALL(im, getInjectionDuration(_)).WillOnce(Return(20.0f));
+	engine->module<InjectorModel>().set(&im);
+
 	engine->rpmCalculator.oneDegreeUs = 100;
 
-	event.onTriggerTooth(0, 1000, nowNt);
+	{
+		InSequence is;
+
+		// Should schedule one normal injection:
+		// rising edge 5 degrees from now
+		float nt5deg = USF2NT(engine->rpmCalculator.oneDegreeUs * 5);
+		efitick_t startTime = nowNt + nt5deg;
+		EXPECT_CALL(mockExec, scheduleByTimestampNt(testing::NotNull(), &event.signalTimerUp, startTime, _));
+		// falling edge 20ms later
+		EXPECT_CALL(mockExec, scheduleByTimestampNt(testing::NotNull(), &event.endOfInjectionEvent, startTime + MS2NT(20), _));
+	}
+
+	// Event scheduled at 715 degrees
+	event.injectionStartAngle = 715;
+
+	// We are at 710 degrees now, next tooth 010
+	event.onTriggerTooth(1000, nowNt, 710, 010);
 }
+
+TEST(injectionScheduling, InjectionIsScheduledAfterWraparound) {
+	StrictMock<MockExecutor> mockExec;
+
+	EngineTestHelper eth(TEST_ENGINE);
+	engine->executor.setMockExecutor(&mockExec);
+
+	efitick_t nowNt = 1000000;
+
+	InjectionEvent event;
+	InjectorOutputPin pin;
+	pin.injectorIndex = 0;
+	event.outputs[0] = &pin;
+
+	// Injection duration of 20ms
+	MockInjectorModel2 im;
+	EXPECT_CALL(im, getInjectionDuration(_)).WillOnce(Return(20.0f));
+	engine->module<InjectorModel>().set(&im);
+
+	engine->rpmCalculator.oneDegreeUs = 100;
+
+	{
+		InSequence is;
+
+		// Should schedule one normal injection:
+		// rising edge 15 degrees from now
+		float nt5deg = USF2NT(engine->rpmCalculator.oneDegreeUs * 15);
+		efitick_t startTime = nowNt + nt5deg;
+		EXPECT_CALL(mockExec, scheduleByTimestampNt(testing::NotNull(), &event.signalTimerUp, startTime, _));
+		// falling edge 20ms later
+		EXPECT_CALL(mockExec, scheduleByTimestampNt(testing::NotNull(), &event.endOfInjectionEvent, startTime + MS2NT(20), _));
+	}
+
+	// Event scheduled at 5 degrees
+	event.injectionStartAngle = 5;
+
+	// We are at 710 degrees now, next tooth 010
+	event.onTriggerTooth(1000, nowNt, 710, 010);
+}
+
+TEST(injectionScheduling, InjectionNotScheduled) {
+	// StrictMock since we expect no scheduler calls!
+	StrictMock<MockExecutor> mockExec;
+
+	EngineTestHelper eth(TEST_ENGINE);
+	engine->executor.setMockExecutor(&mockExec);
+
+	efitick_t nowNt = 1000000;
+
+	InjectionEvent event;
+	InjectorOutputPin pin;
+	pin.injectorIndex = 0;
+	event.outputs[0] = &pin;
+
+	// Expect no calls to injector model
+	StrictMock<MockInjectorModel2> im;
+	engine->module<InjectorModel>().set(&im);
+
+	engine->rpmCalculator.oneDegreeUs = 100;
+
+	{
+		InSequence is;
+
+		// Expect no scheduler calls!
+	}
+
+	
+	// Event scheduled at 125 degrees
+	event.injectionStartAngle = 125;
+
+	// We are at 130 degrees now, next tooth 140
+	event.onTriggerTooth(1000, nowNt, 130, 140);
+}

unit_tests/tests/trigger/test_trigger_decoder.cpp

@@ -310,7 +310,7 @@ TEST(misc, testRpmCalculator) {
 
 	assertEqualsM("fuel #1", 4.5450, engine->injectionDuration);
 	InjectionEvent *ie0 = &engine->injectionEvents.elements[0];
-	assertEqualsM("injection angle", 4.095, ie0->injectionStart.angleOffsetFromTriggerEvent);
+	assertEqualsM("injection angle", 499.095, ie0->injectionStartAngle);
 
 	eth.firePrimaryTriggerRise();
 	ASSERT_EQ(1500, Sensor::getOrZero(SensorType::Rpm));
@@ -490,9 +490,8 @@ TEST(trigger, testTriggerDecoder) {
 }
 
 static void assertInjectionEventBase(const char *msg, InjectionEvent *ev, int injectorIndex, int eventIndex, angle_t angleOffset) {
-	ASSERT_EQ(injectorIndex, ev->outputs[0]->injectorIndex) << msg << "inj index";
+	EXPECT_EQ(injectorIndex, ev->outputs[0]->injectorIndex) << msg << "inj index";
-	assertEqualsM4(msg, " event index", eventIndex, ev->injectionStart.triggerEventIndex);
+	EXPECT_NEAR_M4(angleOffset, ev->injectionStartAngle) << msg << "inj index";
-	assertEqualsM4(msg, " event offset", angleOffset, ev->injectionStart.angleOffsetFromTriggerEvent);
 }
 
 static void assertInjectionEvent(const char *msg, InjectionEvent *ev, int injectorIndex, int eventIndex, angle_t angleOffset) {
@@ -547,8 +546,8 @@ static void setTestBug299(EngineTestHelper *eth) {
 
 	FuelSchedule * t = &engine->injectionEvents;
 
-	assertInjectionEvent("#0", &t->elements[0], 0, 1, 153);
+	assertInjectionEvent("#0", &t->elements[0], 0, 1, 153 + 360);
-	assertInjectionEvent("#1_i_@", &t->elements[1], 1, 1, 333);
+	assertInjectionEvent("#1_i_@", &t->elements[1], 1, 1, 333 + 360);
 	assertInjectionEvent("#2@", &t->elements[2], 0, 0, 153);
 	assertInjectionEvent("inj#3@", &t->elements[3], 1, 0, 153 + 180);
 
@@ -623,8 +622,8 @@ static void setTestBug299(EngineTestHelper *eth) {
 }
 
 static void assertInjectors(const char *msg, int value0, int value1) {
-	assertEqualsM4(msg, "inj#0", value0, enginePins.injectors[0].currentLogicValue);
+	EXPECT_EQ(value0, enginePins.injectors[0].currentLogicValue);
-	assertEqualsM4(msg, "inj#1", value1, enginePins.injectors[1].currentLogicValue);
+	EXPECT_EQ(value1, enginePins.injectors[1].currentLogicValue);
 }
 
 static void setArray(float* p, size_t count, float value) {
@@ -716,7 +715,7 @@ void doTestFuelSchedulerBug299smallAndMedium(int startUpDelayMs) {
 	t = &engine->injectionEvents;
 
 	assertInjectionEvent("#0", &t->elements[0], 0, 0, 315);
-	assertInjectionEvent("#1__", &t->elements[1], 1, 1, 135);
+	assertInjectionEvent("#1__", &t->elements[1], 1, 1, 495);
 	assertInjectionEvent("inj#2", &t->elements[2], 0, 0, 153);
 	assertInjectionEvent("inj#3", &t->elements[3], 1, 0, 333);
 
@@ -803,10 +802,10 @@ void doTestFuelSchedulerBug299smallAndMedium(int startUpDelayMs) {
 
 	t = &engine->injectionEvents;
 
-	assertInjectionEvent("#0#", &t->elements[0], 0, 0, 315);
+	assertInjectionEvent("#0#", &t->elements[0], 0, 0, 135 + 180);
-	assertInjectionEvent("#1#", &t->elements[1], 1, 1, 135);
+	assertInjectionEvent("#1#", &t->elements[1], 1, 1, 135 + 360);
-	assertInjectionEvent("#2#", &t->elements[2], 0, 1, 315);
+	assertInjectionEvent("#2#", &t->elements[2], 0, 1, 135 + 540);
-	assertInjectionEvent("#3#", &t->elements[3], 1, 0, 45 + 90);
+	assertInjectionEvent("#3#", &t->elements[3], 1, 0, 135);
 
 	engine->injectionDuration = 17.5;
 	// Injection duration of 17.5ms
@@ -849,8 +848,8 @@ void doTestFuelSchedulerBug299smallAndMedium(int startUpDelayMs) {
 	t = &engine->injectionEvents;
 
 	assertInjectionEvent("#00", &t->elements[0], 0, 0, 225); // 87.5 duty cycle
-	assertInjectionEvent("#10", &t->elements[1], 1, 1, 45);
+	assertInjectionEvent("#10", &t->elements[1], 1, 1, 45  + 360);
-	assertInjectionEvent("#20", &t->elements[2], 0, 1, 225);
+	assertInjectionEvent("#20", &t->elements[2], 0, 1, 225 + 360);
 	assertInjectionEvent("#30", &t->elements[3], 1, 0, 45);
 
 	 // todo: what's what? a mix of new something and old something?
@@ -903,8 +902,8 @@ TEST(big, testTwoWireBatch) {
 
 	FuelSchedule * t = &engine->injectionEvents;
 
-	assertInjectionEventBatch("#0", &t->elements[0],		0, 3, 1, 153);	// Cyl 1 and 4
+	assertInjectionEventBatch("#0", &t->elements[0],		0, 3, 1, 153 + 360);	// Cyl 1 and 4
-	assertInjectionEventBatch("#1_i_@", &t->elements[1],	2, 1, 1, 153 + 180);	// Cyl 3 and 2
+	assertInjectionEventBatch("#1_i_@", &t->elements[1],	2, 1, 1, 153 + 540);	// Cyl 3 and 2
 	assertInjectionEventBatch("#2@", &t->elements[2],		3, 0, 0, 153);	// Cyl 4 and 1
 	assertInjectionEventBatch("inj#3@", &t->elements[3],	1, 2, 0, 153 + 180);	// Cyl 2 and 3
 }
@@ -931,8 +930,8 @@ TEST(big, testSequential) {
 
 	FuelSchedule * t = &engine->injectionEvents;
 
-	assertInjectionEvent("#0", &t->elements[0],		0, 1, 126);	// Cyl 1
+	assertInjectionEvent("#0", &t->elements[0],		0, 1, 126 + 360);	// Cyl 1
-	assertInjectionEvent("#1_i_@", &t->elements[1],	2, 1, 126 + 180);	// Cyl 3
+	assertInjectionEvent("#1_i_@", &t->elements[1],	2, 1, 126 + 540);	// Cyl 3
 	assertInjectionEvent("#2@", &t->elements[2],	3, 0, 126);	// Cyl 4
 	assertInjectionEvent("inj#3@", &t->elements[3],	1, 0, 126 + 180);	// Cyl 2
 }