| Line | Branch | Decision | Exec | Source |
|---|---|---|---|---|
| 1 | #include "pch.h" | |||
| 2 | #include "gear_detector.h" | |||
| 3 | ||||
| 4 | 98 | static constexpr float geometricMean(float x, float y) { | ||
| 5 | 98 | return sqrtf(x * y); | ||
| 6 | } | |||
| 7 | ||||
| 8 | 677 | GearDetector::GearDetector() | ||
| 9 | 677 | : Sensor(SensorType::DetectedGear) | ||
| 10 | { | |||
| 11 | 677 | } | ||
| 12 | ||||
| 13 | 677 | GearDetector::~GearDetector() { | ||
| 14 | 677 | unregister(); | ||
| 15 | 677 | } | ||
| 16 | ||||
| 17 | 812 | void GearDetector::initGearDetector() { | ||
| 18 | // Compute gear thresholds between gears | |||
| 19 | ||||
| 20 | 812 | uint8_t gearCount = engineConfiguration->totalGearsCount; | ||
| 21 | ||||
| 22 |
2/2✓ Branch 0 taken 786 times.
✓ Branch 1 taken 26 times.
|
2/2✓ Decision 'true' taken 786 times.
✓ Decision 'false' taken 26 times.
|
812 | if (gearCount == 0) { |
| 23 | // No gears, nothing to do here. | |||
| 24 | 786 | return; | ||
| 25 | } | |||
| 26 | ||||
| 27 |
2/2✓ Branch 0 taken 1 time.
✓ Branch 1 taken 25 times.
|
2/2✓ Decision 'true' taken 1 time.
✓ Decision 'false' taken 25 times.
|
26 | if (gearCount > TCU_GEAR_COUNT) { |
| 28 | 1 | criticalError("too many gears"); | ||
| 29 | ✗ | return; | ||
| 30 | } | |||
| 31 | ||||
| 32 | // validate gears | |||
| 33 |
2/2✓ Branch 0 taken 125 times.
✓ Branch 1 taken 24 times.
|
2/2✓ Decision 'true' taken 125 times.
✓ Decision 'false' taken 24 times.
|
149 | for (size_t i = 0; i < gearCount; i++) { |
| 34 |
2/2✓ Branch 2 taken 1 time.
✓ Branch 3 taken 124 times.
|
2/2✓ Decision 'true' taken 1 time.
✓ Decision 'false' taken 124 times.
|
125 | if (engineConfiguration->gearRatio[i] <= 0) { |
| 35 | 1 | criticalError("Expecting positive gear ratio for #%d", i + 1); | ||
| 36 | ✗ | return; | ||
| 37 | } | |||
| 38 | } | |||
| 39 | ||||
| 40 |
2/2✓ Branch 0 taken 99 times.
✓ Branch 1 taken 23 times.
|
2/2✓ Decision 'true' taken 99 times.
✓ Decision 'false' taken 23 times.
|
122 | for (int i = 0; i < gearCount - 1; i++) { |
| 41 | // Threshold i is the threshold between gears i and i+1 | |||
| 42 | 99 | float gearI = engineConfiguration->gearRatio[i]; | ||
| 43 | 99 | float gearIplusOne = engineConfiguration->gearRatio[i + 1]; | ||
| 44 | ||||
| 45 |
2/2✓ Branch 0 taken 1 time.
✓ Branch 1 taken 98 times.
|
2/2✓ Decision 'true' taken 1 time.
✓ Decision 'false' taken 98 times.
|
99 | if (gearI <= gearIplusOne) { |
| 46 | 1 | criticalError("Invalid gear ordering near gear #%d", i + 1); | ||
| 47 | } | |||
| 48 | ||||
| 49 | 98 | m_gearThresholds[i] = geometricMean(gearI, gearIplusOne); | ||
| 50 | } | |||
| 51 | ||||
| 52 | 23 | Register(); | ||
| 53 | } | |||
| 54 | ||||
| 55 | 229 | void GearDetector::onConfigurationChange(engine_configuration_s const * /*previousConfig*/) { | ||
| 56 | 229 | initGearDetector(); | ||
| 57 | 226 | } | ||
| 58 | ||||
| 59 | 1085 | void GearDetector::onSlowCallback() { | ||
| 60 |
2/2✓ Branch 0 taken 583 times.
✓ Branch 1 taken 502 times.
|
2/2✓ Decision 'true' taken 583 times.
✓ Decision 'false' taken 502 times.
|
1085 | if (!isInitialized) { |
| 61 | 583 | initGearDetector(); | ||
| 62 | 583 | isInitialized = true; | ||
| 63 | } | |||
| 64 | ||||
| 65 | 1085 | float ratio = computeGearboxRatio(); | ||
| 66 | 1085 | m_gearboxRatio = ratio; | ||
| 67 | ||||
| 68 | 1085 | m_currentGear = determineGearFromRatio(ratio); | ||
| 69 | 1085 | } | ||
| 70 | ||||
| 71 | 1128 | size_t GearDetector::determineGearFromRatio(float ratio) const { | ||
| 72 | 1128 | auto gearCount = engineConfiguration->totalGearsCount; | ||
| 73 |
2/2✓ Branch 0 taken 986 times.
✓ Branch 1 taken 142 times.
|
2/2✓ Decision 'true' taken 986 times.
✓ Decision 'false' taken 142 times.
|
1128 | if (gearCount == 0) { |
| 74 | // No gears, we only have neutral. | |||
| 75 | 986 | return 0; | ||
| 76 | } | |||
| 77 | ||||
| 78 | // 1.5x first gear is neutral or clutch slip or something | |||
| 79 |
2/2✓ Branch 1 taken 7 times.
✓ Branch 2 taken 135 times.
|
2/2✓ Decision 'true' taken 7 times.
✓ Decision 'false' taken 135 times.
|
142 | if (ratio > engineConfiguration->gearRatio[0] * 1.5f) { |
| 80 | 7 | return 0; | ||
| 81 | } | |||
| 82 | ||||
| 83 | // 0.66x top gear is coasting with engine off or something | |||
| 84 |
2/2✓ Branch 2 taken 102 times.
✓ Branch 3 taken 33 times.
|
2/2✓ Decision 'true' taken 102 times.
✓ Decision 'false' taken 33 times.
|
135 | if (ratio < engineConfiguration->gearRatio[gearCount - 1] * 0.66f) { |
| 85 | 102 | return 0; | ||
| 86 | } | |||
| 87 | ||||
| 88 | 33 | size_t currentGear = gearCount; | ||
| 89 | ||||
| 90 |
2/2✓ Branch 0 taken 100 times.
✓ Branch 1 taken 11 times.
|
2/2✓ Decision 'true' taken 100 times.
✓ Decision 'false' taken 11 times.
|
111 | while (currentGear > 1) { |
| 91 |
2/2✓ Branch 0 taken 22 times.
✓ Branch 1 taken 78 times.
|
2/2✓ Decision 'true' taken 22 times.
✓ Decision 'false' taken 78 times.
|
100 | if (ratio < m_gearThresholds[currentGear - 2]) { |
| 92 | 22 | break; | ||
| 93 | } | |||
| 94 | ||||
| 95 | 78 | currentGear--; | ||
| 96 | } | |||
| 97 | ||||
| 98 | 33 | return currentGear; | ||
| 99 | } | |||
| 100 | ||||
| 101 | 1092 | float GearDetector::getDriveshaftRpm() const { | ||
| 102 | 1092 | auto vssKph = Sensor::getOrZero(SensorType::VehicleSpeed); | ||
| 103 | ||||
| 104 |
2/2✓ Branch 0 taken 969 times.
✓ Branch 1 taken 123 times.
|
2/2✓ Decision 'true' taken 969 times.
✓ Decision 'false' taken 123 times.
|
1092 | if (vssKph < 3) { |
| 105 | // Vehicle too slow to determine gearbox ratio, avoid div/0 | |||
| 106 | 969 | return 0; | ||
| 107 | } | |||
| 108 | ||||
| 109 | // Convert to wheel RPM | |||
| 110 | // km rev 1 hr | |||
| 111 | // ------ * ------------ * __________ | |||
| 112 | // hr km 60 min | |||
| 113 | 123 | float wheelRpm = vssKph * engineConfiguration->driveWheelRevPerKm * (1 / 60.0f); | ||
| 114 | ||||
| 115 | // Convert to driveshaft RPM | |||
| 116 | 123 | return wheelRpm * engineConfiguration->finalGearRatio; | ||
| 117 | } | |||
| 118 | ||||
| 119 | 1085 | float GearDetector::computeGearboxRatio() const { | ||
| 120 | 1085 | float driveshaftRpm = getDriveshaftRpm(); | ||
| 121 | ||||
| 122 |
2/2✓ Branch 0 taken 967 times.
✓ Branch 1 taken 118 times.
|
2/2✓ Decision 'true' taken 967 times.
✓ Decision 'false' taken 118 times.
|
1085 | if (driveshaftRpm == 0) { |
| 123 | 967 | return 0; | ||
| 124 | } | |||
| 125 | ||||
| 126 | float engineRpm; | |||
| 127 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 118 times.
|
1/2✗ Decision 'true' not taken.
✓ Decision 'false' taken 118 times.
|
118 | if (Sensor::hasSensor(SensorType::InputShaftSpeed)) { |
| 128 | ✗ | engineRpm = Sensor::getOrZero(SensorType::InputShaftSpeed); | ||
| 129 | } else { | |||
| 130 | 118 | engineRpm = Sensor::getOrZero(SensorType::Rpm); | ||
| 131 | } | |||
| 132 | ||||
| 133 | 118 | return engineRpm / driveshaftRpm; | ||
| 134 | } | |||
| 135 | ||||
| 136 | 11 | float GearDetector::getRpmInGear(size_t gear) const { | ||
| 137 |
4/4✓ Branch 0 taken 9 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 7 times.
|
2/2✓ Decision 'true' taken 4 times.
✓ Decision 'false' taken 7 times.
|
11 | if (gear <= 0 || gear > engineConfiguration->totalGearsCount) { |
| 138 | 4 | return 0; | ||
| 139 | } | |||
| 140 | ||||
| 141 | // Ideal engine RPM is driveshaft speed times gear | |||
| 142 | 7 | return getDriveshaftRpm() * engineConfiguration->gearRatio[gear - 1]; | ||
| 143 | } | |||
| 144 | ||||
| 145 | 522960 | float GearDetector::getGearboxRatio() const { | ||
| 146 | 522960 | return m_gearboxRatio; | ||
| 147 | } | |||
| 148 | ||||
| 149 | 14668 | SensorResult GearDetector::get() const { | ||
| 150 | 14668 | return m_currentGear; | ||
| 151 | } | |||
| 152 | ||||
| 153 | ✗ | void GearDetector::showInfo(const char* sensorName) const { | ||
| 154 | ✗ | efiPrintf("Sensor \"%s\" is gear detector.", sensorName); | ||
| 155 | ✗ | efiPrintf(" Gearbox ratio: %.3f", m_gearboxRatio); | ||
| 156 | ✗ | efiPrintf(" Detected gear: %d", m_currentGear); | ||
| 157 | ✗ | } | ||
| 158 |