diff --git a/firmware/controllers/algo/engine.h b/firmware/controllers/algo/engine.h index df0a141b67..32482685d2 100644 --- a/firmware/controllers/algo/engine.h +++ b/firmware/controllers/algo/engine.h @@ -26,6 +26,7 @@ #include "idle_thread.h" #include "injector_model.h" #include "launch_control.h" +#include "trigger_scheduler.h" #include "type_list.h" #ifndef EFI_UNIT_TEST @@ -128,6 +129,7 @@ public: #if EFI_IDLE_CONTROL IdleController, #endif + TriggerScheduler, EngineModule // dummy placeholder so the previous entries can all have commas > engineModules; @@ -213,12 +215,6 @@ public: bool needToStopEngine(efitick_t nowNt) const; bool etbAutoTune = false; - /** - * That's the linked list of pending events scheduled in relation to trigger - * At the moment we iterate over the whole list while looking for events for specific trigger index - * We can make it an array of lists per trigger index, but that would take some RAM and probably not needed yet. - */ - AngleBasedEvent *angleBasedEventsHead = nullptr; /** * this is based on isEngineChartEnabled and engineSnifferRpmThreshold settings */ diff --git a/firmware/controllers/engine_cycle/aux_valves.cpp b/firmware/controllers/engine_cycle/aux_valves.cpp index ab1fee66fb..a168ca0453 100644 --- a/firmware/controllers/engine_cycle/aux_valves.cpp +++ b/firmware/controllers/engine_cycle/aux_valves.cpp @@ -25,7 +25,7 @@ static void plainPinTurnOff(NamedOutputPin *output) { static void scheduleOpen(AuxActor *current) { - scheduleOrQueue(¤t->open, + engine->module()->scheduleOrQueue(¤t->open, TRIGGER_EVENT_UNDEFINED, getTimeNowNt(), current->extra + engine->engineState.auxValveStart, @@ -43,7 +43,7 @@ void auxPlainPinTurnOn(AuxActor *current) { fixAngle(duration, "duration", CUSTOM_ERR_6557); - scheduleOrQueue(¤t->close, + engine->module()->scheduleOrQueue(¤t->close, TRIGGER_EVENT_UNDEFINED, getTimeNowNt(), current->extra + engine->engineState.auxValveEnd, diff --git a/firmware/controllers/engine_cycle/main_trigger_callback.cpp b/firmware/controllers/engine_cycle/main_trigger_callback.cpp index 4682d2c5aa..f8e361585c 100644 --- a/firmware/controllers/engine_cycle/main_trigger_callback.cpp +++ b/firmware/controllers/engine_cycle/main_trigger_callback.cpp @@ -422,6 +422,10 @@ void mainTriggerCallback(uint32_t trgEventIndex, efitick_t edgeTimestamp) { * specified duration of time */ handleFuel(limitedFuel, trgEventIndex, rpm, edgeTimestamp); + + engine->module()->scheduleEventsUntilNextTriggerTooth( + rpm, trgEventIndex, edgeTimestamp); + /** * For spark we schedule both start of coil charge and actual spark based on trigger angle */ diff --git a/firmware/controllers/engine_cycle/spark_logic.cpp b/firmware/controllers/engine_cycle/spark_logic.cpp index 7b8b2b80c0..3934dab9a2 100644 --- a/firmware/controllers/engine_cycle/spark_logic.cpp +++ b/firmware/controllers/engine_cycle/spark_logic.cpp @@ -280,68 +280,6 @@ void turnSparkPinHigh(IgnitionEvent *event) { } } -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 - */ -bool scheduleOrQueue(AngleBasedEvent *event, - uint32_t trgEventIndex, - efitick_t edgeTimestamp, - angle_t angle, - action_s action) { - event->position.setAngle(angle); - - /** - * Here's the status as of Jan 2020: - * Once we hit the last trigger tooth prior to needed event, schedule it by time. 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. - */ - if (trgEventIndex != TRIGGER_EVENT_UNDEFINED && event->position.triggerEventIndex == trgEventIndex) { - /** - * Spark should be fired before the next trigger event - time-based delay is best precision possible - */ - scheduling_s * sDown = &event->scheduling; - - scheduleByAngle( - sDown, - edgeTimestamp, - event->position.angleOffsetFromTriggerEvent, - action - ); - - return true; - } else { - event->action = action; - /** - * Spark should be scheduled in relation to some future trigger event, this way we get better firing precision - */ - { - chibios_rt::CriticalSectionLocker csl; - - // TODO: This is O(n), consider some other way of detecting if in a list, - // and consider doubly linked or other list tricks. - - if (!assertNotInIgnitionList(engine->angleBasedEventsHead, event)) { - // Use Append to retain some semblance of event ordering in case of - // time skew. Thus on events are always followed by off events. - LL_APPEND2(engine->angleBasedEventsHead, event, nextToothEvent); - - return false; - } - } -#if SPARK_EXTREME_LOGGING - efiPrintf("isPending thus not adding to queue index=%d rev=%d now=%d", - trgEventIndex, getRevolutionCounter(), (int)getTimeNowUs()); -#endif /* SPARK_EXTREME_LOGGING */ - return false; - } -} - static void scheduleSparkEvent(bool limitedSpark, uint32_t trgEventIndex, IgnitionEvent *event, int rpm, efitick_t edgeTimestamp) { @@ -400,7 +338,9 @@ static void scheduleSparkEvent(bool limitedSpark, uint32_t trgEventIndex, Igniti efiAssertVoid(CUSTOM_ERR_6591, !cisnan(sparkAngle), "findAngle#4"); assertAngleRange(sparkAngle, "findAngle#a5", CUSTOM_ERR_6549); - bool scheduled = scheduleOrQueue(&event->sparkEvent, trgEventIndex, edgeTimestamp, sparkAngle, { fireSparkAndPrepareNextSchedule, event }); + bool scheduled = engine->module()->scheduleOrQueue( + &event->sparkEvent, trgEventIndex, edgeTimestamp, sparkAngle, + { fireSparkAndPrepareNextSchedule, event }); if (scheduled) { #if SPARK_EXTREME_LOGGING @@ -475,60 +415,6 @@ static void prepareIgnitionSchedule() { initializeIgnitionActions(); } -static void scheduleAllSparkEventsUntilNextTriggerTooth(uint32_t trgEventIndex, efitick_t edgeTimestamp) { - AngleBasedEvent *current, *tmp, *keephead; - AngleBasedEvent *keeptail = nullptr; - - { - chibios_rt::CriticalSectionLocker csl; - - keephead = engine->angleBasedEventsHead; - engine->angleBasedEventsHead = nullptr; - } - - LL_FOREACH_SAFE2(keephead, current, tmp, nextToothEvent) - { - if (current->position.triggerEventIndex == trgEventIndex) { - // time to fire a spark which was scheduled previously - - // Yes this looks like O(n^2), but that's only over the entire engine - // cycle. It's really O(mn + nn) where m = # of teeth and n = # events - // fired per cycle. The number of teeth outweigh the number of events, at - // least for 60-2.... So odds are we're only firing an event or two per - // tooth, which means the outer loop is really only O(n). And if we are - // firing many events per teeth, then it's likely the events before this - // one also fired and thus the call to LL_DELETE2 is closer to O(1). - LL_DELETE2(keephead, current, nextToothEvent); - - scheduling_s * sDown = ¤t->scheduling; - -#if SPARK_EXTREME_LOGGING - efiPrintf("time to invoke ind=%d %d %d", trgEventIndex, getRevolutionCounter(), (int)getTimeNowUs()); -#endif /* SPARK_EXTREME_LOGGING */ - - // In case this event was scheduled by overdwell protection, cancel it so we can re-schedule at the correct time - engine->executor.cancel(sDown); - - scheduleByAngle( - sDown, - edgeTimestamp, - current->position.angleOffsetFromTriggerEvent, - current->action - ); - } else { - keeptail = current; // Used for fast list concatenation - } - } - - if (keephead) { - chibios_rt::CriticalSectionLocker csl; - - // Put any new entries onto the end of the keep list - keeptail->nextToothEvent = engine->angleBasedEventsHead; - engine->angleBasedEventsHead = keephead; - } -} - void onTriggerEventSparkLogic(bool limitedSpark, uint32_t trgEventIndex, int rpm, efitick_t edgeTimestamp ) { @@ -548,7 +434,6 @@ void onTriggerEventSparkLogic(bool limitedSpark, uint32_t trgEventIndex, int rpm * Ignition schedule is defined once per revolution * See initializeIgnitionActions() */ - scheduleAllSparkEventsUntilNextTriggerTooth(trgEventIndex, edgeTimestamp); // scheduleSimpleMsg(&logger, "eventId spark ", eventIndex); diff --git a/firmware/controllers/engine_cycle/spark_logic.h b/firmware/controllers/engine_cycle/spark_logic.h index 7424d024ea..25417dde7d 100644 --- a/firmware/controllers/engine_cycle/spark_logic.h +++ b/firmware/controllers/engine_cycle/spark_logic.h @@ -15,10 +15,3 @@ percent_t getCoilDutyCycle(int rpm); void initializeIgnitionActions(); int isIgnitionTimingError(void); - -#define TRIGGER_EVENT_UNDEFINED INT32_MAX -bool scheduleOrQueue(AngleBasedEvent *event, - uint32_t trgEventIndex, - efitick_t edgeTimestamp, - angle_t angle, - action_s action); diff --git a/firmware/controllers/system/system.mk b/firmware/controllers/system/system.mk index 4ed8ac0be2..da1415e26b 100644 --- a/firmware/controllers/system/system.mk +++ b/firmware/controllers/system/system.mk @@ -6,3 +6,4 @@ SYSTEMSRC_CPP = \ $(PROJECT_DIR)/controllers/system/periodic_task.cpp \ $(PROJECT_DIR)/controllers/system/dc_motor.cpp \ $(PROJECT_DIR)/controllers/system/timer/scheduler.cpp \ + $(PROJECT_DIR)/controllers/system/timer/trigger_scheduler.cpp \ diff --git a/firmware/controllers/system/timer/trigger_scheduler.cpp b/firmware/controllers/system/timer/trigger_scheduler.cpp new file mode 100644 index 0000000000..a1efb2d203 --- /dev/null +++ b/firmware/controllers/system/timer/trigger_scheduler.cpp @@ -0,0 +1,149 @@ +#include "pch.h" + +#include "event_queue.h" +#include "os_access.h" + +bool TriggerScheduler::assertNotInList(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 + */ +bool TriggerScheduler::scheduleOrQueue(AngleBasedEvent *event, + uint32_t trgEventIndex, + efitick_t edgeTimestamp, + angle_t angle, + action_s action) { + event->position.setAngle(angle); + + /** + * Here's the status as of Jan 2020: + * Once we hit the last trigger tooth prior to needed event, schedule it by time. 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. + */ + if (trgEventIndex != TRIGGER_EVENT_UNDEFINED && event->position.triggerEventIndex == trgEventIndex) { + /** + * Spark should be fired before the next trigger event - time-based delay is best precision possible + */ + scheduling_s * sDown = &event->scheduling; + + scheduleByAngle( + sDown, + edgeTimestamp, + event->position.angleOffsetFromTriggerEvent, + action + ); + + return true; + } else { + event->action = action; + /** + * Spark should be scheduled in relation to some future trigger event, this way we get better firing precision + */ + { + chibios_rt::CriticalSectionLocker csl; + + // TODO: This is O(n), consider some other way of detecting if in a list, + // and consider doubly linked or other list tricks. + + if (!assertNotInList(m_angleBasedEventsHead, event)) { + // Use Append to retain some semblance of event ordering in case of + // time skew. Thus on events are always followed by off events. + LL_APPEND2(m_angleBasedEventsHead, event, nextToothEvent); + + return false; + } + } +#if SPARK_EXTREME_LOGGING + efiPrintf("isPending thus not adding to queue index=%d rev=%d now=%d", + trgEventIndex, getRevolutionCounter(), (int)getTimeNowUs()); +#endif /* SPARK_EXTREME_LOGGING */ + return false; + } +} + +void TriggerScheduler::scheduleEventsUntilNextTriggerTooth(int rpm, + uint32_t trgEventIndex, + efitick_t edgeTimestamp) { + + // Not sure why we check ignitionEnabled; it's left over from when this code lived in + // spark_logic.cpp, but I think it should be removed. + if (!isValidRpm(rpm) || !engineConfiguration->isIgnitionEnabled) { + // this might happen for instance in case of a single trigger event after a pause + return; + } + + AngleBasedEvent *current, *tmp, *keephead; + AngleBasedEvent *keeptail = nullptr; + + { + chibios_rt::CriticalSectionLocker csl; + + keephead =m_angleBasedEventsHead; + m_angleBasedEventsHead = nullptr; + } + + LL_FOREACH_SAFE2(keephead, current, tmp, nextToothEvent) + { + if (current->position.triggerEventIndex == trgEventIndex) { + // time to fire a spark which was scheduled previously + + // Yes this looks like O(n^2), but that's only over the entire engine + // cycle. It's really O(mn + nn) where m = # of teeth and n = # events + // fired per cycle. The number of teeth outweigh the number of events, at + // least for 60-2.... So odds are we're only firing an event or two per + // tooth, which means the outer loop is really only O(n). And if we are + // firing many events per teeth, then it's likely the events before this + // one also fired and thus the call to LL_DELETE2 is closer to O(1). + LL_DELETE2(keephead, current, nextToothEvent); + + scheduling_s * sDown = ¤t->scheduling; + +#if SPARK_EXTREME_LOGGING + efiPrintf("time to invoke ind=%d %d %d", + trgEventIndex, getRevolutionCounter(), (int)getTimeNowUs()); +#endif /* SPARK_EXTREME_LOGGING */ + + // In case this event was scheduled by overdwell protection, cancel it so + // we can re-schedule at the correct time + engine->executor.cancel(sDown); + + scheduleByAngle( + sDown, + edgeTimestamp, + current->position.angleOffsetFromTriggerEvent, + current->action + ); + } else { + keeptail = current; // Used for fast list concatenation + } + } + + if (keephead) { + chibios_rt::CriticalSectionLocker csl; + + // Put any new entries onto the end of the keep list + keeptail->nextToothEvent = m_angleBasedEventsHead; + m_angleBasedEventsHead = keephead; + } +} + +#if EFI_UNIT_TEST +// todo: reduce code duplication with another 'getElementAtIndexForUnitText' +AngleBasedEvent * TriggerScheduler::getElementAtIndexForUnitTest(int index) { + AngleBasedEvent * current; + + LL_FOREACH2(m_angleBasedEventsHead, current, nextToothEvent) + { + if (index == 0) + return current; + index--; + } + firmwareError(OBD_PCM_Processor_Fault, "getElementAtIndexForUnitText: null"); + return nullptr; +} +#endif /* EFI_UNIT_TEST */ diff --git a/firmware/controllers/system/timer/trigger_scheduler.h b/firmware/controllers/system/timer/trigger_scheduler.h new file mode 100644 index 0000000000..d57030f142 --- /dev/null +++ b/firmware/controllers/system/timer/trigger_scheduler.h @@ -0,0 +1,30 @@ +#pragma once + +#define TRIGGER_EVENT_UNDEFINED INT32_MAX + +class TriggerScheduler : public EngineModule { +public: + bool scheduleOrQueue(AngleBasedEvent *event, + uint32_t trgEventIndex, + efitick_t edgeTimestamp, + angle_t angle, + action_s action); + + void scheduleEventsUntilNextTriggerTooth(int rpm, + uint32_t trgEventIndex, + efitick_t edgeTimestamp); + + // For unit tests + AngleBasedEvent * getElementAtIndexForUnitTest(int index); + +private: + bool assertNotInList(AngleBasedEvent *head, AngleBasedEvent *element); + + /** + * That's the linked list of pending events scheduled in relation to trigger + * At the moment we iterate over the whole list while looking for events for specific + * trigger index We can make it an array of lists per trigger index, but that would take + * some RAM and probably not needed yet. + */ + AngleBasedEvent *m_angleBasedEventsHead = nullptr; +}; diff --git a/unit_tests/engine_test_helper.cpp b/unit_tests/engine_test_helper.cpp index 6aaa6dd5dd..1f0fe79559 100644 --- a/unit_tests/engine_test_helper.cpp +++ b/unit_tests/engine_test_helper.cpp @@ -305,27 +305,12 @@ scheduling_s * EngineTestHelper::assertEvent5(const char *msg, int index, void * return event; } -// todo: reduce code duplication with another 'getElementAtIndexForUnitText' -static AngleBasedEvent * getElementAtIndexForUnitText(int index, Engine *engine) { - AngleBasedEvent * current; - - LL_FOREACH2(engine->angleBasedEventsHead, current, nextToothEvent) - { - if (index == 0) - return current; - index--; - } -#if EFI_UNIT_TEST - firmwareError(OBD_PCM_Processor_Fault, "getElementAtIndexForUnitText: null"); -#endif /* EFI_UNIT_TEST */ - return nullptr; -} - AngleBasedEvent * EngineTestHelper::assertTriggerEvent(const char *msg, int index, AngleBasedEvent *expected, void *callback, int triggerEventIndex, angle_t angleOffsetFromTriggerEvent) { - AngleBasedEvent * event = getElementAtIndexForUnitText(index, &engine); + AngleBasedEvent * event = + engine.module()->getElementAtIndexForUnitTest(index); assertEqualsM4(msg, " callback up/down", (void*)event->action.getCallback() == (void*) callback, 1);