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 | 677 | InstantRpmCalculator::InstantRpmCalculator() : | ||
17 | //https://en.cppreference.com/w/cpp/language/zero_initialization | |||
18 |
2/2✓ Branch 0 taken 189560 times.
✓ Branch 1 taken 677 times.
|
190237 | timeOfLastEvent() | |
19 |
2/2✓ Branch 0 taken 189560 times.
✓ Branch 1 taken 677 times.
|
190237 | , instantRpmValue() | |
20 | { | |||
21 | 677 | } | ||
22 | ||||
23 | 167 | void InstantRpmCalculator::movePreSynchTimestamps() { | ||
24 | // here we take timestamps of events which happened prior to synchronization and place them | |||
25 | // at appropriate locations | |||
26 | 167 | auto triggerSize = getTriggerCentral()->triggerShape.getLength(); | ||
27 | ||||
28 | 167 | 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 149 times.
|
2/2✓ Decision 'true' taken 18 times.
✓ Decision 'false' taken 149 times.
|
167 | 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 | 149 | firstSrc = 0; | ||
40 | 149 | firstDst = triggerSize - spinningEventIndex; | ||
41 | } | |||
42 | ||||
43 | 167 | memcpy(timeOfLastEvent + firstDst, spinningEvents + firstSrc, eventsToCopy * sizeof(timeOfLastEvent[0])); | ||
44 | 167 | } | ||
45 | ||||
46 | 31489 | 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 | 31489 | uint32_t nowNt32 = nowNt; | ||
54 | ||||
55 |
1/3✗ Branch 1 not taken.
✓ Branch 2 taken 31489 times.
✗ Branch 4 not taken.
|
31489 | assertIsInBoundsWithResult(current_index, timeOfLastEvent, "calc timeOfLastEvent", 0); | |
56 | ||||
57 | // Record the time of this event so we can calculate RPM from it later | |||
58 | 31489 | timeOfLastEvent[current_index] = nowNt32; | ||
59 | ||||
60 | // Determine where we currently are in the revolution | |||
61 | 31489 | angle_t currentAngle = triggerFormDetails->eventAngles[current_index]; | ||
62 |
1/3✗ Branch 1 not taken.
✓ Branch 2 taken 31489 times.
✗ Branch 4 not taken.
|
31489 | 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 | 31489 | angle_t previousAngle = currentAngle - 90; | ||
66 |
1/1✓ Branch 1 taken 31489 times.
|
31489 | wrapAngle(previousAngle, "prevAngle", ObdCode::CUSTOM_ERR_TRIGGER_ANGLE_RANGE); | |
67 |
1/1✓ Branch 1 taken 31489 times.
|
31489 | int prevIndex = triggerShape.findAngleIndex(triggerFormDetails, previousAngle); | |
68 | ||||
69 | // now let's get precise angle for that event | |||
70 | 31489 | angle_t prevIndexAngle = triggerFormDetails->eventAngles[prevIndex]; | ||
71 | 31489 | auto time90ago = timeOfLastEvent[prevIndex]; | ||
72 | ||||
73 | // No previous timestamp, instant RPM isn't ready yet | |||
74 |
2/2✓ Branch 0 taken 354 times.
✓ Branch 1 taken 31135 times.
|
2/2✓ Decision 'true' taken 354 times.
✓ Decision 'false' taken 31135 times.
|
31489 | if (time90ago == 0) { |
75 | 354 | return prevInstantRpmValue; | ||
76 | } | |||
77 | ||||
78 | 31135 | uint32_t time = nowNt32 - time90ago; | ||
79 | 31135 | 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 31135 times.
|
31135 | 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 31115 times.
|
2/2✓ Decision 'true' taken 20 times.
✓ Decision 'false' taken 31115 times.
|
31135 | if (time == 0) { |
86 | 20 | return prevInstantRpmValue; | ||
87 | } | |||
88 | ||||
89 | 31115 | float instantRpm = (60000000.0 / 360 * US_TO_NT_MULTIPLIER) * angleDiff / time; | ||
90 |
1/3✗ Branch 1 not taken.
✓ Branch 2 taken 31115 times.
✗ Branch 4 not taken.
|
31115 | assertIsInBoundsWithResult(current_index, instantRpmValue, "instantRpmValue", 0); | |
91 | 31115 | 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 31100 times.
|
2/2✓ Decision 'true' taken 15 times.
✓ Decision 'false' taken 31100 times.
|
31115 | if (instantRpm < RPM_LOW_THRESHOLD) { |
95 | 15 | return prevInstantRpmValue; | ||
96 | } | |||
97 | ||||
98 | 31100 | prevInstantRpmValue = instantRpm; | ||
99 | ||||
100 | 31100 | m_instantRpmRatio = instantRpm / instantRpmValue[prevIndex]; | ||
101 | ||||
102 | 31100 | return instantRpm; | ||
103 | } | |||
104 | ||||
105 | 2737 | 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 1934 times.
|
2/2✓ Decision 'true' taken 803 times.
✓ Decision 'false' taken 1934 times.
|
2737 | 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 | 1934 | uint32_t nowNt32 = nowNt; | ||
115 | 1934 | 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 1652 times.
✓ Branch 2 taken 282 times.
|
1934 | spinningEventIndex += getTriggerCentral()->triggerShape.useOnlyRisingEdges ? 2 : 1; | |
121 | } | |||
122 | ||||
123 | 31489 | 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 | 31489 | m_instantRpm = calculateInstantRpm(triggerShape, triggerFormDetails, index, | ||
130 | nowNt); | |||
131 | #if EFI_UNIT_TEST | |||
132 |
2/2✓ Branch 0 taken 24053 times.
✓ Branch 1 taken 7436 times.
|
2/2✓ Decision 'true' taken 24053 times.
✓ Decision 'false' taken 7436 times.
|
31489 | if (printTriggerDebug) { |
133 | 24053 | printf("instantRpm = %f\n", m_instantRpm); | ||
134 | } | |||
135 | #endif | |||
136 | ||||
137 | 31489 | } | ||
138 | ||||
139 | #endif // EFI_SHAFT_POSITION_INPUT | |||
140 |