two modes trigger scheduler (#4598)

* two modes trigger scheduler

* asOld

* hpfp

firmware/controllers/algo/event_registry.h

@@ -13,15 +13,36 @@
 #include "fl_stack.h"
 #include "trigger_structure.h"
 
-class AngleBasedEvent {
+struct AngleBasedEventOld;
-public:
+
+struct AngleBasedEventBase {
 	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;
+	AngleBasedEventBase *nextToothEvent = nullptr;
+
+	virtual bool shouldSchedule(uint32_t trgEventIndex, float currentPhase, float nextPhase) const = 0;
+	virtual float getAngleFromNow(float currentPhase) const = 0;
+
+	virtual AngleBasedEventOld* asOld() { return nullptr; }
+};
+
+struct AngleBasedEventOld : public AngleBasedEventBase {
+	event_trigger_position_s position;
+
+	bool shouldSchedule(uint32_t trgEventIndex, float currentPhase, float nextPhase) const override;
+	float getAngleFromNow(float currentPhase) const override;
+
+	AngleBasedEventOld* asOld() override { return this; }
+};
+
+struct AngleBasedEventNew : public AngleBasedEventBase {
+	float enginePhase;
+
+	bool shouldSchedule(uint32_t trgEventIndex, float currentPhase, float nextPhase) const override;
+	float getAngleFromNow(float currentPhase) const override;
 };
 
 #define MAX_OUTPUTS_FOR_IGNITION 2
@@ -31,7 +52,7 @@ public:
 	IgnitionEvent();
 	IgnitionOutputPin *outputs[MAX_OUTPUTS_FOR_IGNITION];
 	scheduling_s dwellStartTimer;
-	AngleBasedEvent sparkEvent;
+	AngleBasedEventOld sparkEvent;
 
 	scheduling_s trailingSparkCharge;
 	scheduling_s trailingSparkFire;
@@ -79,8 +100,8 @@ public:
 	int valveIndex;
 	angle_t extra;
 
-	AngleBasedEvent open;
+	AngleBasedEventOld open;
-	AngleBasedEvent close;
+	AngleBasedEventOld close;
 };
 
 

firmware/controllers/engine_cycle/high_pressure_fuel_pump.h

@@ -84,7 +84,7 @@ public:
 #if !EFI_UNIT_TEST
 private:
 #endif // EFI_UNIT_TEST
-	AngleBasedEvent m_event;
+	AngleBasedEventOld m_event;
 
 	HpfpQuantity m_quantity;
 	HpfpLobe     m_lobe;

firmware/controllers/engine_cycle/main_trigger_callback.cpp

@@ -304,7 +304,7 @@ void mainTriggerCallback(uint32_t trgEventIndex, efitick_t edgeTimestamp, angle_
 	handleFuel(trgEventIndex, rpm, edgeTimestamp, currentPhase, nextPhase);
 
 	engine->module<TriggerScheduler>()->scheduleEventsUntilNextTriggerTooth(
-		rpm, trgEventIndex, edgeTimestamp);
+		rpm, trgEventIndex, edgeTimestamp, currentPhase, nextPhase);
 
 	/**
 	 * For spark we schedule both start of coil charge and actual spark based on trigger angle

firmware/controllers/system/timer/trigger_scheduler.cpp

@@ -2,9 +2,8 @@
 
 #include "event_queue.h"
 
-
+bool TriggerScheduler::assertNotInList(AngleBasedEventBase *head, AngleBasedEventBase *element) {
-bool TriggerScheduler::assertNotInList(AngleBasedEvent *head, AngleBasedEvent *element) {
+       assertNotInListMethodBody(AngleBasedEventBase, head, element, nextToothEvent)
-       assertNotInListMethodBody(AngleBasedEvent, head, element, nextToothEvent)
 }
 
 /**
@@ -17,7 +16,7 @@ bool TriggerScheduler::assertNotInList(AngleBasedEvent *head, AngleBasedEvent *e
  * @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,
+bool TriggerScheduler::scheduleOrQueue(AngleBasedEventOld *event,
 		uint32_t trgEventIndex,
 		efitick_t edgeTimestamp,
 		angle_t angle,
@@ -73,17 +72,30 @@ bool TriggerScheduler::scheduleOrQueue(AngleBasedEvent *event,
 	}
 }
 
+bool TriggerScheduler::scheduleOrQueue(AngleBasedEventNew *event,
+		uint32_t trgEventIndex,
+		efitick_t edgeTimestamp,
+		angle_t angle,
+		action_s action) {
+	event->enginePhase = angle;
+	event->action = action;
+
+	// TODO: implement me!
+
+	return false;
+}
+
 void TriggerScheduler::scheduleEventsUntilNextTriggerTooth(int rpm,
 							   uint32_t trgEventIndex,
-							   efitick_t edgeTimestamp) {
+							   efitick_t edgeTimestamp, float currentPhase, float nextPhase) {
 
 	if (!isValidRpm(rpm)) {
 		 // this might happen for instance in case of a single trigger event after a pause
 		return;
 	}
 
-	AngleBasedEvent *current, *tmp, *keephead;
+	AngleBasedEventBase *current, *tmp, *keephead;
-	AngleBasedEvent *keeptail = nullptr;
+	AngleBasedEventBase *keeptail = nullptr;
 
 	{
 		chibios_rt::CriticalSectionLocker csl;
@@ -94,7 +106,7 @@ void TriggerScheduler::scheduleEventsUntilNextTriggerTooth(int rpm,
 
 	LL_FOREACH_SAFE2(keephead, current, tmp, nextToothEvent)
 	{
-		if (current->position.triggerEventIndex == trgEventIndex) {
+		if (current->shouldSchedule(trgEventIndex, currentPhase, nextPhase)) {
 			// time to fire a spark which was scheduled previously
 
 			// Yes this looks like O(n^2), but that's only over the entire engine
@@ -120,7 +132,7 @@ void TriggerScheduler::scheduleEventsUntilNextTriggerTooth(int rpm,
 			scheduleByAngle(
 				sDown,
 				edgeTimestamp,
-				current->position.angleOffsetFromTriggerEvent,
+				current->getAngleFromNow(currentPhase),
 				current->action
 			);
 		} else {
@@ -137,10 +149,28 @@ void TriggerScheduler::scheduleEventsUntilNextTriggerTooth(int rpm,
 	}
 }
 
+bool AngleBasedEventOld::shouldSchedule(uint32_t trgEventIndex, float /*currentPhase*/, float /*nextPhase*/) const {
+	return position.triggerEventIndex == trgEventIndex;
+}
+
+float AngleBasedEventOld::getAngleFromNow(float /*currentPhase*/) const {
+	return position.angleOffsetFromTriggerEvent;
+}
+
+bool AngleBasedEventNew::shouldSchedule(uint32_t trgEventIndex, float currentPhase, float nextPhase) const {
+	// TODO: implement me!
+	return true;
+}
+
+float AngleBasedEventNew::getAngleFromNow(float currentPhase) const {
+	// TODO: implement me!
+	return 0;
+}
+
 #if EFI_UNIT_TEST
 // todo: reduce code duplication with another 'getElementAtIndexForUnitText'
