electronic_throttle.cpp

Go to the documentation of this file.

/**
 * @file    electronic_throttle.cpp
 * @brief   Electronic Throttle driver
 *
 * @see test test_etb.cpp
 *
 * PPS=pedal position sensor=AcceleratorPedal
 * TPS=throttle position sensor, this one is inside ETB=electronic throttle body
 *
 * Limited user documentation at https://github.com/rusefi/rusefi/wiki/HOWTO_electronic_throttle_body
 *
 *
 *  ETB is controlled according to pedal position input (pedal position sensor is a potentiometer)
 *    pedal 0% means pedal not pressed / idle
 *    pedal 100% means pedal all the way down
 *  (not TPS - not the one you can calibrate in TunerStudio)
 *
 *
 * See also pid.cpp
 *
 * @date Dec 7, 2013
 * @author Andrey Belomutskiy, (c) 2012-2020
 *
 * This file is part of rusEfi - see http://rusefi.com
 *
 * rusEfi is free software; you can redistribute it and/or modify it under the terms of
 * the GNU General Public License as published by the Free Software Foundation; either
 * version 3 of the License, or (at your option) any later version.
 *
 * rusEfi is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without
 * even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along with this program.
 * If not, see <http://www.gnu.org/licenses/>.
 */
 
#include "pch.h"
 
#include "electronic_throttle_impl.h"
 
#if EFI_ELECTRONIC_THROTTLE_BODY
 
#include "dc_motor.h"
#include "dc_motors.h"
#include "defaults.h"
#include "tunerstudio.h"
#include "tunerstudio_calibration_channel.h"
#include "transition_events.h"
 
#if defined(HAS_OS_ACCESS)
#error "Unexpected OS ACCESS HERE"
#endif
 
#if HW_PROTEUS
#include "proteus_meta.h"
#endif // HW_PROTEUS
 
#ifndef ETB_MAX_COUNT
#define ETB_MAX_COUNT 2
#endif /* ETB_MAX_COUNT */
 
#ifndef ETB_INTERMITTENT_LIMIT
#define ETB_INTERMITTENT_LIMIT 50
#endif
 
static pedal2tps_t pedal2tpsMap{"p2t"};
static Map3D<ETB2_TRIM_SIZE, ETB2_TRIM_SIZE, int8_t, uint8_t, uint8_t> throttle2TrimTable{"t2t"};
static Map3D<TRACTION_CONTROL_ETB_DROP_SLIP_SIZE, TRACTION_CONTROL_ETB_DROP_SPEED_SIZE, int8_t, uint16_t, uint8_t> tcEtbDropTable{"tce"};
 
constexpr float etbPeriodSeconds = 1.0f / ETB_LOOP_FREQUENCY;
 
//static bool startupPositionError = false;
 
//#define STARTUP_NEUTRAL_POSITION_ERROR_THRESHOLD 5
 
static const float hardCodedetbHitachiBiasBins[8] = {0.0, 19.0, 21.0, 22.0, 23.0, 25.0, 30.0, 100.0};
 
static const float hardCodedetbHitachiBiasValues[8] = {-18.0, -17.0, -15.0, 0.0, 16.0, 20.0, 20.0, 20.0};
 
/* Generated by TS2C on Thu Aug 20 21:10:02 EDT 2020*/
void setHitachiEtbBiasBins() {
    copyArray(config->etbBiasBins, hardCodedetbHitachiBiasBins);
    copyArray(config->etbBiasValues, hardCodedetbHitachiBiasValues);
}
 
static SensorType functionToPositionSensor(dc_function_e func) {
    switch(func) {
        case DC_Throttle1: return SensorType::Tps1;
        case DC_Throttle2: return SensorType::Tps2;
        case DC_IdleValve: return SensorType::IdlePosition;
        case DC_Wastegate: return SensorType::WastegatePosition;
        default: return SensorType::Invalid;
    }
}
 
static SensorType functionToTpsSensor(dc_function_e func) {
    switch(func) {
        case DC_Throttle1: return SensorType::Tps1;
        case DC_Throttle2: return SensorType::Tps2;
        case DC_IdleValve: return SensorType::IdlePosition;
        case DC_Wastegate: return SensorType::WastegatePosition;
        default: return SensorType::Invalid;
    }
}
 
static SensorType functionToTpsSensorPrimary(dc_function_e func) {
    switch(func) {
        case DC_Throttle1: return SensorType::Tps1Primary;
        case DC_Throttle2: return SensorType::Tps2Primary;
        case DC_IdleValve: return SensorType::IdlePosition;
        case DC_Wastegate: return SensorType::WastegatePosition;
        default: return SensorType::Invalid;
    }
}
 
static SensorType functionToTpsSensorSecondary(dc_function_e func) {
    switch(func) {
        case DC_Throttle1: return SensorType::Tps1Secondary;
        case DC_Throttle2: return SensorType::Tps2Secondary;
        /* No secondary sensors for Idle and EWG */
        default: return SensorType::Invalid;
    }
}
 
#if EFI_TUNER_STUDIO
static TsCalMode functionToCalModePriMin(dc_function_e func) {
    switch (func) {
        case DC_Throttle1: return TsCalMode::Tps1Min;
        case DC_Throttle2: return TsCalMode::Tps2Min;
        case DC_Wastegate: return TsCalMode::EwgPosMin;
        default: return TsCalMode::None;
    }
}
 
