diff --git a/firmware/controllers/algo/event_registry.cpp b/firmware/controllers/algo/event_registry.cpp index 3b7d234eec..905491912c 100644 --- a/firmware/controllers/algo/event_registry.cpp +++ b/firmware/controllers/algo/event_registry.cpp @@ -24,6 +24,49 @@ #include "event_registry.h" +static void findTriggerPosition(TriggerWaveform *triggerShape, + TriggerFormDetails *details, + event_trigger_position_s *position, + angle_t angle) { + efiAssertVoid(CUSTOM_ERR_6574, !cisnan(angle), "findAngle#1"); + assertAngleRange(angle, "findAngle#a1", CUSTOM_ERR_6545); + + efiAssertVoid(CUSTOM_ERR_6575, !cisnan(triggerShape->tdcPosition), "tdcPos#1") + assertAngleRange(triggerShape->tdcPosition, "tdcPos#a1", CUSTOM_UNEXPECTED_TDC_ANGLE); + + efiAssertVoid(CUSTOM_ERR_6576, !cisnan(engineConfiguration->globalTriggerAngleOffset), "tdcPos#2") + assertAngleRange(engineConfiguration->globalTriggerAngleOffset, "tdcPos#a2", CUSTOM_INVALID_GLOBAL_OFFSET); + + // convert engine cycle angle into trigger cycle angle + angle += triggerShape->tdcPosition + engineConfiguration->globalTriggerAngleOffset; + efiAssertVoid(CUSTOM_ERR_6577, !cisnan(angle), "findAngle#2"); + wrapAngle2(angle, "addFuel#2", CUSTOM_ERR_6555, getEngineCycle(triggerShape->getWheelOperationMode())); + + int triggerEventIndex = triggerShape->findAngleIndex(details, angle); + angle_t triggerEventAngle = details->eventAngles[triggerEventIndex]; + 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; + } + + { + // This must happen under lock so that the tooth and offset don't get partially read and mismatched + chibios_rt::CriticalSectionLocker csl; + + position->triggerEventIndex = triggerEventIndex; + position->angleOffsetFromTriggerEvent = offsetFromTriggerEvent; + } +} + +void event_trigger_position_s::setAngle(angle_t angle) { + findTriggerPosition(&engine->triggerCentral.triggerShape, + &engine->triggerCentral.triggerFormDetails, + this, angle); +} + IgnitionEvent::IgnitionEvent() { memset(outputs, 0, sizeof(outputs)); } diff --git a/firmware/controllers/algo/event_registry.h b/firmware/controllers/algo/event_registry.h index 8c0ec23e49..2267dc240c 100644 --- a/firmware/controllers/algo/event_registry.h +++ b/firmware/controllers/algo/event_registry.h @@ -29,6 +29,18 @@ struct AngleBasedEventBase { virtual AngleBasedEventOld* asOld() { return nullptr; } }; +/** + * This structure defines an angle position in relation to specific tooth within trigger shape + */ +class event_trigger_position_s { +public: + size_t triggerEventIndex = 0; + + angle_t angleOffsetFromTriggerEvent = 0; + + void setAngle(angle_t angle); +}; + struct AngleBasedEventOld : public AngleBasedEventBase { event_trigger_position_s position; diff --git a/firmware/controllers/trigger/decoders/trigger_structure.cpp b/firmware/controllers/trigger/decoders/trigger_structure.cpp index 4191bae6f0..17be2ed08a 100644 --- a/firmware/controllers/trigger/decoders/trigger_structure.cpp +++ b/firmware/controllers/trigger/decoders/trigger_structure.cpp @@ -43,12 +43,6 @@ #include "sensor_chart.h" #endif /* EFI_SENSOR_CHART */ -void event_trigger_position_s::setAngle(angle_t angle) { - findTriggerPosition(&engine->triggerCentral.triggerShape, - &engine->triggerCentral.triggerFormDetails, - this, angle); -} - TriggerWaveform::TriggerWaveform() { initialize(OM_NONE, SyncEdge::Rise); } @@ -389,43 +383,6 @@ void TriggerWaveform::setShapeDefinitionError(bool value) { shapeDefinitionError = value; } -void findTriggerPosition(TriggerWaveform *triggerShape, - TriggerFormDetails *details, - event_trigger_position_s *position, - angle_t angle) { - efiAssertVoid(CUSTOM_ERR_6574, !cisnan(angle), "findAngle#1"); - assertAngleRange(angle, "findAngle#a1", CUSTOM_ERR_6545); - - efiAssertVoid(CUSTOM_ERR_6575, !cisnan(triggerShape->tdcPosition), "tdcPos#1") - assertAngleRange(triggerShape->tdcPosition, "tdcPos#a1", CUSTOM_UNEXPECTED_TDC_ANGLE); - - efiAssertVoid(CUSTOM_ERR_6576, !cisnan(engineConfiguration->globalTriggerAngleOffset), "tdcPos#2") - assertAngleRange(engineConfiguration->globalTriggerAngleOffset, "tdcPos#a2", CUSTOM_INVALID_GLOBAL_OFFSET); - - // convert engine cycle angle into trigger cycle angle - angle += triggerShape->tdcPosition + engineConfiguration->globalTriggerAngleOffset; - efiAssertVoid(CUSTOM_ERR_6577, !cisnan(angle), "findAngle#2"); - wrapAngle2(angle, "addFuel#2", CUSTOM_ERR_6555, getEngineCycle(triggerShape->getWheelOperationMode())); - - int triggerEventIndex = triggerShape->findAngleIndex(details, angle); - angle_t triggerEventAngle = details->eventAngles[triggerEventIndex]; - 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; - } - - { - // This must happen under lock so that the tooth and offset don't get partially read and mismatched - chibios_rt::CriticalSectionLocker csl; - - position->triggerEventIndex = triggerEventIndex; - position->angleOffsetFromTriggerEvent = offsetFromTriggerEvent; - } -} - void TriggerWaveform::setTriggerSynchronizationGap(float syncRatio) { setTriggerSynchronizationGap3(/*gapIndex*/0, syncRatio * TRIGGER_GAP_DEVIATION_LOW, syncRatio * TRIGGER_GAP_DEVIATION_HIGH); } diff --git a/firmware/controllers/trigger/decoders/trigger_structure.h b/firmware/controllers/trigger/decoders/trigger_structure.h index c2a60d3554..688e771a23 100644 --- a/firmware/controllers/trigger/decoders/trigger_structure.h +++ b/firmware/controllers/trigger/decoders/trigger_structure.h @@ -47,18 +47,6 @@ angle -= engineCycleDurationLocalCopy; \ } -/** - * This structure defines an angle position in relation to specific tooth within trigger shape - */ -class event_trigger_position_s { -public: - size_t triggerEventIndex = 0; - - angle_t angleOffsetFromTriggerEvent = 0; - - void setAngle(angle_t angle); -}; - class TriggerDecoderBase; class TriggerFormDetails; class TriggerConfiguration; @@ -279,10 +267,4 @@ public: angle_t eventAngles[2 * PWM_PHASE_MAX_COUNT]; }; -void findTriggerPosition( - TriggerWaveform *shape, - TriggerFormDetails *details, - event_trigger_position_s *position, - angle_t angle); - #define TRIGGER_WAVEFORM(x) getTriggerCentral()->triggerShape.x