| Line | Branch | Decision | Exec | Source |
|---|---|---|---|---|
| 1 | ||||
| 2 | #include "pch.h" | |||
| 3 | #include "instant_rpm_calculator.h" | |||
| 4 | ||||
| 5 | /** | |||
| 6 | * sensorChartMode | |||
| 7 | */ | |||
| 8 | #include "engine_state.h" | |||
| 9 | ||||
| 10 | #if EFI_UNIT_TEST | |||
| 11 | extern bool printTriggerDebug; | |||
| 12 | #endif | |||
| 13 | ||||
| 14 | #if EFI_SHAFT_POSITION_INPUT | |||
| 15 | ||||
| 16 | 678 | InstantRpmCalculator::InstantRpmCalculator() : | ||
| 17 | //https://en.cppreference.com/w/cpp/language/zero_initialization | |||
| 18 |
2/2✓ Branch 0 taken 189840 times.
✓ Branch 1 taken 678 times.
|
190518 | timeOfLastEvent() | |
| 19 |
2/2✓ Branch 0 taken 189840 times.
✓ Branch 1 taken 678 times.
|
190518 | , instantRpmValue() | |
| 20 | { | |||
| 21 | 678 | } | ||
| 22 | ||||
| 23 | 170 | void InstantRpmCalculator::movePreSynchTimestamps() { | ||
| 24 | // here we take timestamps of events which happened prior to synchronization and place them | |||
| 25 | // at appropriate locations | |||
| 26 | 170 | auto triggerSize = getTriggerCentral()->triggerShape.getLength(); | ||
| 27 | ||||
| 28 | 170 | size_t eventsToCopy = minI(spinningEventIndex, triggerSize); | ||
| 29 | ||||
| 30 | size_t firstSrc; | |||
| 31 | size_t firstDst; | |||
| 32 | ||||
| 33 |
2/2✓ Branch 0 taken 18 times.
✓ Branch 1 taken 152 times.
|
2/2✓ Decision 'true' taken 18 times.
✓ Decision 'false' taken 152 times.
|
170 | if (eventsToCopy >= triggerSize) { |
| 34 | // Only copy one trigger length worth of events, filling the whole buffer | |||
| 35 | 18 | firstSrc = spinningEventIndex - triggerSize; | ||
| 36 | 18 | firstDst = 0; | ||
| 37 | } else { | |||
| 38 | // There is less than one full cycle, copy to the end of the buffer | |||
| 39 | 152 | firstSrc = 0; | ||
| 40 | 152 | firstDst = triggerSize - spinningEventIndex; | ||
| 41 | } | |||
| 42 | ||||
| 43 | 170 | memcpy(timeOfLastEvent + firstDst, spinningEvents + firstSrc, eventsToCopy * sizeof(timeOfLastEvent[0])); | ||
| 44 | 170 | } | ||
| 45 | ||||
| 46 | 33836 | float InstantRpmCalculator::calculateInstantRpm( | ||
| 47 | TriggerWaveform const & triggerShape, TriggerFormDetails *triggerFormDetails, | |||
| 48 | uint32_t current_index, efitick_t nowNt) { | |||
| 49 | ||||
| 50 | // It's OK to truncate from 64b to 32b, ARM with single precision FPU uses an expensive | |||
| 51 | // software function to convert 64b int -> float, while 32b int -> float is very cheap hardware conversion | |||
| 52 | // The difference is guaranteed to be short (it's 90 degrees of engine rotation!), so it won't overflow. | |||
| 53 | 33836 | uint32_t nowNt32 = nowNt; | ||
| 54 | ||||
| 55 |
1/3✗ Branch 1 not taken.
✓ Branch 2 taken 33836 times.
✗ Branch 4 not taken.
|
33836 | assertIsInBoundsWithResult(current_index, timeOfLastEvent, "calc timeOfLastEvent", 0); | |
| 56 | ||||
| 57 | // Record the time of this event so we can calculate RPM from it later | |||
| 58 | 33836 | timeOfLastEvent[current_index] = nowNt32; | ||
| 59 | ||||
| 60 | // Determine where we currently are in the revolution | |||
| 61 | 33836 | angle_t currentAngle = triggerFormDetails->eventAngles[current_index]; | ||
| 62 |
1/3✗ Branch 1 not taken.
✓ Branch 2 taken 33836 times.
✗ Branch 4 not taken.
|
33836 | efiAssert(ObdCode::OBD_PCM_Processor_Fault, !std::isnan(currentAngle), "eventAngles", 0); | |
| 63 | ||||
| 64 | // Hunt for a tooth ~90 degrees ago to compare to the current time | |||
| 65 | 33836 | angle_t previousAngle = currentAngle - 90; | ||
| 66 |
1/1✓ Branch 1 taken 33836 times.
|
33836 | wrapAngle(previousAngle, "prevAngle", ObdCode::CUSTOM_ERR_TRIGGER_ANGLE_RANGE); | |
| 67 |
1/1✓ Branch 1 taken 33836 times.
|
33836 | int prevIndex = triggerShape.findAngleIndex(triggerFormDetails, previousAngle); | |
| 68 | ||||
| 69 | // now let's get precise angle for that event | |||
| 70 | 33836 | angle_t prevIndexAngle = triggerFormDetails->eventAngles[prevIndex]; | ||
| 71 | 33836 | auto time90ago = timeOfLastEvent[prevIndex]; | ||
| 72 | ||||
| 73 | // No previous timestamp, instant RPM isn't ready yet | |||
| 74 |
2/2✓ Branch 0 taken 361 times.
✓ Branch 1 taken 33475 times.
|
2/2✓ Decision 'true' taken 361 times.
✓ Decision 'false' taken 33475 times.
|
33836 | if (time90ago == 0) { |
| 75 | 361 | return prevInstantRpmValue; | ||
| 76 | } | |||
| 77 | ||||
| 78 | 33475 | uint32_t time = nowNt32 - time90ago; | ||
| 79 | 33475 | angle_t angleDiff = currentAngle - prevIndexAngle; | ||
| 80 | ||||
| 81 | // Wrap the angle in to the correct range (ie, could be -630 when we want +90) | |||
| 82 |
1/1✓ Branch 1 taken 33475 times.
|
33475 | wrapAngle(angleDiff, "angleDiff", ObdCode::CUSTOM_ERR_6561); | |
| 83 | ||||
| 84 | // just for safety, avoid divide-by-0 | |||
| 85 |
2/2✓ Branch 0 taken 20 times.
✓ Branch 1 taken 33455 times.
|
2/2✓ Decision 'true' taken 20 times.
✓ Decision 'false' taken 33455 times.
|
33475 | if (time == 0) { |
| 86 | 20 | return prevInstantRpmValue; | ||
| 87 | } | |||
| 88 | ||||
| 89 | 33455 | float instantRpm = (60000000.0 / 360 * US_TO_NT_MULTIPLIER) * angleDiff / time; | ||
| 90 |
1/3✗ Branch 1 not taken.
✓ Branch 2 taken 33455 times.
✗ Branch 4 not taken.
|
33455 | assertIsInBoundsWithResult(current_index, instantRpmValue, "instantRpmValue", 0); | |
| 91 | 33455 | instantRpmValue[current_index] = instantRpm; | ||
| 92 | ||||
| 93 | // This fixes early RPM instability based on incomplete data | |||
| 94 |
2/2✓ Branch 0 taken 15 times.
✓ Branch 1 taken 33440 times.
|
2/2✓ Decision 'true' taken 15 times.
✓ Decision 'false' taken 33440 times.
|
33455 | if (instantRpm < RPM_LOW_THRESHOLD) { |
| 95 | 15 | return prevInstantRpmValue; | ||
| 96 | } | |||
| 97 | ||||
| 98 | 33440 | prevInstantRpmValue = instantRpm; | ||
| 99 | ||||
| 100 | 33440 | m_instantRpmRatio = instantRpm / instantRpmValue[prevIndex]; | ||
| 101 | ||||
| 102 | 33440 | return instantRpm; | ||
| 103 | } | |||
| 104 | ||||
| 105 | 2741 | void InstantRpmCalculator::setLastEventTimeForInstantRpm(efitick_t nowNt) { | ||
| 106 | // here we remember tooth timestamps which happen prior to synchronization | |||
| 107 |
2/2✓ Branch 1 taken 803 times.
✓ Branch 2 taken 1938 times.
|
2/2✓ Decision 'true' taken 803 times.
✓ Decision 'false' taken 1938 times.
|
2741 | if (spinningEventIndex >= efi::size(spinningEvents)) { |
| 108 | // too many events while trying to find synchronization point | |||
| 109 | // todo: better implementation would be to shift here or use cyclic buffer so that we keep last | |||
| 110 | // 'PRE_SYNC_EVENTS' events | |||
| 111 | 803 | return; | ||
| 112 | } | |||
| 113 | ||||
| 114 | 1938 | uint32_t nowNt32 = nowNt; | ||
| 115 | 1938 | spinningEvents[spinningEventIndex] = nowNt32; | ||
| 116 | ||||
| 117 | // If we are using only rising edges, we never write in to the odd-index slots that | |||
| 118 | // would be used by falling edges | |||
| 119 | // TODO: don't reach across to trigger central to get this info | |||
| 120 |
2/2✓ Branch 1 taken 1656 times.
✓ Branch 2 taken 282 times.
|
1938 | spinningEventIndex += getTriggerCentral()->triggerShape.useOnlyRisingEdges ? 2 : 1; | |
| 121 | } | |||
| 122 | ||||
| 123 | 33836 | void InstantRpmCalculator::updateInstantRpm( | ||
| 124 | uint32_t current_index, | |||
| 125 | TriggerWaveform const & triggerShape, TriggerFormDetails *triggerFormDetails, | |||
| 126 | uint32_t index, efitick_t nowNt) { | |||
| 127 | UNUSED(current_index); | |||
| 128 | ||||
| 129 | 33836 | m_instantRpm = calculateInstantRpm(triggerShape, triggerFormDetails, index, | ||
| 130 | nowNt); | |||
| 131 | #if EFI_UNIT_TEST | |||
| 132 |
2/2✓ Branch 0 taken 26400 times.
✓ Branch 1 taken 7436 times.
|
2/2✓ Decision 'true' taken 26400 times.
✓ Decision 'false' taken 7436 times.
|
33836 | if (printTriggerDebug) { |
| 133 | 26400 | printf("instantRpm = %f\n", m_instantRpm); | ||
| 134 | } | |||
| 135 | #endif | |||
| 136 | ||||
| 137 | 33836 | } | ||
| 138 | ||||
| 139 | #endif // EFI_SHAFT_POSITION_INPUT | |||
| 140 |