static TsCalMode functionToCalModePriMax(dc_function_e func) {
    switch (func) {
        case DC_Throttle1: return TsCalMode::Tps1Max;
        case DC_Throttle2: return TsCalMode::Tps2Max;
        case DC_Wastegate: return TsCalMode::EwgPosMax;
        default: return TsCalMode::None;
    }
}
 
static TsCalMode functionToCalModeSecMin(dc_function_e func) {
    switch (func) {
        case DC_Throttle1: return TsCalMode::Tps1SecondaryMin;
        case DC_Throttle2: return TsCalMode::Tps2SecondaryMin;
        default: return TsCalMode::None;
    }
}
 
static TsCalMode functionToCalModeSecMax(dc_function_e func) {
    switch (func) {
        case DC_Throttle1: return TsCalMode::Tps1SecondaryMax;
        case DC_Throttle2: return TsCalMode::Tps2SecondaryMax;
        default: return TsCalMode::None;
    }
}
#endif // EFI_TUNER_STUDIO
 
#define ETB_DUTY_LIMIT 0.9
// this macro clamps both positive and negative percentages from about -100% to 100%
#define ETB_PERCENT_TO_DUTY(x) (clampF(-ETB_DUTY_LIMIT, 0.01f * (x), ETB_DUTY_LIMIT))
 
PUBLIC_API_WEAK bool isBoardAllowingLackOfPps() {
  return false;
}
 
bool EtbController::init(dc_function_e function, DcMotor *motor, pid_s *pidParameters, const ValueProvider3D* pedalProvider) {
    state = (uint8_t)EtbState::InInit;
    if (function == DC_None) {
        // if not configured, don't init.
        state = (uint8_t)EtbState::NotEbt;
        etbErrorCode = (int8_t)EtbStatus::NotConfigured;
        return false;
    }
 
    m_function = function;
    m_positionSensor = functionToPositionSensor(function);
 
    // If we are a throttle, require redundant TPS sensor
    if (isEtbMode()) {
        // If no sensor is configured for this throttle, skip initialization.
        if (!Sensor::hasSensor(functionToTpsSensor(function))) {
            etbErrorCode = (int8_t)EtbStatus::TpsError;
            return false;
        }
 
        if (!isBoardAllowingLackOfPps() && !Sensor::isRedundant(m_positionSensor)) {
            etbErrorCode = (int8_t)EtbStatus::Redundancy;
            return false;
        }
    }
 
    m_motor = motor;
    m_pedalProvider = pedalProvider;
 
    m_pid.initPidClass(pidParameters);
 
#if !EFI_UNIT_TEST
    if (isEtbMode()) {
        m_pid.iTermMin = engineConfiguration->etb_iTermMin;
        m_pid.iTermMax = engineConfiguration->etb_iTermMax;
    } else {
        // Some defaults from setDefaultEtbParameters(), find better values for EWG and Idle or add config options
        m_pid.iTermMin = -30;
        m_pid.iTermMax = 30;
    }
#endif
 
    // Ignore 3% position error before complaining
    m_targetErrorAccumulator.init(3.0f, etbPeriodSeconds);
 
    state = (uint8_t)EtbState::SuccessfulInit;
    return true;
}
 
#if EFI_UNIT_TEST
int ebtResetCounter;
#endif // EFI_UNIT_TEST
 
void EtbController::reset(const char *reason) {
    efiPrintf("ETB reset %s", reason);
    m_shouldResetPid = true;
    etbTpsErrorCounter = 0;
    etbPpsErrorCounter = 0;
#if EFI_UNIT_TEST
    ebtResetCounter++;
#endif // EFI_UNIT_TEST
 
}
 
// todo: document why is EtbController not engine_module?
void EtbController::onConfigurationChange(pid_s* previousConfiguration) {
    if (m_motor && !m_pid.isSame(previousConfiguration)) {
      efiPrintf(" ETB m_shouldResetPid");
        m_shouldResetPid = true;
    }
 
    doInitElectronicThrottle(/*isStartupInit*/false);
}
 
void EtbController::showStatus() {
    m_pid.showPidStatus("ETB");
}
 
expected<percent_t> EtbController::observePlant() {
    expected<percent_t> plant = Sensor::get(m_positionSensor);
    validPlantPosition = plant.Valid;
    return plant;
}
 
void EtbController::setIdlePosition(percent_t pos) {
    m_idlePosition = pos;
}
 
void EtbController::setWastegatePosition(percent_t pos) {
    m_wastegatePosition = pos;
}
 
expected<percent_t> EtbController::getSetpoint() {
    switch (m_function) {
        case DC_Throttle1:
        case DC_Throttle2:
            return getSetpointEtb();
        case DC_IdleValve:
            return getSetpointIdleValve();
        case DC_Wastegate:
            return getSetpointWastegate();
        default:
            return unexpected;
    }
}
 
expected<percent_t> EtbController::getSetpointIdleValve() const {
    // VW ETB idle mode uses an ETB only for idle (a mini-ETB sets the lower stop, and a normal cable
    // can pull the throttle up off the stop.), so we directly control the throttle with the idle position.
#if EFI_TUNER_STUDIO && (EFI_PROD_CODE || EFI_SIMULATOR)
    // todo: where do we want to log this? engine->outputChannels.etbTarget = m_idlePosition;
#endif // EFI_TUNER_STUDIO
    return clampPercentValue(m_idlePosition);
}
 
