| Line | Branch | Decision | Exec | Source |
|---|---|---|---|---|
| 1 | /** | |||
| 2 | * @file electronic_throttle_impl.h | |||
| 3 | * | |||
| 4 | * @date Dec 7, 2013 | |||
| 5 | * @author Andrey Belomutskiy, (c) 2012-2020 | |||
| 6 | */ | |||
| 7 | ||||
| 8 | #pragma once | |||
| 9 | ||||
| 10 | // include the "public" ETB interface | |||
| 11 | #include "electronic_throttle.h" | |||
| 12 | ||||
| 13 | #include "sensor.h" | |||
| 14 | #include "efi_pid.h" | |||
| 15 | #include "error_accumulator.h" | |||
| 16 | #include "electronic_throttle_generated.h" | |||
| 17 | #include "tunerstudio_calibration_channel.h" | |||
| 18 | ||||
| 19 | /** | |||
| 20 | * Hard code ETB update speed. | |||
| 21 | * Since this is a safety critical system with no real reason for a user to ever need to change the update rate, | |||
| 22 | * it's locked to 500hz, along with the ADC. | |||
| 23 | * https://en.wikipedia.org/wiki/Nyquist%E2%80%93Shannon_sampling_theorem | |||
| 24 | */ | |||
| 25 | #define ETB_LOOP_FREQUENCY 500 | |||
| 26 | #define DEFAULT_ETB_PWM_FREQUENCY 800 | |||
| 27 | ||||
| 28 | class EtbController : public IEtbController, public electronic_throttle_s { | |||
| 29 | public: | |||
| 30 | bool init(dc_function_e function, DcMotor *motor, pid_s *pidParameters, const ValueProvider3D* pedalMap) override; | |||
| 31 | void setIdlePosition(percent_t pos) override; | |||
| 32 | void setWastegatePosition(percent_t pos) override; | |||
| 33 | void reset(const char *reason) override; | |||
| 34 | ||||
| 35 | // Update the controller's state: read sensors, send output, etc | |||
| 36 | void update() override; | |||
| 37 | ||||
| 38 | // Called when the configuration may have changed. Controller will | |||
| 39 | // reset if necessary. | |||
| 40 | void onConfigurationChange(pid_s* previousConfiguration); | |||
| 41 | ||||
| 42 | // Print this throttle's status. | |||
| 43 | void showStatus(); | |||
| 44 | ||||
| 45 | // Helpers for individual parts of throttle control | |||
| 46 | expected<percent_t> observePlant() override; | |||
| 47 | ||||
| 48 | expected<percent_t> getSetpoint() override; | |||
| 49 | expected<percent_t> getSetpointEtb(); | |||
| 50 | expected<percent_t> getSetpointWastegate() const; | |||
| 51 | expected<percent_t> getSetpointIdleValve() const; | |||
| 52 | ||||
| 53 | expected<percent_t> getOpenLoop(percent_t target) override; | |||
| 54 | expected<percent_t> getClosedLoop(percent_t setpoint, percent_t observation) override; | |||
| 55 | expected<percent_t> getClosedLoopAutotune(percent_t setpoint, percent_t actualThrottlePosition); | |||
| 56 | ||||
| 57 | ✗ | dc_function_e getFunction() const { return m_function; } | ||
| 58 | ||||
| 59 | void checkJam(percent_t setpoint, percent_t observation); | |||
| 60 | ||||
| 61 | void setOutput(expected<percent_t> outputValue) override; | |||
| 62 | ||||
| 63 | // Used to inspect the internal PID controller's state | |||
| 64 | ✗ | const pid_state_s& getPidState() const override { return m_pid; }; | ||
| 65 | ||||
| 66 | // Override if this throttle needs special per-throttle adjustment (bank-to-bank trim, for example) | |||
| 67 | 242 | virtual percent_t getThrottleTrim(float /*rpm*/, percent_t /*targetPosition*/) const { | ||
| 68 | 242 | return 0; | ||
| 69 | } | |||
| 70 | ||||
| 71 | // pedal-based part of ETB target, without idle and other interventions | |||
| 72 | ✗ | float getCurrentTarget() const override { | ||
| 73 | ✗ | return etbCurrentTarget; | ||
| 74 | } | |||
| 75 | ||||
| 76 | 2721 | bool isEtbMode() const override { | ||
| 77 |
4/4✓ Branch 0 taken 1039 times.
✓ Branch 1 taken 1682 times.
✓ Branch 2 taken 1031 times.
✓ Branch 3 taken 8 times.
|
2721 | return m_function == DC_Throttle1 || m_function == DC_Throttle2; | |
| 78 | } | |||
| 79 | ||||
| 80 | // Lua throttle adjustment | |||
| 81 | void setLuaAdjustment(percent_t adjustment) override; | |||
| 82 | float getLuaAdjustment() const; | |||
| 83 | ||||
| 84 | float prevOutput = 0; | |||
| 85 | ||||
| 86 | protected: | |||
| 87 | bool hadTpsError = false; | |||
| 88 | bool hadPpsError = false; | |||
| 89 | ||||
| 90 | ✗ | DcMotor* getMotor() { return m_motor; } | ||
| 91 | ||||
| 92 | private: | |||
| 93 | dc_function_e m_function = DC_None; | |||
| 94 | SensorType m_positionSensor = SensorType::Invalid; | |||
| 95 | DcMotor *m_motor = nullptr; | |||
| 96 | Pid m_pid; | |||
| 97 | bool m_shouldResetPid = false; | |||
| 98 | ||||
| 99 | ErrorAccumulator m_targetErrorAccumulator; | |||
| 100 | ||||
| 101 | /** | |||
| 102 | * @return true if OK, false if should be disabled | |||
| 103 | */ | |||
| 104 | bool checkStatus(); | |||
| 105 | ||||
| 106 | Timer m_jamDetectTimer; | |||
| 107 | ||||
| 108 | // Pedal -> target map | |||
| 109 | const ValueProvider3D* m_pedalProvider = nullptr; | |||
| 110 | ||||
| 111 | float m_idlePosition = 0; | |||
| 112 | ||||
| 113 | // This is set if automatic PID cal should be run | |||
| 114 | bool m_isAutotune = false; | |||
| 115 | ||||
| 116 | // Autotune helpers | |||
| 117 | bool m_lastIsPositive = false; | |||
| 118 | Timer m_cycleTimer; | |||
| 119 | float m_minCycleTps = 0; | |||
| 120 | float m_maxCycleTps = 0; | |||
| 121 | // Autotune measured parameters: gain and ultimate period | |||
| 122 | // These are set to correct order of magnitude starting points | |||
| 123 | // so we converge more quickly on the correct values | |||
| 124 | float m_a = 8; | |||
| 125 | float m_tu = 0.1f; | |||
| 126 | ||||
| 127 | #if EFI_TUNER_STUDIO | |||
| 128 | uint8_t m_autotuneCounter = 0; | |||
| 129 | uint8_t m_autotuneCurrentParam = 0; | |||
| 130 | #endif | |||
| 131 | ||||
| 132 | Timer m_luaAdjustmentTimer; | |||
| 133 | ||||
| 134 | efitimeus_t lastTickUs; | |||
| 135 | }; | |||
| 136 | ||||
| 137 | void etbPidReset(); | |||
| 138 | ||||
| 139 | class EtbController1 : public EtbController { }; | |||
| 140 | ||||
| 141 | class EtbController2 : public EtbController { | |||
| 142 | public: | |||
| 143 | 2 | EtbController2(const ValueProvider3D& throttle2TrimTable) | ||
| 144 | 2 | : m_throttle2Trim(throttle2TrimTable) | ||
| 145 | { | |||
| 146 | 2 | } | ||
| 147 | ||||
| 148 | percent_t getThrottleTrim(float rpm, percent_t /*targetPosition*/) const override; | |||
| 149 | ||||
| 150 | private: | |||
| 151 | const ValueProvider3D& m_throttle2Trim; | |||
| 152 | }; | |||
| 153 | ||||
| 154 | template <typename TBase> | |||
| 155 | class EtbImpl final : public TBase { | |||
| 156 | private: | |||
| 157 | enum class ACPhase { | |||
| 158 | Stopped, | |||
| 159 | ||||
| 160 | Start, | |||
| 161 | ||||
| 162 | // Drive the motor open | |||
| 163 | Open, | |||
| 164 | ||||
| 165 | // Drive the motor closed | |||
| 166 | Close, | |||
| 167 | ||||
| 168 | // Write learned values to TS | |||
| 169 | TransmitPrimaryMax, | |||
| 170 | TransmitPrimaryMin, | |||
| 171 | TransmitSecondaryMax, | |||
| 172 | TransmitSecondaryMin, | |||
| 173 | }; | |||
| 174 | ||||
| 175 | public: | |||
| 176 | template <typename... TArgs> | |||
| 177 | 2 | EtbImpl(TArgs&&... args) : TBase(std::forward<TArgs>(args)...) { } | ||
| 178 | ||||
| 179 | 206 | void update() override { | ||
| 180 | #if EFI_TUNER_STUDIO | |||
| 181 |
1/4EtbImpl<EtbController1>::update():
✗ Branch 0 not taken.
✓ Branch 1 taken 206 times.
EtbImpl<EtbController2>::update():
✗ Branch 0 not taken.
✗ Branch 1 not taken.
|
206 | if (m_autocalPhase != ACPhase::Stopped) { | |
| 182 | ✗ | ACPhase nextPhase = doAutocal(m_autocalPhase); | ||
| 183 | ||||
| 184 | // if we changed phase, reset the phase timer | |||
| 185 | ✗ | if (m_autocalPhase != nextPhase) { | ||
| 186 | ✗ | m_autocalTimer.reset(); | ||
| 187 | ✗ | m_autocalPhase = nextPhase; | ||
| 188 | } | |||
| 189 | } else | |||
| 190 | #endif /* EFI_TUNER_STUDIO */ | |||
| 191 | ||||
| 192 | { | |||
| 193 | 206 | TBase::update(); | ||
| 194 | } | |||
| 195 | 206 | } | ||
| 196 | ||||
| 197 | ✗ | void autoCalibrateTps(bool reportToTs) override { | ||
| 198 | // Only auto calibrate throttles | |||
| 199 | ✗ | if (TBase::getFunction() == DC_Throttle1 || TBase::getFunction() == DC_Throttle2 || TBase::getFunction() == DC_Wastegate) { | ||
| 200 | ✗ | m_isAutocalTs = reportToTs; | ||
| 201 | ✗ | m_autocalPhase = ACPhase::Start; | ||
| 202 | } | |||
| 203 | ✗ | } | ||
| 204 | ||||
| 205 | ACPhase doAutocal(ACPhase phase) { | |||
| 206 | // Don't allow if engine is running! | |||
| 207 | if (Sensor::getOrZero(SensorType::Rpm) > 0) { | |||
| 208 | efiPrintf(" ****************** ERROR: Not while RPM ********************"); | |||
| 209 | return ACPhase::Stopped; | |||
| 210 | } | |||
| 211 | ||||
| 212 | auto motor = TBase::getMotor(); | |||
| 213 | if (!motor) { | |||
| 214 | efiPrintf(" ****************** ERROR: No DC motor ********************"); | |||
| 215 | return ACPhase::Stopped; | |||
| 216 | } | |||
| 217 | ||||
| 218 | TBase::etbErrorCode = (uint8_t)EtbStatus::AutoCalibrate; | |||
| 219 | ||||
| 220 | auto myFunction = TBase::getFunction(); | |||
| 221 | ||||
| 222 | switch (phase) { | |||
| 223 | case ACPhase::Start: | |||
| 224 | // Open the throttle | |||
| 225 | motor->set(0.5f); | |||
| 226 | motor->enable(); | |||
| 227 | return ACPhase::Open; | |||
| 228 | case ACPhase::Open: | |||
| 229 | if (m_autocalTimer.hasElapsedMs(1000)) { | |||
| 230 | // Capture open position | |||
| 231 | m_primaryMax = Sensor::getRaw(functionToTpsSensorPrimary(myFunction)); | |||
| 232 | m_secondaryMax = Sensor::getRaw(functionToTpsSensorSecondary(myFunction)); | |||
| 233 | ||||
| 234 | // Next: close the throttle | |||
| 235 | motor->set(-0.5f); | |||
| 236 | return ACPhase::Close; | |||
| 237 | } | |||
| 238 | break; | |||
| 239 | case ACPhase::Close: | |||
| 240 | if (m_autocalTimer.hasElapsedMs(1000)) { | |||
| 241 | // Capture closed position | |||
| 242 | m_primaryMin = Sensor::getRaw(functionToTpsSensorPrimary(myFunction)); | |||
| 243 | m_secondaryMin = Sensor::getRaw(functionToTpsSensorSecondary(myFunction)); | |||
| 244 | ||||
| 245 | // Disable the motor, we're done | |||
| 246 | motor->disable("autotune"); | |||
| 247 | ||||
| 248 | // Check that the calibrate actually moved the throttle | |||
| 249 | if (std::abs(m_primaryMax - m_primaryMin) < 0.5f) { | |||
| 250 | firmwareError(ObdCode::OBD_TPS_Configuration, "Auto calibrate failed, check your wiring!\r\nClosed voltage: %.1fv Open voltage: %.1fv", m_primaryMin, m_primaryMax); | |||
| 251 | return ACPhase::Stopped; | |||
| 252 | } | |||
| 253 | ||||
| 254 | if (!m_isAutocalTs) { | |||
| 255 | if (myFunction == DC_Throttle1) { | |||
| 256 | engineConfiguration->tpsMin = convertVoltageTo10bitADC(m_primaryMin); | |||
| 257 | engineConfiguration->tpsMax = convertVoltageTo10bitADC(m_primaryMax); | |||
| 258 | engineConfiguration->tps1SecondaryMin = convertVoltageTo10bitADC(m_secondaryMin); | |||
| 259 | engineConfiguration->tps1SecondaryMax = convertVoltageTo10bitADC(m_secondaryMax); | |||
| 260 | } else if (myFunction == DC_Throttle2) { | |||
| 261 | engineConfiguration->tps2Min = convertVoltageTo10bitADC(m_primaryMin); | |||
| 262 | engineConfiguration->tps2Max = convertVoltageTo10bitADC(m_primaryMax); | |||
| 263 | engineConfiguration->tps2SecondaryMin = convertVoltageTo10bitADC(m_secondaryMin); | |||
| 264 | engineConfiguration->tps2SecondaryMax = convertVoltageTo10bitADC(m_secondaryMax); | |||
| 265 | } else if (myFunction == DC_Wastegate) { | |||
| 266 | engineConfiguration->wastegatePositionClosedVoltage = m_primaryMin; | |||
| 267 | engineConfiguration->wastegatePositionOpenedVoltage = m_primaryMax; | |||
| 268 | } else { | |||
| 269 | /* TODO */ | |||
| 270 | } | |||
| 271 | return ACPhase::Stopped; | |||
| 272 | } | |||
| 273 | ||||
| 274 | // Next: start transmitting results | |||
| 275 | tsCalibrationSetData(functionToCalModePriMax(myFunction), m_primaryMax); | |||
| 276 | return ACPhase::TransmitPrimaryMax; | |||
| 277 | } | |||
| 278 | break; | |||
| 279 | case ACPhase::TransmitPrimaryMax: | |||
| 280 | if (tsCalibrationIsIdle()) { | |||
| 281 | tsCalibrationSetData(functionToCalModePriMin(myFunction), m_primaryMin); | |||
| 282 | return ACPhase::TransmitPrimaryMin; | |||
| 283 | } | |||
| 284 | break; | |||
| 285 | case ACPhase::TransmitPrimaryMin: | |||
| 286 | if (tsCalibrationIsIdle()) { | |||
| 287 | tsCalibrationSetData(functionToCalModeSecMax(myFunction), m_secondaryMax); | |||
| 288 | // No secondary sensor? | |||
| 289 | if (tsCalibrationIsIdle()) | |||
| 290 | return ACPhase::Stopped; | |||
| 291 | return ACPhase::TransmitSecondaryMax; | |||
| 292 | } | |||
| 293 | break; | |||
| 294 | case ACPhase::TransmitSecondaryMax: | |||
| 295 | if (tsCalibrationIsIdle()) { | |||
| 296 | tsCalibrationSetData(functionToCalModeSecMin(myFunction), m_secondaryMin); | |||
| 297 | return ACPhase::TransmitSecondaryMin; | |||
| 298 | } | |||
| 299 | break; | |||
| 300 | case ACPhase::TransmitSecondaryMin: | |||
| 301 | if (tsCalibrationIsIdle()) { | |||
| 302 | // Done! | |||
| 303 | return ACPhase::Stopped; | |||
| 304 | } | |||
| 305 | break; | |||
| 306 | case ACPhase::Stopped: break; | |||
| 307 | } | |||
| 308 | ||||
| 309 | // by default, stay in the same phase | |||
| 310 | return phase; | |||
| 311 | } | |||
| 312 | ||||
| 313 | private: | |||
| 314 | ACPhase m_autocalPhase = ACPhase::Stopped; | |||
| 315 | Timer m_autocalTimer; | |||
| 316 | // Report calibrated values to TS, if false - set directly to config | |||
| 317 | bool m_isAutocalTs; | |||
| 318 | ||||
| 319 | float m_primaryMax; | |||
| 320 | float m_secondaryMax; | |||
| 321 | float m_primaryMin; | |||
| 322 | float m_secondaryMin; | |||
| 323 | }; | |||
| 324 | ||||
| 325 | extern EtbImpl<EtbController1> etb1; | |||
| 326 | extern EtbImpl<EtbController2> etb2; | |||
| 327 | ||||
| 328 | static constexpr electronic_throttle_s const* etbData1_ptr = &etb1; | |||
| 329 | static constexpr electronic_throttle_s const* etbData2_ptr = &etb2; | |||
| 330 | ||||
| 331 | // To be used by LogFields, for LiveData fragments see non constexpr getLiveData() in electronic_throttle.cpp | |||
| 332 | template<typename T, size_t idx> | |||
| 333 | consteval electronic_throttle_s const* getLiveDataConstexpr() requires std::is_same_v<T, electronic_throttle_s> { | |||
| 334 | #if EFI_ELECTRONIC_THROTTLE_BODY | |||
| 335 | ||||
| 336 | static_assert(idx < ETB_COUNT); | |||
| 337 | ||||
| 338 | if constexpr (idx == 0) { | |||
| 339 | return etbData1_ptr; | |||
| 340 | } | |||
| 341 | ||||
| 342 | return etbData2_ptr; | |||
| 343 | #else | |||
| 344 | return nullptr; | |||
| 345 | #endif | |||
| 346 | } | |||
| 347 |