From c0f290b92292cd78a51b3843850179e820b5772f Mon Sep 17 00:00:00 2001 From: Matthew Kennedy Date: Fri, 24 Jun 2022 14:43:23 -0700 Subject: [PATCH] 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 | 7 +- .../engine_cycle/fuel_schedule.cpp | 11 +- .../engine_cycle/main_trigger_callback.cpp | 49 +++++-- .../engine_cycle/main_trigger_callback.h | 2 +- .../controllers/trigger/trigger_central.cpp | 14 +- .../trigger/test_injection_scheduling.cpp | 131 +++++++++++++++++- .../tests/trigger/test_trigger_decoder.cpp | 37 +++-- 7 files changed, 204 insertions(+), 47 deletions(-) diff --git a/firmware/controllers/algo/event_registry.h b/firmware/controllers/algo/event_registry.h index 63acbf200c..52e971c97a 100644 --- a/firmware/controllers/algo/event_registry.h +++ b/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 diff --git a/firmware/controllers/engine_cycle/fuel_schedule.cpp b/firmware/controllers/engine_cycle/fuel_schedule.cpp index 81f8ec7c31..68873d754d 100644 --- a/firmware/controllers/engine_cycle/fuel_schedule.cpp +++ b/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); } } diff --git a/firmware/controllers/engine_cycle/main_trigger_callback.cpp b/firmware/controllers/engine_cycle/main_trigger_callback.cpp index 48ce3d99af..b5ca22cf0a 100644 --- a/firmware/controllers/engine_cycle/main_trigger_callback.cpp +++ b/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) { - uint32_t eventIndex = injectionStart.triggerEventIndex; -// 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 -// efiAssertVoid(CUSTOM_ERR_ASSERT_VOID, eventIndex < engine->triggerShape.getLength(), "handleFuel/event sch index"); - if (eventIndex != trgEventIndex) { +static bool isPhaseInRange(float test, float current, float next) { + bool afterCurrent = test >= current; + bool beforeNext = test < next; + + 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", - eventIndex, + printf("fuel injectionDuration=%.2fms adjusted=%.2fms\n", 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()->scheduleEventsUntilNextTriggerTooth( rpm, trgEventIndex, edgeTimestamp); diff --git a/firmware/controllers/engine_cycle/main_trigger_callback.h b/firmware/controllers/engine_cycle/main_trigger_callback.h index 56585b5fd4..def606d2f0 100644 --- a/firmware/controllers/engine_cycle/main_trigger_callback.h +++ b/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); diff --git a/firmware/controllers/trigger/trigger_central.cpp b/firmware/controllers/trigger/trigger_central.cpp index cf8df0c69f..0ff0343166 100644 --- a/firmware/controllers/trigger/trigger_central.cpp +++ b/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); } } diff --git a/unit_tests/tests/trigger/test_injection_scheduling.cpp b/unit_tests/tests/trigger/test_injection_scheduling.cpp index d199ff1bf6..26228e9c51 100644 --- a/unit_tests/tests/trigger/test_injection_scheduling.cpp +++ b/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 mockExec; EngineTestHelper eth(TEST_ENGINE); @@ -24,17 +24,136 @@ TEST(injectionScheduling, NormalDutyCycle) { EXPECT_CALL(im, getInjectionDuration(_)).WillOnce(Return(20.0f)); engine->module().set(&im); + engine->rpmCalculator.oneDegreeUs = 100; + { InSequence is; // Should schedule one normal injection: - // rising edge now - EXPECT_CALL(mockExec, scheduleByTimestampNt(testing::NotNull(), &event.signalTimerUp, nowNt + 0, _)); - // falling edge 10ms later - EXPECT_CALL(mockExec, scheduleByTimestampNt(testing::NotNull(), &event.endOfInjectionEvent, nowNt + MS2NT(20), _)); + // 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 125 degrees + event.injectionStartAngle = 125; + + // We are at 120 degrees now, next tooth 130 + event.onTriggerTooth(1000, nowNt, 120, 130); +} + +TEST(injectionScheduling, InjectionIsScheduledBeforeWraparound) { + StrictMock 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().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 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().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 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 im; + engine->module().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); +} \ No newline at end of file diff --git a/unit_tests/tests/trigger/test_trigger_decoder.cpp b/unit_tests/tests/trigger/test_trigger_decoder.cpp index 19fa41a2c2..176b1af207 100644 --- a/unit_tests/tests/trigger/test_trigger_decoder.cpp +++ b/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"; - assertEqualsM4(msg, " event index", eventIndex, ev->injectionStart.triggerEventIndex); - assertEqualsM4(msg, " event offset", angleOffset, ev->injectionStart.angleOffsetFromTriggerEvent); + EXPECT_EQ(injectorIndex, ev->outputs[0]->injectorIndex) << msg << "inj index"; + EXPECT_NEAR_M4(angleOffset, ev->injectionStartAngle) << msg << "inj index"; } 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("#1_i_@", &t->elements[1], 1, 1, 333); + assertInjectionEvent("#0", &t->elements[0], 0, 1, 153 + 360); + 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); - assertEqualsM4(msg, "inj#1", value1, enginePins.injectors[1].currentLogicValue); + EXPECT_EQ(value0, enginePins.injectors[0].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("#1#", &t->elements[1], 1, 1, 135); - assertInjectionEvent("#2#", &t->elements[2], 0, 1, 315); - assertInjectionEvent("#3#", &t->elements[3], 1, 0, 45 + 90); + assertInjectionEvent("#0#", &t->elements[0], 0, 0, 135 + 180); + assertInjectionEvent("#1#", &t->elements[1], 1, 1, 135 + 360); + assertInjectionEvent("#2#", &t->elements[2], 0, 1, 135 + 540); + 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("#20", &t->elements[2], 0, 1, 225); + assertInjectionEvent("#10", &t->elements[1], 1, 1, 45 + 360); + 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("#1_i_@", &t->elements[1], 2, 1, 1, 153 + 180); // Cyl 3 and 2 + assertInjectionEventBatch("#0", &t->elements[0], 0, 3, 1, 153 + 360); // Cyl 1 and 4 + 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("#1_i_@", &t->elements[1], 2, 1, 126 + 180); // Cyl 3 + assertInjectionEvent("#0", &t->elements[0], 0, 1, 126 + 360); // Cyl 1 + 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 }