expected<percent_t> EtbController::getSetpointWastegate() const {
    percent_t targetPosition = m_wastegatePosition + getLuaAdjustment();
 
    return clampPercentValue(targetPosition);
}
 
float getSanitizedPedal() {
    auto pedalPosition = Sensor::get(SensorType::AcceleratorPedal);
    // If the pedal has failed, just use 0 position.
    // This is safer than disabling throttle control - we can at least push the throttle closed
    // and let the engine idle.
    return clampPercentValue(pedalPosition.value_or(0));
}
 
PUBLIC_API_WEAK float boardAdjustEtbTarget(float currentEtbTarget) {
  return currentEtbTarget;
}
 
expected<percent_t> EtbController::getSetpointEtb() {
    // Autotune runs with 50% target position
    if (m_isAutotune) {
        return 50.0f;
    }
 
//  // A few extra preconditions if throttle control is invalid
//  if (startupPositionError) {
//      return unexpected;
//  }
 
    // If the pedal map hasn't been set, we can't provide a setpoint.
    if (!m_pedalProvider) {
    state = (uint8_t)EtbState::NoPedal;
        return unexpected;
    }
 
    float sanitizedPedal = getSanitizedPedal();
 
    float rpm = Sensor::getOrZero(SensorType::Rpm);
  percent_t preBoard = m_pedalProvider->getValue(rpm, sanitizedPedal);
    etbCurrentTarget = boardAdjustEtbTarget(preBoard);
    boardEtbAdjustment = etbCurrentTarget - preBoard;
 
    percent_t etbIdlePosition = clampPercentValue(m_idlePosition);
    percent_t etbIdleAddition = PERCENT_DIV * engineConfiguration->etbIdleThrottleRange * etbIdlePosition;
 
    // Interpolate so that the idle adder just "compresses" the throttle's range upward.
    // [0, 100] -> [idle, 100]
    // 0% target from table -> idle position as target
    // 100% target from table -> 100% target position
    targetWithIdlePosition = interpolateClamped(0, etbIdleAddition, 100, 100, etbCurrentTarget);
 
    percent_t targetPosition = targetWithIdlePosition + getLuaAdjustment();
    // just an additional logging data point
    adjustedEtbTarget = targetPosition;
 
#if EFI_ANTILAG_SYSTEM
    if (engine->antilagController.isAntilagCondition) {
        targetPosition += engineConfiguration->ALSEtbAdd;
    }
#endif /* EFI_ANTILAG_SYSTEM */
 
  float vehicleSpeed = Sensor::getOrZero(SensorType::VehicleSpeed);
  float wheelSlip = Sensor::getOrZero(SensorType::WheelSlipRatio);
  tcEtbDrop = tcEtbDropTable.getValue(wheelSlip, vehicleSpeed);
 
    // Apply any adjustment that this throttle alone needs
    // Clamped to +-10 to prevent anything too wild
    trim = clampF(-10, getThrottleTrim(rpm, targetPosition), 10);
    targetPosition += trim + tcEtbDrop;
 
    // Clamp before rev limiter to avoid ineffective rev limit due to crazy out of range position target
    targetPosition = clampPercentValue(targetPosition);
 
    // Lastly, apply ETB rev limiter
    auto etbRpmLimit = engineConfiguration->etbRevLimitStart;
    if (etbRpmLimit != 0) {
        auto fullyLimitedRpm = etbRpmLimit + engineConfiguration->etbRevLimitRange;
 
        float targetPositionBefore = targetPosition;
        // Linearly taper throttle to closed from the limit across the range
        targetPosition = interpolateClamped(etbRpmLimit, targetPosition, fullyLimitedRpm, 0, rpm);
 
        // rev limit active if the position was changed by rev limiter
        etbRevLimitActive = std::abs(targetPosition - targetPositionBefore) > 0.1f;
    }
 
    float minPosition = engineConfiguration->etbMinimumPosition;
 
    // Keep the throttle just barely off the lower stop, and less than the user-configured maximum
    float maxPosition = engineConfiguration->etbMaximumPosition;
    // Don't allow max position over 100
    maxPosition = std::min(maxPosition, 100.0f);
 
    targetPosition = clampF(minPosition, targetPosition, maxPosition);
    m_adjustedTarget = targetPosition;
 
    return targetPosition;
}
 
void EtbController::setLuaAdjustment(float adjustment) {
    luaAdjustment = adjustment;
    m_luaAdjustmentTimer.reset();
}
 
/**
 * positive adjustment opens TPS, negative closes TPS
 */
float EtbController::getLuaAdjustment() const {
    // If the lua position hasn't been set in 0.2 second, don't adjust!
    // This avoids a stuck throttle due to hung/rogue/etc Lua script
    if (m_luaAdjustmentTimer.getElapsedSeconds() > 0.2f) {
        return 0;
    } else {
        return luaAdjustment;
    }
}
 
percent_t EtbController2::getThrottleTrim(float rpm, percent_t targetPosition) const {
    return m_throttle2Trim.getValue(rpm, targetPosition);
}
 
