diff --git a/firmware/controllers/system/event_queue.cpp b/firmware/controllers/system/event_queue.cpp index cfe9aa9e94..c6a04a6dc5 100644 --- a/firmware/controllers/system/event_queue.cpp +++ b/firmware/controllers/system/event_queue.cpp @@ -26,6 +26,9 @@ bool EventQueue::checkIfPending(scheduling_s *scheduling) { } void EventQueue::insertTask(scheduling_s *scheduling, uint64_t timeX, schfunc_t callback, void *param) { +#if EFI_UNIT_TEST + assertListIsSorted(); +#endif if (callback == NULL) firmwareError("NULL callback"); @@ -37,7 +40,20 @@ void EventQueue::insertTask(scheduling_s *scheduling, uint64_t timeX, schfunc_t scheduling->callback = callback; scheduling->param = param; - LL_PREPEND(head, scheduling); + if (head == NULL || timeX < head->momentX) { + LL_PREPEND(head, scheduling); + } else { + scheduling_s *insertPosition = head; + while (insertPosition->next != NULL && insertPosition->next->momentX < timeX) { + insertPosition = insertPosition->next; + } + + scheduling->next = insertPosition->next; + insertPosition->next = scheduling; + } +#if EFI_UNIT_TEST + assertListIsSorted(); +#endif } //void EventQueue::insertTask(scheduling_s *scheduling, int delayUs, schfunc_t callback, void *param) { @@ -104,6 +120,9 @@ int EventQueue::executeAll(uint64_t now) { LL_PREPEND(executionList, current); } } +#if EFI_UNIT_TEST + assertListIsSorted(); +#endif /* * we need safe iteration here because 'callback' might change change 'current->next' @@ -132,6 +151,16 @@ int EventQueue::size(void) { return result; } +#if EFI_UNIT_TEST +void EventQueue::assertListIsSorted() { + scheduling_s *current = head; + while (current != NULL && current->next != NULL) { + efiAssertVoid(current->momentX <= current->next->momentX, "list order"); + current = current->next; + } +} +#endif + void EventQueue::setLateDelay(int value) { lateDelay = value; } diff --git a/firmware/controllers/system/event_queue.h b/firmware/controllers/system/event_queue.h index 7ddb65c79f..0b88c96451 100644 --- a/firmware/controllers/system/event_queue.h +++ b/firmware/controllers/system/event_queue.h @@ -53,6 +53,7 @@ public: scheduling_s *getForUnitText(int index); void setLateDelay(int value); scheduling_s * getHead(); + void assertListIsSorted(); private: bool checkIfPending(scheduling_s *scheduling); scheduling_s *head; diff --git a/unit_tests/test_signal_executor.cpp b/unit_tests/test_signal_executor.cpp index 9837e3b99c..08c684243b 100644 --- a/unit_tests/test_signal_executor.cpp +++ b/unit_tests/test_signal_executor.cpp @@ -96,14 +96,14 @@ void testSignalExecutor(void) { eq.insertTask(&s1, 10, callback, NULL); eq.insertTask(&s4, 10, callback, NULL); - eq.insertTask(&s2, 11, callback, NULL); eq.insertTask(&s3, 12, callback, NULL); + eq.insertTask(&s2, 11, callback, NULL); assertEquals(4, eq.size()); - assertEquals(12, eq.getHead()->momentX); - assertEquals(11, eq.getHead()->next->momentX); - assertEquals(10, eq.getHead()->next->next->momentX); - assertEquals(10, eq.getHead()->next->next->next->momentX); + assertEquals(10, eq.getHead()->momentX); + assertEquals(10, eq.getHead()->next->momentX); + assertEquals(11, eq.getHead()->next->next->momentX); + assertEquals(12, eq.getHead()->next->next->next->momentX); callbackCounter = 0; eq.executeAll(10); diff --git a/unit_tests/test_trigger_decoder.cpp b/unit_tests/test_trigger_decoder.cpp index 5a6fb9cbe8..bc1cecfe99 100644 --- a/unit_tests/test_trigger_decoder.cpp +++ b/unit_tests/test_trigger_decoder.cpp @@ -417,11 +417,11 @@ static void testRpmCalculator(void) { assertEqualsM("index #2", 0, eth.triggerCentral.triggerState.getCurrentIndex()); assertEqualsM("queue size", 6, schedulingQueue.size()); scheduling_s *ev1 = schedulingQueue.getForUnitText(0); - assertREquals((void*)ev1->callback, (void*)turnPinLow); - assertREquals((void*)&enginePins.coils[0], ev1->param); + assertREquals((void*)ev1->callback, (void*)turnPinHigh); + assertREquals((void*)&enginePins.coils[3], ev1->param); - assertEqualsM("ev 1", 247059, ev1->momentX); - assertEqualsM("ev 2", 246559, schedulingQueue.getForUnitText(1)->momentX); + assertEqualsM("ev 1", 245000, ev1->momentX); + assertEqualsM("ev 2", 245000, schedulingQueue.getForUnitText(1)->momentX); schedulingQueue.clear(); timeNow += 5000; @@ -432,10 +432,10 @@ static void testRpmCalculator(void) { eth.triggerCentral.handleShaftSignal(SHAFT_PRIMARY_DOWN PASS_ENGINE_PARAMETER); assertEqualsM("index #3", 3, eth.triggerCentral.triggerState.getCurrentIndex()); assertEqualsM("queue size 3", 6, schedulingQueue.size()); - assertEqualsM("ev 3", 260392, schedulingQueue.getForUnitText(0)->momentX); - assertEquals(259892, schedulingQueue.getForUnitText(1)->momentX); - assertEqualsM2("ev 5", 261362, schedulingQueue.getForUnitText(2)->momentX, 2); - assertEqualsM("3/3", 258333, schedulingQueue.getForUnitText(3)->momentX); + assertEqualsM("ev 3", 258333, schedulingQueue.getForUnitText(0)->momentX); + assertEqualsM("ev 4", 258333, schedulingQueue.getForUnitText(1)->momentX); + assertEqualsM2("ev 5", 259892, schedulingQueue.getForUnitText(2)->momentX, 2); + assertEqualsM("3/3", 260392, schedulingQueue.getForUnitText(3)->momentX); schedulingQueue.clear(); timeNow += 5000; @@ -446,7 +446,7 @@ static void testRpmCalculator(void) { eth.triggerCentral.handleShaftSignal(SHAFT_PRIMARY_UP PASS_ENGINE_PARAMETER); assertEqualsM("index #4", 6, eth.triggerCentral.triggerState.getCurrentIndex()); assertEqualsM("queue size 4", 6, schedulingQueue.size()); - assertEqualsM("4/0", 273725, schedulingQueue.getForUnitText(0)->momentX); + assertEqualsM("4/0", 271666, schedulingQueue.getForUnitText(0)->momentX); schedulingQueue.clear(); timeNow += 5000; @@ -458,9 +458,9 @@ static void testRpmCalculator(void) { timeNow += 5000; // 5ms eth.triggerCentral.handleShaftSignal(SHAFT_PRIMARY_UP PASS_ENGINE_PARAMETER); assertEqualsM("queue size 6", 6, schedulingQueue.size()); - assertEqualsM("6/0", 287059, schedulingQueue.getForUnitText(0)->momentX); - assertEqualsM("6/1", 286559, schedulingQueue.getForUnitText(1)->momentX); - assertEqualsM2("6/2", 288029, schedulingQueue.getForUnitText(2)->momentX, 1); + assertEqualsM("6/0", 285000, schedulingQueue.getForUnitText(0)->momentX); + assertEqualsM("6/1", 285000, schedulingQueue.getForUnitText(1)->momentX); + assertEqualsM2("6/2", 286559, schedulingQueue.getForUnitText(2)->momentX, 1); schedulingQueue.clear(); timeNow += 5000; @@ -471,10 +471,10 @@ static void testRpmCalculator(void) { timeNow += 5000; // 5ms eth.triggerCentral.handleShaftSignal(SHAFT_PRIMARY_UP PASS_ENGINE_PARAMETER); assertEqualsM("queue size 8", 5, schedulingQueue.size()); - assertEqualsM("8/0", 299892, schedulingQueue.getForUnitText(0)->momentX); - assertEqualsM("8/1", 301363, schedulingQueue.getForUnitText(1)->momentX); - assertEqualsM2("8/2", 298333, schedulingQueue.getForUnitText(2)->momentX, 1); - assertEqualsM("8/3", 301363, schedulingQueue.getForUnitText(3)->momentX); + assertEqualsM("8/0", 298333, schedulingQueue.getForUnitText(0)->momentX); + assertEqualsM("8/1", 298333, schedulingQueue.getForUnitText(1)->momentX); + assertEqualsM2("8/2", 299892, schedulingQueue.getForUnitText(2)->momentX, 0); + assertEqualsM2("8/3", 301363, schedulingQueue.getForUnitText(3)->momentX, 0); schedulingQueue.clear(); timeNow += 5000;