Separate out scheduleOrQueue into it's own standalone scheduler. (#3573)

It's a large enough entity that it shouldn't be buried in ignition logic.

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
 	 */

firmware/controllers/engine_cycle/aux_valves.cpp

@@ -25,7 +25,7 @@ static void plainPinTurnOff(NamedOutputPin *output) {
 
 
 static void scheduleOpen(AuxActor *current) {
-	scheduleOrQueue(&current->open,
+	engine->module<TriggerScheduler>()->scheduleOrQueue(&current->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(&current->close,
+	engine->module<TriggerScheduler>()->scheduleOrQueue(&current->close,
 			TRIGGER_EVENT_UNDEFINED,
 			getTimeNowNt(),
 			current->extra + engine->engineState.auxValveEnd,

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<TriggerScheduler>()->scheduleEventsUntilNextTriggerTooth(
+		rpm, trgEventIndex, edgeTimestamp);
+
 	/**
 	 * For spark we schedule both start of coil charge and actual spark based on trigger angle
 	 */

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<TriggerScheduler>()->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 = &current->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);

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

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 \

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 = &current->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 */

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;
+};

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<TriggerScheduler>()->getElementAtIndexForUnitTest(index);
 
 	assertEqualsM4(msg, " callback up/down", (void*)event->action.getCallback() == (void*) callback, 1);