expected<percent_t> EtbController::getOpenLoop(percent_t target) {
    // Don't apply open loop for idle valve, only real ETB or wastegate
    switch(m_function){
        case DC_Throttle1:
        case DC_Throttle2: {
            etbFeedForward = interpolate2d(target, config->etbBiasBins, config->etbBiasValues);
            break;
        }
        case DC_Wastegate: {
            etbFeedForward = interpolate2d(target, config->dcWastegateBiasBins, config->dcWastegateBiasValues);
            break;
        }
        case DC_IdleValve: {
            etbFeedForward = 0;
            break;
        }
        default: { // or DC_None
            etbFeedForward = 0;
        }
    }
 
    return etbFeedForward;
}
 
expected<percent_t> EtbController::getClosedLoopAutotune(percent_t target, percent_t actualThrottlePosition) {
    // Estimate gain at current position - this should be well away from the spring and in the linear region
    // GetSetpoint sets this to 50%
    bool isPositive = actualThrottlePosition > target;
 
    float autotuneAmplitude = 20;
 
    // End of cycle - record & reset
    if (!isPositive && m_lastIsPositive) {
        // Determine period
        float tu = m_cycleTimer.getElapsedSecondsAndReset(getTimeNowNt());
 
        // Determine amplitude
        float a = m_maxCycleTps - m_minCycleTps;
 
        // Filter - it's pretty noisy since the ultimate period is not very many loop periods
        constexpr float alpha = 0.05;
        m_a  = alpha * a  + (1 - alpha) * m_a;
        m_tu = alpha * tu + (1 - alpha) * m_tu;
 
        // Reset bounds
        m_minCycleTps = 100;
        m_maxCycleTps = 0;
 
        // Math is for Åström–Hägglund (relay) auto tuning
        // https://warwick.ac.uk/fac/cross_fac/iatl/reinvention/archive/volume5issue2/hornsey
 
        // Amplitude of input (duty cycle %)
        float b = 2 * autotuneAmplitude;
 
        // Ultimate gain per A-H relay tuning rule
        float ku = 4 * b / (CONST_PI * m_a);
 
        // The multipliers below are somewhere near the "no overshoot"
        // and "some overshoot" flavors of the Ziegler-Nichols method
        // Kp
        float kp = 0.35f * ku;
        float ki = 0.25f * ku / m_tu;
        float kd = 0.08f * ku * m_tu;
 
        // Publish to TS state
#if EFI_TUNER_STUDIO
        // Every 5 cycles (of the throttle), cycle to the next value
        if (m_autotuneCounter >= 5) {
            m_autotuneCounter = 0;
            m_autotuneCurrentParam = (m_autotuneCurrentParam + 1) % 3; // three ETB calibs: P-I-D
        }
 
        m_autotuneCounter++;
 
        switch (m_autotuneCurrentParam) {
        case 0:
            tsCalibrationSetData(TsCalMode::EtbKp, kp);
            break;
        case 1:
            tsCalibrationSetData(TsCalMode::EtbKi, ki);
            break;
        case 2:
            tsCalibrationSetData(TsCalMode::EtbKd, kd);
            break;
        }
 
        // Also output to debug channels if configured
        if (engineConfiguration->debugMode == DBG_ETB_AUTOTUNE) {
            // a - amplitude of output (TPS %)
            engine->outputChannels.debugFloatField1 = m_a;
            // b - amplitude of input (Duty cycle %)
            engine->outputChannels.debugFloatField2 = b;
            // Tu - oscillation period (seconds)
            engine->outputChannels.debugFloatField3 = m_tu;
 
            engine->outputChannels.debugFloatField4 = ku;
            engine->outputChannels.debugFloatField5 = kp;
            engine->outputChannels.debugFloatField6 = ki;
            engine->outputChannels.debugFloatField7 = kd;
        }
#endif
        // TODO: directly update PID settings in engineConfiguration
    }
 
    m_lastIsPositive = isPositive;
 
    // Find the min/max of each cycle
    if (actualThrottlePosition < m_minCycleTps) {
        m_minCycleTps = actualThrottlePosition;
    }
 
    if (actualThrottlePosition > m_maxCycleTps) {
        m_maxCycleTps = actualThrottlePosition;
    }
 
    // Bang-bang control the output to induce oscillation
    return autotuneAmplitude * (isPositive ? -1 : 1);
}
 
expected<percent_t> EtbController::getClosedLoop(percent_t target, percent_t observation) {
    if (m_shouldResetPid) {
        m_pid.reset();
        onTransitionEvent(TransitionEvent::EtbPidReset);
        m_shouldResetPid = false;
    }
 
    if (m_isAutotune) {
        state = (uint8_t)EtbState::Autotune;
 
        m_targetErrorAccumulator.reset();
 
        return getClosedLoopAutotune(target, observation);
    } else {
        checkJam(target, observation);
 
        integralError = m_targetErrorAccumulator.accumulate(target - observation);
 
        float dt = m_cycleTimer.getElapsedSecondsAndReset(getTimeNowNt());
        m_lastPidDtMs = dt * 1000.0;
 
        // Normal case - use PID to compute closed loop part
        return m_pid.getOutput(target, observation, dt);
    }
}
 
