From af3118fd42cc729b211593d6ac9a04ffac362c42 Mon Sep 17 00:00:00 2001 From: rusefi Date: Sat, 23 Nov 2019 20:36:40 -0500 Subject: [PATCH] making progres on angle-based scheduling --- firmware/controllers/algo/engine.cpp | 9 ++ firmware/controllers/algo/engine.h | 2 +- firmware/controllers/algo/event_registry.h | 8 +- .../controllers/scheduling/event_queue.cpp | 2 +- firmware/controllers/scheduling/scheduler.h | 4 +- .../scheduling/signal_executor_sleep.cpp | 4 +- firmware/controllers/trigger/spark_logic.cpp | 128 ++++++++++-------- unit_tests/engine_test_helper.cpp | 4 +- unit_tests/tests/test_trigger_decoder.cpp | 8 +- 9 files changed, 98 insertions(+), 71 deletions(-) diff --git a/firmware/controllers/algo/engine.cpp b/firmware/controllers/algo/engine.cpp index 578cfebaec..c577b96f5b 100644 --- a/firmware/controllers/algo/engine.cpp +++ b/firmware/controllers/algo/engine.cpp @@ -392,3 +392,12 @@ void action_s::execute() { efiAssertVoid(CUSTOM_ERR_ASSERT, callback != NULL, "callback==null1"); callback(param); } + +schfunc_t action_s::getCallback() const { + return callback; +} + +void * action_s::getArgument() const { + return param; +} + diff --git a/firmware/controllers/algo/engine.h b/firmware/controllers/algo/engine.h index c4ff7f42bf..a9918adc59 100644 --- a/firmware/controllers/algo/engine.h +++ b/firmware/controllers/algo/engine.h @@ -104,7 +104,7 @@ public: /** * That's the linked list of pending spark firing events */ - IgnitionEvent *ignitionEventsHead = nullptr; + AngleBasedEvent *ignitionEventsHead = nullptr; /** * this is based on isEngineChartEnabled and engineSnifferRpmThreshold settings */ diff --git a/firmware/controllers/algo/event_registry.h b/firmware/controllers/algo/event_registry.h index cb62ac9dca..086bd42546 100644 --- a/firmware/controllers/algo/event_registry.h +++ b/firmware/controllers/algo/event_registry.h @@ -74,6 +74,10 @@ public: scheduling_s scheduling; event_trigger_position_s position; action_s action; + /** + * Trigger-based scheduler maintains a linked list of all pending tooth-based events. + */ + AngleBasedEvent *nextToothEvent = nullptr; }; #define MAX_OUTPUTS_FOR_IGNITION 2 @@ -94,10 +98,6 @@ public: */ uint32_t actualStartOfDwellNt; event_trigger_position_s dwellPosition; - /** - * Ignition scheduler maintains a linked list of all pending ignition events. - */ - IgnitionEvent *nextIgnitionEvent = nullptr; /** * Sequential number of currently processed spark event * @see globalSparkIdCounter diff --git a/firmware/controllers/scheduling/event_queue.cpp b/firmware/controllers/scheduling/event_queue.cpp index d631c7aff7..3367f6ec1f 100644 --- a/firmware/controllers/scheduling/event_queue.cpp +++ b/firmware/controllers/scheduling/event_queue.cpp @@ -171,7 +171,7 @@ int EventQueue::executeAll(efitime_t now) { uint32_t howFarOff = now - current->momentX; maxSchedulingPrecisionLoss = maxI(maxSchedulingPrecisionLoss, howFarOff); #if EFI_UNIT_TEST - printf("QUEUE: execute current=%d param=%d\r\n", (long)current, (long)current->action.param); + printf("QUEUE: execute current=%d param=%d\r\n", (long)current, (long)current->action.getArgument()); #endif { diff --git a/firmware/controllers/scheduling/scheduler.h b/firmware/controllers/scheduling/scheduler.h index 06676d3d66..17935acf9e 100644 --- a/firmware/controllers/scheduling/scheduler.h +++ b/firmware/controllers/scheduling/scheduler.h @@ -14,10 +14,10 @@ class action_s { public: void setAction(schfunc_t callback, void *param); void execute(); + schfunc_t getCallback() const; + void * getArgument() const; -#if EFI_PROD_CODE private: -#endif /* EFI_PROD_CODE */ schfunc_t callback = nullptr; void *param = nullptr; }; diff --git a/firmware/controllers/scheduling/signal_executor_sleep.cpp b/firmware/controllers/scheduling/signal_executor_sleep.cpp index 663e938f55..f08d30a608 100644 --- a/firmware/controllers/scheduling/signal_executor_sleep.cpp +++ b/firmware/controllers/scheduling/signal_executor_sleep.cpp @@ -39,8 +39,8 @@ void SleepExecutor::scheduleByTimestamp(scheduling_s *scheduling, efitimeus_t ti static void timerCallback(scheduling_s *scheduling) { #if EFI_PRINTF_FUEL_DETAILS - if (scheduling->action.callback == (schfunc_t)&seTurnPinLow) { - printf("executing cb=seTurnPinLow p=%d sch=%d now=%d\r\n", (int)scheduling->action.param, (int)scheduling, + if (scheduling->action.getCallback() == (schfunc_t)&seTurnPinLow) { + printf("executing cb=seTurnPinLow p=%d sch=%d now=%d\r\n", (int)scheduling->action.getArgument(), (int)scheduling, (int)getTimeNowUs()); } else { // printf("exec cb=%d p=%d\r\n", (int)scheduling->callback, (int)scheduling->param); diff --git a/firmware/controllers/trigger/spark_logic.cpp b/firmware/controllers/trigger/spark_logic.cpp index 911f008ee3..176015f60c 100644 --- a/firmware/controllers/trigger/spark_logic.cpp +++ b/firmware/controllers/trigger/spark_logic.cpp @@ -215,8 +215,56 @@ void turnSparkPinHigh(IgnitionEvent *event) { } } -static bool assertNotInIgnitionList(IgnitionEvent *head, IgnitionEvent *element) { - assertNotInListMethodBody(IgnitionEvent, head, element, nextIgnitionEvent) +static bool assertNotInIgnitionList(AngleBasedEvent *head, AngleBasedEvent *element) { + assertNotInListMethodBody(AngleBasedEvent, head, element, nextToothEvent) +} + +/** + * @return true if event corresponds to current tooth and was time-based scheduler + * false if event was put into queue for scheduling at a later tooth + */ +static bool scheduleOrQueue(AngleBasedEvent *event, uint32_t trgEventIndex, angle_t advance, schfunc_t callback, void *param DECLARE_ENGINE_PARAMETER_SUFFIX) { + TRIGGER_SHAPE(findTriggerPosition(&event->position, advance PASS_CONFIG_PARAM(engineConfiguration->globalTriggerAngleOffset))); + + /** + * todo: extract a "scheduleForAngle" method with best implementation into a separate utility method + * + * Here's the status as of Nov 2018: + * "scheduleForLater" uses time only and for best precision it's best to use "scheduleForLater" only + * once we hit the last trigger tooth prior to needed event. This case we use as much trigger position angle as possible + * and only use less precise RPM-based time calculation for the last portion of the angle, the one between two teeth closest to the + * desired angle moment. + * + * At the moment we only have time-based scheduler. I believe what needs to be added is a trigger-event based scheduler on top of the + * time-based schedule. This case we would be firing events with best possible angle precision. + * + */ + if (event->position.triggerEventIndex == trgEventIndex) { + /** + * Spark should be fired before the next trigger event - time-based delay is best precision possible + */ + float timeTillIgnitionUs = ENGINE(rpmCalculator.oneDegreeUs) * event->position.angleOffsetFromTriggerEvent; + + + scheduling_s * sDown = &event->scheduling; + + engine->executor.scheduleForLater(sDown, (int) timeTillIgnitionUs, callback, param); + return true; + } else { + event->action.setAction(callback, param); + /** + * Spark should be scheduled in relation to some future trigger event, this way we get better firing precision + */ + bool isPending = assertNotInIgnitionList(ENGINE(ignitionEventsHead), event); + if (isPending) { +#if SPARK_EXTREME_LOGGING + scheduleMsg(logger, "isPending thus nt adding to queue index=%d rev=%d now=%d", trgEventIndex, getRevolutionCounter(), (int)getTimeNowUs()); +#endif /* FUEL_MATH_EXTREME_LOGGING */ + } else { + LL_APPEND2(ENGINE(ignitionEventsHead), event, nextToothEvent); + } + return false; + } } static ALWAYS_INLINE void handleSparkEvent(bool limitedSpark, uint32_t trgEventIndex, IgnitionEvent *iEvent, @@ -277,7 +325,21 @@ static ALWAYS_INLINE void handleSparkEvent(bool limitedSpark, uint32_t trgEventI efiAssertVoid(CUSTOM_ERR_6591, !cisnan(advance), "findAngle#4"); assertAngleRange(advance, "findAngle#a5", CUSTOM_ERR_6549); - TRIGGER_SHAPE(findTriggerPosition(&iEvent->sparkEvent.position, advance PASS_CONFIG_PARAM(engineConfiguration->globalTriggerAngleOffset))); + + + bool scheduled = scheduleOrQueue(&iEvent->sparkEvent, trgEventIndex, advance, (schfunc_t)fireSparkAndPrepareNextSchedule, iEvent PASS_ENGINE_PARAMETER_SUFFIX); + + if (scheduled) { +#if SPARK_EXTREME_LOGGING + scheduleMsg(logger, "scheduling sparkDown ind=%d %d %s now=%d later id=%d", trgEventIndex, getRevolutionCounter(), iEvent->getOutputForLoggins()->name, (int)getTimeNowUs(), iEvent->sparkId); +#endif /* FUEL_MATH_EXTREME_LOGGING */ + } else { +#if SPARK_EXTREME_LOGGING + scheduleMsg(logger, "to queue sparkDown ind=%d %d %s now=%d for id=%d", trgEventIndex, getRevolutionCounter(), iEvent->getOutputForLoggins()->name, (int)getTimeNowUs(), iEvent->sparkEvent.position.triggerEventIndex); +#endif /* FUEL_MATH_EXTREME_LOGGING */ + } + + #if EFI_UNIT_TEST if (verboseMode) { @@ -286,50 +348,6 @@ static ALWAYS_INLINE void handleSparkEvent(bool limitedSpark, uint32_t trgEventI iEvent->sparkId); } #endif - - /** - * todo: extract a "scheduleForAngle" method with best implementation into a separate utility method - * - * Here's the status as of Nov 2018: - * "scheduleForLater" uses time only and for best precision it's best to use "scheduleForLater" only - * once we hit the last trigger tooth prior to needed event. This case we use as much trigger position angle as possible - * and only use less precise RPM-based time calculation for the last portion of the angle, the one between two teeth closest to the - * desired angle moment. - * - * At the moment we only have time-based scheduler. I believe what needs to be added is a trigger-event based scheduler on top of the - * time-based schedule. This case we would be firing events with best possible angle precision. - * - */ - if (iEvent->sparkEvent.position.triggerEventIndex == trgEventIndex) { - /** - * Spark should be fired before the next trigger event - time-based delay is best precision possible - */ - float timeTillIgnitionUs = ENGINE(rpmCalculator.oneDegreeUs) * iEvent->sparkEvent.position.angleOffsetFromTriggerEvent; - -#if SPARK_EXTREME_LOGGING - scheduleMsg(logger, "scheduling sparkDown ind=%d %d %s now=%d %d later id=%d", trgEventIndex, getRevolutionCounter(), iEvent->getOutputForLoggins()->name, (int)getTimeNowUs(), (int)timeTillIgnitionUs, iEvent->sparkId); -#endif /* FUEL_MATH_EXTREME_LOGGING */ - - scheduling_s * sDown = &iEvent->sparkEvent.scheduling; - - engine->executor.scheduleForLater(sDown, (int) timeTillIgnitionUs, (schfunc_t) &fireSparkAndPrepareNextSchedule, iEvent); - } else { -#if SPARK_EXTREME_LOGGING - scheduleMsg(logger, "to queue sparkDown ind=%d %d %s %d for %d", trgEventIndex, getRevolutionCounter(), iEvent->getOutputForLoggins()->name, (int)getTimeNowUs(), iEvent->sparkEvent.position.triggerEventIndex); -#endif /* FUEL_MATH_EXTREME_LOGGING */ - /** - * Spark should be scheduled in relation to some future trigger event, this way we get better firing precision - */ - bool isPending = assertNotInIgnitionList(ENGINE(ignitionEventsHead), iEvent); - if (isPending) { -#if SPARK_EXTREME_LOGGING - scheduleMsg(logger, "not adding to queue sparkDown ind=%d %d %s %d", trgEventIndex, getRevolutionCounter(), iEvent->getOutputForLoggins()->name, (int)getTimeNowUs()); -#endif /* FUEL_MATH_EXTREME_LOGGING */ - return; - } - - LL_APPEND2(ENGINE(ignitionEventsHead), iEvent, nextIgnitionEvent); - } } void initializeIgnitionActions(DECLARE_ENGINE_PARAMETER_SIGNATURE) { @@ -386,23 +404,23 @@ static ALWAYS_INLINE void prepareIgnitionSchedule(DECLARE_ENGINE_PARAMETER_SIGNA static void scheduleAllSparkEventsUntilNextTriggerTooth(uint32_t trgEventIndex DECLARE_ENGINE_PARAMETER_SUFFIX) { - IgnitionEvent *current, *tmp; + AngleBasedEvent *current, *tmp; - LL_FOREACH_SAFE2(ENGINE(ignitionEventsHead), current, tmp, nextIgnitionEvent) + LL_FOREACH_SAFE2(ENGINE(ignitionEventsHead), current, tmp, nextToothEvent) { - if (current->sparkEvent.position.triggerEventIndex == trgEventIndex) { + if (current->position.triggerEventIndex == trgEventIndex) { // time to fire a spark which was scheduled previously - LL_DELETE2(ENGINE(ignitionEventsHead), current, nextIgnitionEvent); + LL_DELETE2(ENGINE(ignitionEventsHead), current, nextToothEvent); - scheduling_s * sDown = ¤t->sparkEvent.scheduling; + scheduling_s * sDown = ¤t->scheduling; #if SPARK_EXTREME_LOGGING - scheduleMsg(logger, "time to sparkDown ind=%d %d %s %d", trgEventIndex, getRevolutionCounter(), current->getOutputForLoggins()->name, (int)getTimeNowUs()); + scheduleMsg(logger, "time to invoke ind=%d %d %d", trgEventIndex, getRevolutionCounter(), (int)getTimeNowUs()); #endif /* FUEL_MATH_EXTREME_LOGGING */ - float timeTillIgnitionUs = ENGINE(rpmCalculator.oneDegreeUs) * current->sparkEvent.position.angleOffsetFromTriggerEvent; - engine->executor.scheduleForLater(sDown, (int) timeTillIgnitionUs, (schfunc_t) &fireSparkAndPrepareNextSchedule, current); + float timeTillIgnitionUs = ENGINE(rpmCalculator.oneDegreeUs) * current->position.angleOffsetFromTriggerEvent; + engine->executor.scheduleForLater(sDown, (int) timeTillIgnitionUs, (schfunc_t) current->action.getCallback(), current->action.getArgument()); } } } diff --git a/unit_tests/engine_test_helper.cpp b/unit_tests/engine_test_helper.cpp index 4566b93f86..08c0e1f6fb 100644 --- a/unit_tests/engine_test_helper.cpp +++ b/unit_tests/engine_test_helper.cpp @@ -165,7 +165,7 @@ scheduling_s * EngineTestHelper::assertEvent5(const char *msg, int index, void * TestExecutor *executor = &engine.executor; EXPECT_TRUE(executor->size() > index) << msg; scheduling_s *event = executor->getForUnitTest(index); - assertEqualsM4(msg, " up/down", (void*)event->action.callback == (void*) callback, 1); + assertEqualsM4(msg, " up/down", (void*)event->action.getCallback() == (void*) callback, 1); efitime_t start = getTimeNowUs(); assertEqualsM(msg, expectedTimestamp, event->momentX - start); return event; @@ -174,7 +174,7 @@ scheduling_s * EngineTestHelper::assertEvent5(const char *msg, int index, void * void EngineTestHelper::assertEvent(const char *msg, int index, void *callback, efitime_t momentX, InjectionEvent *expectedEvent) { scheduling_s *event = assertEvent5(msg, index, callback, momentX); - InjectionEvent *actualEvent = (InjectionEvent *)event->action.param; + InjectionEvent *actualEvent = (InjectionEvent *)event->action.getArgument(); assertEqualsLM(msg, expectedEvent->outputs[0], (long)actualEvent->outputs[0]); // but this would not work assertEqualsLM(msg, expectedPair, (long)eventPair); diff --git a/unit_tests/tests/test_trigger_decoder.cpp b/unit_tests/tests/test_trigger_decoder.cpp index 1efe4ce7e2..805f504d44 100644 --- a/unit_tests/tests/test_trigger_decoder.cpp +++ b/unit_tests/tests/test_trigger_decoder.cpp @@ -359,14 +359,14 @@ TEST(misc, testRpmCalculator) { { scheduling_s *ev0 = engine->executor.getForUnitTest(0); - assertREqualsM("Call@0", (void*)ev0->action.callback, (void*)turnSparkPinHigh); + assertREqualsM("Call@0", (void*)ev0->action.getCallback(), (void*)turnSparkPinHigh); assertEqualsM("ev 0", start + 944, ev0->momentX); - assertEqualsLM("coil 0", (long)&enginePins.coils[0], (long)((IgnitionEvent*)ev0->action.param)->outputs[0]); + assertEqualsLM("coil 0", (long)&enginePins.coils[0], (long)((IgnitionEvent*)ev0->action.getArgument())->outputs[0]); scheduling_s *ev1 = engine->executor.getForUnitTest(1); - assertREqualsM("Call@1", (void*)ev1->action.callback, (void*)fireSparkAndPrepareNextSchedule); + assertREqualsM("Call@1", (void*)ev1->action.getCallback(), (void*)fireSparkAndPrepareNextSchedule); assertEqualsM("ev 1", start + 1444, ev1->momentX); - assertEqualsLM("coil 1", (long)&enginePins.coils[0], (long)((IgnitionEvent*)ev1->action.param)->outputs[0]); + assertEqualsLM("coil 1", (long)&enginePins.coils[0], (long)((IgnitionEvent*)ev1->action.getArgument())->outputs[0]); }