-AngleBasedEvent * TriggerScheduler::getElementAtIndexForUnitTest(int index) {
+AngleBasedEventBase * TriggerScheduler::getElementAtIndexForUnitTest(int index) {
-	AngleBasedEvent * current;
+	AngleBasedEventBase * current;
 
 	LL_FOREACH2(m_angleBasedEventsHead, current, nextToothEvent)
 	{

firmware/controllers/system/timer/trigger_scheduler.h

@@ -4,7 +4,13 @@
 
 class TriggerScheduler : public EngineModule {
 public:
-	bool scheduleOrQueue(AngleBasedEvent *event,
+	bool scheduleOrQueue(AngleBasedEventOld *event,
+			     uint32_t trgEventIndex,
+			     efitick_t edgeTimestamp,
+			     angle_t angle,
+			     action_s action);
+
+	bool scheduleOrQueue(AngleBasedEventNew *event,
 			     uint32_t trgEventIndex,
 			     efitick_t edgeTimestamp,
 			     angle_t angle,
@@ -12,13 +18,14 @@ public:
 
 	void scheduleEventsUntilNextTriggerTooth(int rpm,
 						 uint32_t trgEventIndex,
-						 efitick_t edgeTimestamp);
+						 efitick_t edgeTimestamp,
+						 float currentPhase, float nextPhase);
 
 	// For unit tests
-	AngleBasedEvent * getElementAtIndexForUnitTest(int index);
+	AngleBasedEventBase * getElementAtIndexForUnitTest(int index);
 
 private:
-	bool assertNotInList(AngleBasedEvent *head, AngleBasedEvent *element);
+	bool assertNotInList(AngleBasedEventBase *head, AngleBasedEventBase *element);
 
 	/**
 	 * That's the linked list of pending events scheduled in relation to trigger
@@ -26,5 +33,5 @@ private:
 	 * 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;
+	AngleBasedEventBase *m_angleBasedEventsHead = nullptr;
 };

unit_tests/engine_test_helper.cpp

@@ -308,13 +308,15 @@ scheduling_s * EngineTestHelper::assertEvent5(const char *msg, int index, void *
 	return event;
 }
 
-AngleBasedEvent * EngineTestHelper::assertTriggerEvent(const char *msg,
+AngleBasedEventBase * EngineTestHelper::assertTriggerEvent(const char *msg,
-		int index, AngleBasedEvent *expected,
+		int index, AngleBasedEventBase *expected,
 		void *callback,
 		int triggerEventIndex, angle_t angleOffsetFromTriggerEvent) {
-	AngleBasedEvent * event =
+	AngleBasedEventBase * event2 =
 		engine.module<TriggerScheduler>()->getElementAtIndexForUnitTest(index);
 
+	auto event = event2->asOld();
+
 	assertEqualsM4(msg, " callback up/down", (void*)event->action.getCallback() == (void*) callback, 1);
 
 	assertEqualsM4(msg, " trigger", triggerEventIndex, event->position.triggerEventIndex);

unit_tests/engine_test_helper.h

@@ -84,7 +84,7 @@ public:
 	scheduling_s * assertEvent5(const char *msg, int index, void *callback, efitimeus_t expectedTimestamp);
 	scheduling_s * assertScheduling(const char *msg, int index, scheduling_s *expected, void *callback, efitimeus_t expectedTimestamp);
 
-	AngleBasedEvent * assertTriggerEvent(const char *msg, int index, AngleBasedEvent *expected, void *callback, int triggerEventIndex, angle_t angleOffsetFromTriggerEvent);
+	AngleBasedEventBase * assertTriggerEvent(const char *msg, int index, AngleBasedEventBase *expected, void *callback, int triggerEventIndex, angle_t angleOffsetFromTriggerEvent);
 
 	void assertEvent(const char *msg, int index, void *callback, efitimeus_t momentX, InjectionEvent *event);
 	void assertInjectorUpEvent(const char *msg, int eventIndex, efitimeus_t momentX, long injectorIndex);

unit_tests/tests/test_hpfp.cpp

@@ -277,7 +277,7 @@ TEST(HPFP, Schedule) {
 
 	// Make the previous event happen, schedule the next.
 	engine->module<TriggerScheduler>()->scheduleEventsUntilNextTriggerTooth(
-		1000, 1, tick_per_deg * 0);
+		1000, 1, tick_per_deg * 0, 0, 0);
 	// Mock executor doesn't run events, so we run it manually
 	HpfpController::pinTurnOff(&hpfp);
 
@@ -287,7 +287,7 @@ TEST(HPFP, Schedule) {
 
 	// Make it happen
 	engine->module<TriggerScheduler>()->scheduleEventsUntilNextTriggerTooth(
-		1000, 2, tick_per_deg * 180);
+		1000, 2, tick_per_deg * 180, 0, 0);
 
 	// Since we have a mock scheduler, lets insert the correct timestamp in the scheduling
 	// struct.