void EtbController::setOutput(expected<percent_t> outputValue) {
#if EFI_TUNER_STUDIO
    // Only report first-throttle stats
    if (m_function == DC_Throttle1) {
        engine->outputChannels.etb1DutyCycle = outputValue.value_or(0);
    }
#endif
 
    if (!m_motor) {
        state = (uint8_t)EtbState::NoMotor;
        return;
    }
 
    bool isEnabled;
    if (!isEtbMode()) {
      // technical debt: non-ETB usages of DC motor are still mixed into ETB controller?
        state = (uint8_t)EtbState::NotEbt;
        isEnabled = true;
    } else if (!getLimpManager()->allowElectronicThrottle()) {
        state = (uint8_t)EtbState::LimpProhibited;
        isEnabled = false;
    } else if (engineConfiguration->pauseEtbControl) {
        state = (uint8_t)EtbState::Paused;
        isEnabled = false;
    } else if (!outputValue) {
        state = (uint8_t)EtbState::NoOutput;
        isEnabled = false;
    } else {
        state = (uint8_t)EtbState::Active;
        isEnabled = true;
    }
 
    // If not ETB, or ETB is allowed, output is valid, and we aren't paused, output to motor.
    if (isEnabled) {
        m_motor->enable();
        m_motor->set(ETB_PERCENT_TO_DUTY(outputValue.Value));
    } else {
        // Otherwise disable the motor.
        m_motor->disable("no-ETB");
    }
}
 
bool EtbController::checkStatus() {
#if EFI_TUNER_STUDIO
    // Only debug throttle #1
    if (m_function == DC_Throttle1) {
        m_pid.postState(engine->outputChannels.etbStatus);
    } else if (m_function == DC_Wastegate) {
        m_pid.postState(engine->outputChannels.wastegateDcStatus);
    }
#endif /* EFI_TUNER_STUDIO */
 
    if (!isEtbMode()) {
        // no validation for h-bridge or idle mode
        return true;
    }
    // ETB-specific code belo. The whole mix-up between DC and ETB is shameful :(
 
    // Only allow autotune with stopped engine, and on the first throttle
    // Update local state about autotune
    m_isAutotune = Sensor::getOrZero(SensorType::Rpm) == 0
        && engine->etbAutoTune
        && m_function == DC_Throttle1;
 
    bool shouldCheckSensorFunction = engine->module<SensorChecker>()->analogSensorsShouldWork();
 
    if (!m_isAutotune && shouldCheckSensorFunction) {
        bool isTpsError = !Sensor::get(m_positionSensor).Valid;
 
        // If we have an error that's new, increment the counter
        if (isTpsError && !hadTpsError) {
            etbTpsErrorCounter++;
        }
 
        hadTpsError = isTpsError;
 
        bool isPpsError = !Sensor::get(SensorType::AcceleratorPedal).Valid;
 
        // If we have an error that's new, increment the counter
        if (isPpsError && !hadPpsError) {
            etbPpsErrorCounter++;
        }
 
        hadPpsError = isPpsError;
    } else {
        // Either sensors are expected to not work, or autotune is running, so reset the error counter
        etbTpsErrorCounter = 0;
        etbPpsErrorCounter = 0;
    }
 
    EtbStatus localReason = EtbStatus::None;
    if (etbTpsErrorCounter > ETB_INTERMITTENT_LIMIT) {
        localReason = EtbStatus::IntermittentTps;
#if EFI_SHAFT_POSITION_INPUT
    } else if (engineConfiguration->disableEtbWhenEngineStopped
      && !engine->triggerCentral.engineMovedRecently()
      && !engine->etbAutoTune) {
        localReason = EtbStatus::EngineStopped;
#endif // EFI_SHAFT_POSITION_INPUT
    } else if (etbPpsErrorCounter > ETB_INTERMITTENT_LIMIT) {
        localReason = EtbStatus::IntermittentPps;
    } else if (engine->engineState.lua.luaDisableEtb) {
        localReason = EtbStatus::Lua;
    } else if (!getLimpManager()->allowElectronicThrottle()) {
        localReason = EtbStatus::JamDetected;
    } else if(!isBoardAllowingLackOfPps() && !Sensor::isRedundant(SensorType::AcceleratorPedal)) {
        localReason = EtbStatus::Redundancy;
    }
 
    etbErrorCode = (int8_t)localReason;
 
    return localReason == EtbStatus::None;
}
 
void EtbController::update() {
#if !EFI_UNIT_TEST
    // If we didn't get initialized, fail fast
    if (!m_motor) {
        state = (uint8_t)EtbState::FailFast;
        return;
    }
#endif // EFI_UNIT_TEST
 
    bool isOk = checkStatus();
 
    if (!isOk) {
        // If engine is stopped and so configured, skip the ETB update entirely
        // This is quieter and pulls less power than leaving it on all the time
        m_motor->disable("etb status");
        return;
    }
 
    ClosedLoopController::update();
 
    if (isEtbMode() && !validPlantPosition) {
        etbErrorCode = (int8_t)EtbStatus::TpsError;
    }
}
 
