diff --git a/firmware/controllers/trigger/decoders/trigger_structure.cpp b/firmware/controllers/trigger/decoders/trigger_structure.cpp index 425384ae63..1f43828b82 100644 --- a/firmware/controllers/trigger/decoders/trigger_structure.cpp +++ b/firmware/controllers/trigger/decoders/trigger_structure.cpp @@ -159,7 +159,10 @@ angle_t TriggerWaveform::getAngle(int index) const { int crankCycle = index / privateTriggerDefinitionSize; int remainder = index % privateTriggerDefinitionSize; - return getCycleDuration() * crankCycle + getSwitchAngle(remainder); + auto cycleStartAngle = getCycleDuration() * crankCycle; + auto positionWithinCycle = getSwitchAngle(remainder); + + return cycleStartAngle + positionWithinCycle; } void TriggerWaveform::addEventClamped(angle_t angle, trigger_wheel_e const channelIndex, trigger_value_e const stateParam, float filterLeft, float filterRight) { @@ -418,7 +421,10 @@ void findTriggerPosition(TriggerWaveform *triggerShape, int triggerEventIndex = details->triggerIndexByAngle[(int)angle]; angle_t triggerEventAngle = details->eventAngles[triggerEventIndex]; - if (angle < triggerEventAngle) { + angle_t offsetFromTriggerEvent = angle - triggerEventAngle; + + // Guarantee that we aren't going to try and schedule an event prior to the tooth + if (offsetFromTriggerEvent < 0) { warning(CUSTOM_OBD_ANGLE_CONSTRAINT_VIOLATION, "angle constraint violation in findTriggerPosition(): %.2f/%.2f", angle, triggerEventAngle); return; } @@ -428,7 +434,7 @@ void findTriggerPosition(TriggerWaveform *triggerShape, chibios_rt::CriticalSectionLocker csl; position->triggerEventIndex = triggerEventIndex; - position->angleOffsetFromTriggerEvent = angle - triggerEventAngle; + position->angleOffsetFromTriggerEvent = offsetFromTriggerEvent; } } diff --git a/firmware/controllers/trigger/trigger_decoder.cpp b/firmware/controllers/trigger/trigger_decoder.cpp index 1866d9b9cd..db81ecf7c0 100644 --- a/firmware/controllers/trigger/trigger_decoder.cpp +++ b/firmware/controllers/trigger/trigger_decoder.cpp @@ -170,6 +170,12 @@ void prepareEventAngles(TriggerWaveform *shape, memset(details->eventAngles, 0, sizeof(details->eventAngles)); + // this may be privateTriggerDefinitionSize; + + assertAngleRange(shape->triggerShapeSynchPointIndex, "triggerShapeSynchPointIndex", CUSTOM_TRIGGER_SYNC_ANGLE2); + efiAssertVoid(CUSTOM_TRIGGER_CYCLE, engine->engineCycleEventCount != 0, "zero engineCycleEventCount"); + for (int eventIndex = 0; eventIndex < length; eventIndex++) { if (eventIndex == 0) { // explicit check for zero to avoid issues where logical zero is not exactly zero due to float nature @@ -177,15 +183,25 @@ void prepareEventAngles(TriggerWaveform *shape, // this value would be used in case of front-only details->eventAngles[1] = 0; } else { - assertAngleRange(shape->triggerShapeSynchPointIndex, "triggerShapeSynchPointIndex", CUSTOM_TRIGGER_SYNC_ANGLE2); - unsigned int triggerDefinitionCoordinate = (shape->triggerShapeSynchPointIndex + eventIndex) % length; - efiAssertVoid(CUSTOM_TRIGGER_CYCLE, engine->engineCycleEventCount != 0, "zero engineCycleEventCount"); - int triggerDefinitionIndex = triggerDefinitionCoordinate >= shape->privateTriggerDefinitionSize ? triggerDefinitionCoordinate - shape->privateTriggerDefinitionSize : triggerDefinitionCoordinate; - float angle = shape->getAngle(triggerDefinitionCoordinate) - firstAngle; + // Rotate the trigger around so that the sync point is at position 0 + auto wrappedIndex = (shape->triggerShapeSynchPointIndex + eventIndex) % length; + + // Compute this tooth's position within the trigger definition + // (wrap, as the trigger def may be smaller than total trigger length) + auto triggerDefinitionIndex = wrappedIndex % triggerShapeLength; + + // Compute the relative angle of this tooth to the sync point's tooth + float angle = shape->getAngle(wrappedIndex) - firstAngle; + efiAssertVoid(CUSTOM_TRIGGER_CYCLE, !cisnan(angle), "trgSyncNaN"); + // Wrap the angle back in to [0, 720) fixAngle(angle, "trgSync", CUSTOM_TRIGGER_SYNC_ANGLE_RANGE); + if (engineConfiguration->useOnlyRisingEdgeForTrigger) { - assertIsInBounds(triggerDefinitionIndex, shape->isRiseEvent, "isRise"); + efiAssertVoid(OBD_PCM_Processor_Fault, triggerDefinitionIndex < triggerShapeLength, "trigger shape fail"); + assertIsInBounds(triggerDefinitionIndex, shape->isRiseEvent, "isRise"); + + // In case this is a rising event, replace the following fall event with the rising as well if (shape->isRiseEvent[triggerDefinitionIndex]) { riseOnlyIndex += 2; details->eventAngles[riseOnlyIndex] = angle; diff --git a/unit_tests/tests/trigger/test_symmetrical_crank.cpp b/unit_tests/tests/trigger/test_symmetrical_crank.cpp index 48081524a2..b77d5f4110 100644 --- a/unit_tests/tests/trigger/test_symmetrical_crank.cpp +++ b/unit_tests/tests/trigger/test_symmetrical_crank.cpp @@ -21,20 +21,25 @@ TEST(engine, testAngleLogicInSymmetricalCrankIssue2980) { TriggerWaveform * form = &ENGINE(triggerCentral.triggerShape); - ASSERT_EQ(form->findAngleIndex(triggerForm, 10), 1); - ASSERT_EQ(form->findAngleIndex(triggerForm, 180), 5); + #define EXPECT_FINDANGLE(angle, idx) EXPECT_EQ(form->findAngleIndex(triggerForm, angle) & 0xFFFF'FFFE, idx); - ASSERT_EQ(form->findAngleIndex(triggerForm, 310), 5); - ASSERT_EQ(form->findAngleIndex(triggerForm, 540), 5); + // Check one angle just after every trigger tooth, for two full revolutions (720 degrees, one engine cycle, 4 loops of the trigger) - ASSERT_EQ(form->findAngleIndex(triggerForm, 640), 5); - ASSERT_EQ(form->findAngleIndex(triggerForm, 650), 7); - ASSERT_EQ(form->findAngleIndex(triggerForm, 660), 15); - ASSERT_EQ(form->findAngleIndex(triggerForm, 670), 15); - ASSERT_EQ(form->findAngleIndex(triggerForm, 680), 15); + // First quarter + EXPECT_FINDANGLE(0 * 180 + 5, 0); // 5 + EXPECT_FINDANGLE(0 * 180 + 115, 2); // 115 - ASSERT_EQ(form->findAngleIndex(triggerForm, 700), 15); - ASSERT_EQ(form->findAngleIndex(triggerForm, 710), 15); + // Second quarter + EXPECT_FINDANGLE(1 * 180 + 5, 4); // 180+5 = 185 + EXPECT_FINDANGLE(1 * 180 + 115, 6); // 180+115 = 295 + + // Third quarter + EXPECT_FINDANGLE(2 * 180 + 5, 8); // 360+5 = 365 + EXPECT_FINDANGLE(2 * 180 + 115, 10); // 360+115 = 475 + + // Fourth quarter + EXPECT_FINDANGLE(3 * 180 + 5, 12); // 540+5 = 545 + EXPECT_FINDANGLE(3 * 180 + 115, 14); // 540+115 = 655 } TEST(engine, testSymmetricalCrank) {