diff --git a/firmware/controllers/algo/event_registry.h b/firmware/controllers/algo/event_registry.h index 094aeafcf8..8c0ec23e49 100644 --- a/firmware/controllers/algo/event_registry.h +++ b/firmware/controllers/algo/event_registry.h @@ -52,7 +52,7 @@ public: IgnitionEvent(); IgnitionOutputPin *outputs[MAX_OUTPUTS_FOR_IGNITION]; scheduling_s dwellStartTimer; - AngleBasedEventOld sparkEvent; + AngleBasedEventNew sparkEvent; scheduling_s trailingSparkCharge; scheduling_s trailingSparkFire; @@ -70,7 +70,9 @@ public: * this timestamp allows us to measure actual dwell time */ uint32_t actualStartOfDwellNt = 0; - event_trigger_position_s dwellPosition{}; + + float dwellAngle = 0; + /** * Sequential number of currently processed spark event * @see engineState.sparkCounter diff --git a/firmware/controllers/engine_cycle/spark_logic.cpp b/firmware/controllers/engine_cycle/spark_logic.cpp index cd032e8162..2bcf3e53f1 100644 --- a/firmware/controllers/engine_cycle/spark_logic.cpp +++ b/firmware/controllers/engine_cycle/spark_logic.cpp @@ -100,7 +100,7 @@ static void prepareCylinderIgnitionSchedule(angle_t dwellAngleDuration, floatms_ // let's save planned duration so that we can later compare it with reality event->sparkDwell = sparkDwell; - const angle_t sparkAngle = + angle_t sparkAngle = // Negate because timing *before* TDC, and we schedule *after* TDC - getEngineState()->timingAdvance[event->cylinderNumber] // Offset by this cylinder's position in the cycle @@ -135,6 +135,8 @@ static void prepareCylinderIgnitionSchedule(angle_t dwellAngleDuration, floatms_ event->outputs[0] = output; event->outputs[1] = secondOutput; + + wrapAngle2(sparkAngle, "findAngle#2", CUSTOM_ERR_6550, getEngineCycle(getEngineRotationState()->getOperationMode())); event->sparkAngle = sparkAngle; // Stash which cylinder we're scheduling so that knock sensing knows which // cylinder just fired @@ -142,12 +144,14 @@ static void prepareCylinderIgnitionSchedule(angle_t dwellAngleDuration, floatms_ angle_t dwellStartAngle = sparkAngle - dwellAngleDuration; efiAssertVoid(CUSTOM_ERR_6590, !cisnan(dwellStartAngle), "findAngle#5"); + assertAngleRange(dwellStartAngle, "findAngle dwellStartAngle", CUSTOM_ERR_6550); - event->dwellPosition.setAngle(dwellStartAngle); + wrapAngle2(dwellStartAngle, "findAngle#7", CUSTOM_ERR_6550, getEngineCycle(getEngineRotationState()->getOperationMode())); + event->dwellAngle = dwellStartAngle; #if FUEL_MATH_EXTREME_LOGGING if (printFuelDebug) { - printf("addIgnitionEvent %s ind=%d\n", output->name, event->dwellPosition.triggerEventIndex); + printf("addIgnitionEvent %s angle=%.1f\n", output->name, dwellStartAngle); } // efiPrintf("addIgnitionEvent %s ind=%d", output->name, event->dwellPosition->eventIndex); #endif /* FUEL_MATH_EXTREME_LOGGING */ @@ -316,7 +320,7 @@ void turnSparkPinHigh(IgnitionEvent *event) { } static void scheduleSparkEvent(bool limitedSpark, uint32_t trgEventIndex, IgnitionEvent *event, - int rpm, efitick_t edgeTimestamp, float currentPhase) { + int rpm, efitick_t edgeTimestamp, float currentPhase, float nextPhase) { angle_t sparkAngle = event->sparkAngle; const floatms_t dwellMs = engine->engineState.sparkDwell; @@ -329,14 +333,9 @@ static void scheduleSparkEvent(bool limitedSpark, uint32_t trgEventIndex, Igniti return; } - float angleOffset = event->dwellPosition.angleOffsetFromTriggerEvent; - int isIgnitionError = angleOffset < 0; - ignitionErrorDetection.add(isIgnitionError); - if (isIgnitionError) { -#if EFI_PROD_CODE - efiPrintf("Negative spark delay=%.1f deg", angleOffset); -#endif /* EFI_PROD_CODE */ - return; + float angleOffset = event->dwellAngle - currentPhase; + if (angleOffset < 0) { + angleOffset += engine->engineState.engineCycle; } /** @@ -351,7 +350,7 @@ static void scheduleSparkEvent(bool limitedSpark, uint32_t trgEventIndex, Igniti */ if (!limitedSpark) { #if SPARK_EXTREME_LOGGING - efiPrintf("scheduling sparkUp ind=%d %d %s now=%d %d later id=%d", trgEventIndex, getRevolutionCounter(), event->getOutputForLoggins()->name, (int)getTimeNowUs(), (int)angleOffset, + efiPrintf("scheduling sparkUp %d %s now=%d %d later id=%d", getRevolutionCounter(), event->getOutputForLoggins()->name, (int)getTimeNowUs(), (int)angleOffset, event->sparkId); #endif /* SPARK_EXTREME_LOGGING */ @@ -378,7 +377,8 @@ static void scheduleSparkEvent(bool limitedSpark, uint32_t trgEventIndex, Igniti bool scheduled = engine->module()->scheduleOrQueue( &event->sparkEvent, trgEventIndex, edgeTimestamp, sparkAngle, - { fireSparkAndPrepareNextSchedule, event }); + { fireSparkAndPrepareNextSchedule, event }, + currentPhase, nextPhase); if (scheduled) { #if SPARK_EXTREME_LOGGING @@ -386,7 +386,7 @@ static void scheduleSparkEvent(bool limitedSpark, uint32_t trgEventIndex, Igniti #endif /* FUEL_MATH_EXTREME_LOGGING */ } else { #if SPARK_EXTREME_LOGGING - efiPrintf("to queue sparkDown ind=%d %d %s now=%d for id=%d", trgEventIndex, getRevolutionCounter(), event->getOutputForLoggins()->name, (int)getTimeNowUs(), event->sparkEvent.position.triggerEventIndex); + efiPrintf("to queue sparkDown ind=%d %d %s now=%d for id=%d angle=%.1f", trgEventIndex, getRevolutionCounter(), event->getOutputForLoggins()->name, (int)getTimeNowUs(), event->sparkId, sparkAngle); #endif /* SPARK_EXTREME_LOGGING */ if (!limitedSpark && engine->enableOverdwellProtection) { @@ -398,8 +398,8 @@ static void scheduleSparkEvent(bool limitedSpark, uint32_t trgEventIndex, Igniti #if EFI_UNIT_TEST if (verboseMode) { - printf("spark dwell@ %d/%d spark@ %d/%d id=%d\r\n", event->dwellPosition.triggerEventIndex, (int)event->dwellPosition.angleOffsetFromTriggerEvent, - event->sparkEvent.position.triggerEventIndex, (int)event->sparkEvent.position.angleOffsetFromTriggerEvent, + printf("spark dwell@ %.1f spark@ %.2f id=%d\r\n", event->dwellAngle, + event->sparkEvent.enginePhase, event->sparkId); } #endif @@ -482,8 +482,10 @@ void onTriggerEventSparkLogic(uint32_t trgEventIndex, int rpm, efitick_t edgeTim if (engine->ignitionEvents.isReady) { for (size_t i = 0; i < engineConfiguration->specs.cylindersCount; i++) { IgnitionEvent *event = &engine->ignitionEvents.elements[i]; - if (event->dwellPosition.triggerEventIndex != trgEventIndex) + + if (!isPhaseInRange(event->dwellAngle, currentPhase, nextPhase)) { continue; + } if (i == 0 && engineConfiguration->artificialTestMisfire && (getRevolutionCounter() % ((int)engineConfiguration->scriptSetting[5]) == 0)) { // artificial misfire on cylinder #1 for testing purposes @@ -498,7 +500,7 @@ void onTriggerEventSparkLogic(uint32_t trgEventIndex, int rpm, efitick_t edgeTim } #endif // EFI_LAUNCH_CONTROL - scheduleSparkEvent(limitedSpark, trgEventIndex, event, rpm, edgeTimestamp, currentPhase); + scheduleSparkEvent(limitedSpark, trgEventIndex, event, rpm, edgeTimestamp, currentPhase, nextPhase); } } } diff --git a/firmware/controllers/system/timer/trigger_scheduler.cpp b/firmware/controllers/system/timer/trigger_scheduler.cpp index 184043a761..14c1f7bb59 100644 --- a/firmware/controllers/system/timer/trigger_scheduler.cpp +++ b/firmware/controllers/system/timer/trigger_scheduler.cpp @@ -76,11 +76,39 @@ bool TriggerScheduler::scheduleOrQueue(AngleBasedEventNew *event, uint32_t trgEventIndex, efitick_t edgeTimestamp, angle_t angle, - action_s action) { + action_s action, + float currentPhase, float nextPhase) { event->enginePhase = angle; - event->action = action; - // TODO: implement me! + if (event->shouldSchedule(trgEventIndex, currentPhase, nextPhase)) { + // if we're due now, just schedule the event + scheduleByAngle( + &event->scheduling, + edgeTimestamp, + event->getAngleFromNow(currentPhase), + action + ); + + return true; + } else { + // Not due at this tooth, add to the list to execute later + event->action = action; + + { + 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; + } + } + } return false; } @@ -158,13 +186,16 @@ float AngleBasedEventOld::getAngleFromNow(float /*currentPhase*/) const { } bool AngleBasedEventNew::shouldSchedule(uint32_t trgEventIndex, float currentPhase, float nextPhase) const { - // TODO: implement me! - return true; + return isPhaseInRange(this->enginePhase, currentPhase, nextPhase); } float AngleBasedEventNew::getAngleFromNow(float currentPhase) const { - // TODO: implement me! - return 0; + float angleOffset = this->enginePhase - currentPhase; + if (angleOffset < 0) { + angleOffset += engine->engineState.engineCycle; + } + + return angleOffset; } #if EFI_UNIT_TEST diff --git a/firmware/controllers/system/timer/trigger_scheduler.h b/firmware/controllers/system/timer/trigger_scheduler.h index 6371dcaac7..74e6fa544b 100644 --- a/firmware/controllers/system/timer/trigger_scheduler.h +++ b/firmware/controllers/system/timer/trigger_scheduler.h @@ -14,7 +14,8 @@ public: uint32_t trgEventIndex, efitick_t edgeTimestamp, angle_t angle, - action_s action); + action_s action, + float currentPhase, float nextPhase); void scheduleEventsUntilNextTriggerTooth(int rpm, uint32_t trgEventIndex, diff --git a/unit_tests/tests/trigger/test_trigger_decoder.cpp b/unit_tests/tests/trigger/test_trigger_decoder.cpp index dbfe871be8..7cc51901f9 100644 --- a/unit_tests/tests/trigger/test_trigger_decoder.cpp +++ b/unit_tests/tests/trigger/test_trigger_decoder.cpp @@ -183,13 +183,10 @@ TEST(trigger, test1995FordInline6TriggerDecoder) { eth.fireTriggerEvents(48); IgnitionEventList *ecl = &engine->ignitionEvents; - ASSERT_EQ( 1, ecl->isReady) << "ford inline ignition events size"; - ASSERT_EQ( 0, ecl->elements[0].dwellPosition.triggerEventIndex) << "event index"; - ASSERT_NEAR(7.9579, ecl->elements[0].dwellPosition.angleOffsetFromTriggerEvent, EPS2D) << "angle offset#1"; - - ASSERT_EQ( 10, ecl->elements[5].dwellPosition.triggerEventIndex) << "event index"; - ASSERT_NEAR(7.9579, ecl->elements[5].dwellPosition.angleOffsetFromTriggerEvent, EPS2D) << "angle offset#2"; + ASSERT_EQ(true, ecl->isReady) << "ford inline ignition events size"; + EXPECT_NEAR(ecl->elements[0].dwellAngle, 7.960f, 1e-3); + EXPECT_NEAR(ecl->elements[5].dwellAngle, 607.960f, 1e-3); ASSERT_FLOAT_EQ(0.5, engine->ignitionState.getSparkDwell(2000)) << "running dwell"; } @@ -318,8 +315,7 @@ TEST(misc, testRpmCalculator) { assertEqualsM("fuel #2", 4.5450, engine->engineState.injectionDuration); assertEqualsM("one degree", 111.1111, engine->rpmCalculator.oneDegreeUs); ASSERT_EQ( 1, ilist->isReady) << "size #2"; - ASSERT_EQ( 0, ilist->elements[0].dwellPosition.triggerEventIndex) << "dwell @ index"; - assertEqualsM("dwell offset", 8.5, ilist->elements[0].dwellPosition.angleOffsetFromTriggerEvent); + EXPECT_NEAR(ilist->elements[0].dwellAngle, 8.5f, 1e-3); ASSERT_EQ( 0, eth.engine.triggerCentral.triggerState.getCurrentIndex()) << "index #2"; ASSERT_EQ( 4, engine->executor.size()) << "queue size/2";