void EtbController::checkJam(percent_t setpoint, percent_t observation) {
    float absError = std::abs(setpoint - observation);
 
    auto jamDetectThreshold = engineConfiguration->etbJamDetectThreshold;
    auto jamTimeout = engineConfiguration->etbJamTimeout;
 
    if (jamDetectThreshold != 0 && jamTimeout != 0) {
        auto nowNt = getTimeNowNt();
 
        if (absError > jamDetectThreshold && engine->module<IgnitionController>()->getIgnState()) {
            if (m_jamDetectTimer.hasElapsedSec(jamTimeout)) {
                efiPrintf(" ************* ETB is jammed! ***************");
                jamDetected = true;
 
                getLimpManager()->reportEtbProblem();
            }
        } else {
            m_jamDetectTimer.reset(nowNt);
            jamDetected = false;
        }
 
        jamTimer = m_jamDetectTimer.getElapsedSeconds(nowNt);
    }
}
 
#if !EFI_UNIT_TEST
/**
 * Things running on a timer (instead of a thread) don't participate it the RTOS's thread priority system,
 * and operate essentially "first come first serve", which risks starvation.
 * Since ETB is a safety critical device, we need the hard RTOS guarantee that it will be scheduled over other less important tasks.
 */
#include "periodic_thread_controller.h"
#endif // EFI_UNIT_TEST
 
#include <utility>
 
// real implementation (we mock for some unit tests)
EtbImpl<EtbController1> etb1;
EtbImpl<EtbController2> etb2(throttle2TrimTable);
 
static EtbController* etbControllers[] = { &etb1, &etb2 };
static_assert(ETB_COUNT == sizeof(etbControllers) / sizeof(EtbController*));
 
void blinkEtbErrorCodes(bool blinkPhase) {
    for (int i = 0;i<ETB_COUNT;i++) {
        int8_t etbErrorCode = etbControllers[i]->etbErrorCode;
        if (etbErrorCode && engine->etbAutoTune) {
            etbErrorCode = (int8_t)EtbStatus::AutoTune;
        }
        etbControllers[i]->etbErrorCodeBlinker = blinkPhase ? 0 : etbErrorCode;
    }
}
 
#if !EFI_UNIT_TEST
 
struct DcThread final : public PeriodicController<512> {
    DcThread() : PeriodicController("DC", PRIO_ETB, ETB_LOOP_FREQUENCY) {}
 
    void PeriodicTask(efitick_t) override {
        // Simply update all controllers
        for (int i = 0 ; i < ETB_COUNT; i++) {
            auto controller = engine->etbControllers[i];
            assertNotNullVoid(controller);
            etbControllers[i]->update();
        }
    }
};
 
static DcThread dcThread CCM_OPTIONAL;
 
#endif // !EFI_UNIT_TEST
 
#if EFI_UNIT_TEST
void etbPidReset() {
    for (int i = 0 ; i < ETB_COUNT; i++) {
        if (auto controller = engine->etbControllers[i]) {
            assertNotNullVoid(controller);
            controller->reset("unit_test");
        }
    }
    ebtResetCounter = 0;
}
#endif // EFI_UNIT_TEST
 
void etbAutocal(dc_function_e function, bool reportToTs) {
    for (size_t i = 0 ; i < ETB_COUNT; i++) {
        /* TODO: use from engine, add getFunction() to base class */
        //if (auto controller = engine->etbControllers[i]) {
        if (auto controller = etbControllers[i]) {
            assertNotNullVoid(controller);
            if (controller->getFunction() == function) {
                /* TODO: is it possible that we have several controllers with same function? */
                controller->autoCalibrateTps(reportToTs);
                // todo fix root cause! work-around: make sure not to write bad tune since that would brick requestBurn();
            }
        }
    }
}
 
EtbStatus etbGetState(size_t throttleIndex)
{
    if (throttleIndex >= ETB_COUNT) {
        return EtbStatus::NotConfigured;
    }
 
    return (EtbStatus)etbControllers[throttleIndex]->etbErrorCode;
}
 
/**
 * This specific throttle has default position of about 7% open
 */
static const float boschBiasBins[] = {
    0, 1, 5, 7, 14, 65, 66, 100
};
static const float boschBiasValues[] = {
    -15, -15, -10, 0, 19, 20, 26, 28
};
 
void setBoschVAGETB() {
    engineConfiguration->tpsMin = 890; // convert 12to10 bit (ADC/4)
    engineConfiguration->tpsMax = 70; // convert 12to10 bit (ADC/4)
 
    engineConfiguration->tps1SecondaryMin = 102;
    engineConfiguration->tps1SecondaryMax = 891;
 
    engineConfiguration->etb.pFactor = 5.12;
    engineConfiguration->etb.iFactor = 47;
    engineConfiguration->etb.dFactor = 0.088;
    engineConfiguration->etb.offset = 0;
}
 
void setBoschVNH2SP30Curve() {
    copyArray(config->etbBiasBins, boschBiasBins);
    copyArray(config->etbBiasValues, boschBiasValues);
}
 
void setDefaultEtbParameters() {
    engineConfiguration->etbIdleThrottleRange = 15;
 
    setLinearCurve(config->pedalToTpsPedalBins, /*from*/0, /*to*/100, 1);
    setRpmTableBin(config->pedalToTpsRpmBins);
 
    for (int pedalIndex = 0;pedalIndex<PEDAL_TO_TPS_SIZE;pedalIndex++) {
        for (int rpmIndex = 0;rpmIndex<PEDAL_TO_TPS_RPM_SIZE;rpmIndex++) {
            config->pedalToTpsTable[pedalIndex][rpmIndex] = config->pedalToTpsPedalBins[pedalIndex];
        }
    }
 
    // Default is to run each throttle off its respective hbridge
    engineConfiguration->etbFunctions[0] = DC_Throttle1;
    engineConfiguration->etbFunctions[1] = DC_Throttle2;
 
    engineConfiguration->etbFreq = DEFAULT_ETB_PWM_FREQUENCY;
 
    // voltage, not ADC like with TPS
    setPPSCalibration(0, 5, 5, 0);
 
    engineConfiguration->etb = {
        1,      // Kp
        10,     // Ki
        0.05,   // Kd
        0,      // offset
        0,      // Update rate, unused
        -100, 100 // min/max
    };
 
    engineConfiguration->etb_iTermMin = -30;
    engineConfiguration->etb_iTermMax = 30;
 
    engineConfiguration->etbJamDetectThreshold = 10;
//  engineConfiguration->etbJamTimeout = 1;
}
 
void onConfigurationChangeElectronicThrottleCallback(engine_configuration_s *previousConfiguration) {
    for (int i = 0; i < ETB_COUNT; i++) {
        etbControllers[i]->onConfigurationChange(&previousConfiguration->etb);
    }
}
 
static const float defaultBiasBins[] = {
    0, 1, 2, 4, 7, 98, 99, 100
};
static const float defaultBiasValues[] = {
    -20, -18, -17, 0, 20, 21, 22, 25
};
 
void setDefaultEtbBiasCurve() {
    copyArray(config->etbBiasBins, defaultBiasBins);
    copyArray(config->etbBiasValues, defaultBiasValues);
    copyArray(config->dcWastegateBiasBins, defaultBiasBins);
    copyArray(config->dcWastegateBiasValues, defaultBiasValues);
}
 
void unregisterEtbPins() {
    // todo: we probably need an implementation here?!
}
 
static pid_s* getPidForDcFunction(dc_function_e function) {
    switch (function) {
        case DC_Wastegate: return &engineConfiguration->etbWastegatePid;
        default: return &engineConfiguration->etb;
    }
}
 
PUBLIC_API_WEAK ValueProvider3D* pedal2TpsProvider() {
  return &pedal2tpsMap;
}
 
void doInitElectronicThrottle(bool isStartupInit) {
    bool anyEtbConfigured = false;
 
    // todo: technical debt: we still have DC motor code initialization in ETB-specific file while DC motors are used not just as ETB
    // like DC motor wastegate code flow should probably NOT go through electronic_throttle.cpp right?
    // todo: rename etbFunctions to something-without-etb for same reason?
    for (int i = 0 ; i < ETB_COUNT; i++) {
        auto func = engineConfiguration->etbFunctions[i];
        if (func == DC_None) {
            // do not touch HW pins if function not selected, this way Lua can use DC motor hardware pins directly
            continue;
        }
        auto motor = initDcMotor("ETB disable",
                engineConfiguration->etbIo[i], i, engineConfiguration->etb_use_two_wires);
 
        auto controller = engine->etbControllers[i];
        criticalAssertVoid(controller != nullptr, "null ETB");
 
        auto pid = getPidForDcFunction(func);
 
        bool dcConfigured = controller->init(func, motor, pid, pedal2TpsProvider());
        if (isStartupInit && dcConfigured) {
            controller->reset("init");
        }
        anyEtbConfigured |= dcConfigured && controller->isEtbMode();
    }
 
    // It's not valid to have a PPS without any ETBs - check that at least one ETB was enabled along with the pedal
    if (!anyEtbConfigured && Sensor::hasSensor(SensorType::AcceleratorPedalPrimary)) {
        criticalError("A pedal position sensor was configured, but no electronic throttles are configured.");
    }
 
#if 0 && ! EFI_UNIT_TEST
    percent_t startupThrottlePosition = getTPS();
    if (std::abs(startupThrottlePosition - engineConfiguration->etbNeutralPosition) > STARTUP_NEUTRAL_POSITION_ERROR_THRESHOLD) {
        /**
         * Unexpected electronic throttle start-up position is worth a critical error
         */
        firmwareError(ObdCode::OBD_Throttle_Actuator_Control_Range_Performance_Bank_1, "startup ETB position %.2f not %d",
                startupThrottlePosition,
                engineConfiguration->etbNeutralPosition);
        startupPositionError = true;
    }
#endif /* EFI_UNIT_TEST */
 
#if !EFI_UNIT_TEST
    static bool started = false;
    if (started == false) {
        dcThread.start();
        started = true;
    }
#endif
}
 
void initElectronicThrottle() {
    if (hasFirmwareError()) {
        return;
    }
 
    for (int i = 0; i < ETB_COUNT; i++) {
        engine->etbControllers[i] = etbControllers[i];
    }
 
#if EFI_PROD_CODE
    addConsoleAction("etbautocal", [](){
        efiPrintf("etbAutocal invoked");
        etbAutocal(DC_Throttle1);
    });
 
    addConsoleAction("etbinfo", [](){
        efiPrintf("etbAutoTune=%d", engine->etbAutoTune);
        efiPrintf("TPS=%.2f", Sensor::getOrZero(SensorType::Tps1));
 
        efiPrintf("ETB1 duty=%.2f",
            (float)engine->outputChannels.etb1DutyCycle);
 
        efiPrintf("ETB freq=%d",
            engineConfiguration->etbFreq);
 
        for (int i = 0; i < ETB_COUNT; i++) {
            efiPrintf("ETB%d", i);
            efiPrintf(" dir1=%s", hwPortname(engineConfiguration->etbIo[i].directionPin1));
            efiPrintf(" dir2=%s", hwPortname(engineConfiguration->etbIo[i].directionPin2));
            efiPrintf(" control=%s", hwPortname(engineConfiguration->etbIo[i].controlPin));
            efiPrintf(" disable=%s", hwPortname(engineConfiguration->etbIo[i].disablePin));
            showDcMotorInfo(i);
        }
    });
 
#endif /* EFI_PROD_CODE */
 
    pedal2tpsMap.initTable(config->pedalToTpsTable, config->pedalToTpsRpmBins, config->pedalToTpsPedalBins);
    throttle2TrimTable.initTable(config->throttle2TrimTable, config->throttle2TrimRpmBins, config->throttle2TrimTpsBins);
    tcEtbDropTable.initTable(engineConfiguration->tractionControlEtbDrop, engineConfiguration->tractionControlSlipBins, engineConfiguration->tractionControlSpeedBins);
 
    doInitElectronicThrottle(/*isStartupInit*/true);
}
 
void setEtbIdlePosition(percent_t pos) {
    for (int i = 0; i < ETB_COUNT; i++) {
        if (auto etb = engine->etbControllers[i]) {
            assertNotNullVoid(etb);
            etb->setIdlePosition(pos);
        }
    }
}
 
void setEtbWastegatePosition(percent_t pos) {
    for (int i = 0; i < ETB_COUNT; i++) {
        if (auto etb = engine->etbControllers[i]) {
            assertNotNullVoid(etb);
            etb->setWastegatePosition(pos);
        }
    }
}
 
void setEtbLuaAdjustment(percent_t pos) {
    for (int i = 0; i < ETB_COUNT; i++) {
        /* TODO: use from engine, add getFunction() to base class */
        //if (auto etb = engine->etbControllers[i]) {
        if (auto etb = etbControllers[i]) {
            assertNotNullVoid(etb);
            // try to adjust all ETB
            if (etb->getFunction() == DC_Throttle1 || etb->getFunction() == DC_Throttle2) {
                etb->setLuaAdjustment(pos);
            }
        }
    }
}
 
void setEwgLuaAdjustment(percent_t pos) {
    for (int i = 0; i < ETB_COUNT; i++) {
        /* TODO: use from engine, add getFunction() to base class */
        //if (auto etb = engine->etbControllers[i]) {
        if (auto etb = etbControllers[i]) {
            assertNotNullVoid(etb);
            // try to adjust all ETB
            if (etb->getFunction() == DC_Wastegate) {
                etb->setLuaAdjustment(pos);
            }
        }
    }
}
 
void setToyota89281_33010_pedal_position_sensor() {
    setPPSCalibration(0, 4.1, 0.73, 4.9);
}
 
void setHitachiEtbCalibration() {
    setToyota89281_33010_pedal_position_sensor();
 
    setHitachiEtbBiasBins();
 
    engineConfiguration->etb.pFactor = 2.7999;
    engineConfiguration->etb.iFactor = 25.5;
    engineConfiguration->etb.dFactor = 0.053;
    engineConfiguration->etb.offset = 0.0;
    engineConfiguration->etb.periodMs = 5.0;
    engineConfiguration->etb.minValue = -100.0;
    engineConfiguration->etb.maxValue = 100.0;
 
    // Nissan 60mm throttle
    engineConfiguration->tpsMin = engineConfiguration->tps2Min = 113;
    engineConfiguration->tpsMax = engineConfiguration->tps2Max = 846;
    engineConfiguration->tps1SecondaryMin = engineConfiguration->tps2SecondaryMin = 897;
    engineConfiguration->tps1SecondaryMax = engineConfiguration->tps2SecondaryMax = 161;
}
 
void setProteusHitachiEtbDefaults() {
#if HW_PROTEUS
    setHitachiEtbCalibration();
 
    // EFI_ADC_12: "Analog Volt 3"
    engineConfiguration->tps1_2AdcChannel = PROTEUS_IN_TPS1_2;
    // EFI_ADC_13: "Analog Volt 4"
    engineConfiguration->tps2_1AdcChannel = PROTEUS_IN_TPS2_1;
    // EFI_ADC_0: "Analog Volt 5"
    engineConfiguration->tps2_2AdcChannel = PROTEUS_IN_ANALOG_VOLT_5;
    setPPSInputs(PROTEUS_IN_ANALOG_VOLT_6, PROTEUS_IN_PPS2);
#endif // HW_PROTEUS
}
 
#endif /* EFI_ELECTRONIC_THROTTLE_BODY */
 
// So far used by FragmentEntry (LiveData)
template<>
const electronic_throttle_s* getLiveData(size_t idx) {
#if EFI_ELECTRONIC_THROTTLE_BODY
    if (idx >= efi::size(etbControllers)) {
        return nullptr;
    }
 
    return etbControllers[idx];
#else
    return nullptr;
#endif
}