fome-fw/firmware/controllers/actuators/electronic_throttle.cpp

1032 lines
29 KiB
C++
Raw Permalink Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

/**
* @file electronic_throttle.cpp
* @brief Electronic Throttle driver
*
* @see test test_etb.cpp
*
*
* 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
*
* Relevant console commands:
*
* ETB_BENCH_ENGINE
* set engine_type 58
*
* enable verbose_etb
* disable verbose_etb
* etbinfo
* set mock_pedal_position X
*
*
* set debug_mode 17
* for PID outputs
*
* set_etb_duty X
*
* http://rusefi.com/forum/viewtopic.php?f=5&t=592
*
* @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"
#if HW_PROTEUS
#include "proteus_meta.h"
#endif // HW_PROTEUS
#ifndef ETB_MAX_COUNT
#define ETB_MAX_COUNT 2
#endif /* ETB_MAX_COUNT */
static pedal2tps_t pedal2tpsMap;
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_Wastegate: return SensorType::WastegatePosition;
default: return SensorType::Invalid;
}
}
static SensorType functionToTpsSensorPrimary(dc_function_e func) {
switch(func) {
case DC_Throttle1: return SensorType::Tps1Primary;
default: return SensorType::Tps2Primary;
}
}
static SensorType functionToTpsSensorSecondary(dc_function_e func) {
switch(func) {
case DC_Throttle1: return SensorType::Tps1Secondary;
default: return SensorType::Tps2Secondary;
}
}
#if EFI_TUNER_STUDIO
static TsCalMode functionToCalModePriMin(dc_function_e func) {
switch (func) {
case DC_Throttle1: return TsCalMode::Tps1Min;
default: return TsCalMode::Tps2Min;
}
}
static TsCalMode functionToCalModePriMax(dc_function_e func) {
switch (func) {
case DC_Throttle1: return TsCalMode::Tps1Max;
default: return TsCalMode::Tps2Max;
}
}
static TsCalMode functionToCalModeSecMin(dc_function_e func) {
switch (func) {
case DC_Throttle1: return TsCalMode::Tps1SecondaryMin;
default: return TsCalMode::Tps2SecondaryMin;
}
}
static TsCalMode functionToCalModeSecMax(dc_function_e func) {
switch (func) {
case DC_Throttle1: return TsCalMode::Tps1SecondaryMax;
default: return TsCalMode::Tps2SecondaryMax;
}
}
#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))
bool EtbController::init(dc_function_e function, DcMotor *motor, pid_s *pidParameters, const ValueProvider3D* pedalMap, bool hasPedal) {
if (function == DC_None) {
// if not configured, don't init.
etbErrorCode = (int8_t)TpsState::None;
return false;
}
m_function = function;
m_positionSensor = functionToPositionSensor(function);
// If we are a throttle, require redundant TPS sensor
if (isEtbMode()) {
// We don't need to init throttles, so nothing to do here.
if (!hasPedal) {
etbErrorCode = (int8_t)TpsState::None;
return false;
}
// If no sensor is configured for this throttle, skip initialization.
if (!Sensor::hasSensor(functionToTpsSensorPrimary(function))) {
etbErrorCode = (int8_t)TpsState::TpsError;
return false;
}
if (!Sensor::isRedundant(m_positionSensor)) {
firmwareError(
ObdCode::OBD_TPS_Configuration,
"Use of electronic throttle requires %s to be redundant.",
Sensor::getSensorName(m_positionSensor)
);
etbErrorCode = (int8_t)TpsState::Redundancy;
return false;
}
if (!Sensor::isRedundant(SensorType::AcceleratorPedal)) {
firmwareError(
ObdCode::OBD_TPS_Configuration,
"Use of electronic throttle requires accelerator pedal to be redundant."
);
etbErrorCode = (int8_t)TpsState::Redundancy;
return false;
}
}
m_motor = motor;
m_pid.initPidClass(pidParameters);
m_pedalMap = pedalMap;
reset();
return true;
}
void EtbController::reset() {
m_shouldResetPid = true;
etbTpsErrorCounter = 0;
etbPpsErrorCounter = 0;
}
void EtbController::onConfigurationChange(pid_s* previousConfiguration) {
if (m_motor && !m_pid.isSame(previousConfiguration)) {
m_shouldResetPid = true;
}
doInitElectronicThrottle();
}
expected<percent_t> EtbController::observePlant() const {
return Sensor::get(m_positionSensor);
}
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_Wastegate:
return getSetpointWastegate();
default:
return unexpected;
}
}
expected<percent_t> EtbController::getSetpointWastegate() const {
return clampF(0, m_wastegatePosition, 100);
}
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_pedalMap) {
return unexpected;
}
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.
float sanitizedPedal = clampF(0, pedalPosition.value_or(0), 100);
float rpm = Sensor::getOrZero(SensorType::Rpm);
float baseTarget = m_pedalMap->getValue(rpm, sanitizedPedal);
m_baseTarget = baseTarget;
percent_t etbIdlePosition = clampF(0, m_idlePosition, 100);
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
percent_t targetPosition = interpolateClamped(0, etbIdleAddition, 100, 100, baseTarget);
// Adjust up/down by Lua adjustment
targetPosition += getLuaAdjustment();
#if EFI_ANTILAG_SYSTEM
if (engine->antilagController.isAntilagCondition) {
targetPosition += engineConfiguration->ALSEtbAdd;
}
#endif /* EFI_ANTILAG_SYSTEM */
// Apply any adjustment that this throttle alone needs
// Clamped to +-10 to prevent anything too wild
float trim = clampF(-10, getThrottleTrim(rpm, targetPosition), 10);
m_trim = trim;
targetPosition += trim;
// Clamp before rev limiter to avoid ineffective rev limit due to crazy out of range position target
targetPosition = clampF(0, targetPosition, 100);
// 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
revLimitActive = std::abs(targetPosition - targetPositionBefore) > 0.1f;
} else {
revLimitActive = false;
}
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();
}
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 interpolate3d(
config->throttle2TrimTable,
config->throttle2TrimTpsBins, targetPosition,
config->throttle2TrimRpmBins, rpm
);
}
expected<percent_t> EtbController::getOpenLoop(percent_t target) {
// Don't apply open loop for wastegate/idle valve, only real ETB
float feedForward = 0;
if (m_function != DC_Wastegate) {
feedForward = interpolate2d(target, config->etbBiasBins, config->etbBiasValues);
}
m_feedForward = feedForward;
return feedForward;
}
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) {
efitick_t now = getTimeNowNt();
// Determine period
float tu = m_autotuneCycleStart.getElapsedSecondsAndReset(now);
// 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ömHägglund (relay) auto tuning
// https://warwick.ac.uk/fac/cross_fac/iatl/reinvention/archive/volume5issue2/hornsey
// Publish to TS state
#if EFI_TUNER_STUDIO
// 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;
// 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++;
// Multiplex 3 signals on to the {mode, value} format
engine->outputChannels.calibrationMode = (uint8_t)static_cast<TsCalMode>((uint8_t)TsCalMode::EtbKp + m_autotuneCurrentParam);
switch (m_autotuneCurrentParam) {
case 0:
engine->outputChannels.calibrationValue = kp;
break;
case 1:
engine->outputChannels.calibrationValue = ki;
break;
case 2:
engine->outputChannels.calibrationValue = 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
}
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();
m_shouldResetPid = false;
}
if (m_isAutotune) {
return getClosedLoopAutotune(target, observation);
} else {
checkJam(target, observation);
m_pid.iTermMin = engineConfiguration->etb_iTermMin;
m_pid.iTermMax = engineConfiguration->etb_iTermMax;
// Normal case - use PID to compute closed loop part
m_error = target - observation;
return m_pid.getOutput(target, observation, etbPeriodSeconds);
}
}
void EtbController::setOutput(expected<percent_t> outputValue) {
if (!m_motor) {
return;
}
bool limpAllowThrottle = getLimpManager()->allowElectronicThrottle() || engine->etbIgnoreJamProtection;
// If not ETB, or ETB is allowed, output is valid, and we aren't paused, output to motor.
if (!isEtbMode() ||
(limpAllowThrottle
&& outputValue
&& !engineConfiguration->pauseEtbControl)) {
m_motor->enable();
m_motor->set(ETB_PERCENT_TO_DUTY(outputValue.Value));
m_outputDuty = outputValue.Value;
} else {
// Otherwise disable the motor.
m_motor->disable("setOutput");
m_outputDuty = 0;
}
}
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 below. 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;
}
TpsState localReason = TpsState::None;
if (etbTpsErrorCounter > 50) {
// TODO: https://github.com/FOME-Tech/fome-fw/issues/169
// localReason = TpsState::IntermittentTps;
#if EFI_SHAFT_POSITION_INPUT
} else if (engineConfiguration->disableEtbWhenEngineStopped && !engine->triggerCentral.engineMovedRecently()) {
localReason = TpsState::EngineStopped;
#endif // EFI_SHAFT_POSITION_INPUT
} else if (etbPpsErrorCounter > 50) {
// TODO: https://github.com/FOME-Tech/fome-fw/issues/169
// localReason = TpsState::IntermittentPps;
} else if (engine->engineState.lua.luaDisableEtb) {
localReason = TpsState::Lua;
}
etbErrorCode = (int8_t)localReason;
return localReason == TpsState::None;
}
void EtbController::update() {
#if !EFI_UNIT_TEST
// If we didn't get initialized, fail fast
if (!m_motor) {
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();
}
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)) {
// ETB is jammed!
jamDetected = true;
getLimpManager()->reportEtbProblem();
}
} else {
m_jamDetectTimer.reset(nowNt);
jamDetected = false;
}
jamTimer = m_jamDetectTimer.getElapsedSeconds(nowNt);
}
}
#if EFI_UNIT_TEST
#define chThdSleepMilliseconds(x) {}
#endif // EFI_UNIT_TEST
#include <utility>
template <typename TBase>
class EtbImpl final : public TBase {
private:
enum class ACPhase {
Stopped,
Start,
// Drive the motor open
Open,
// Drive the motor closed
Close,
// Write learned values to TS
TransmitPrimaryMax,
TransmitPrimaryMin,
TransmitSecondaryMax,
TransmitSecondaryMin,
};
public:
template <typename... TArgs>
EtbImpl(TArgs&&... args) : TBase(std::forward<TArgs>(args)...) { }
void update() override {
#if EFI_TUNER_STUDIO
if (m_autocalPhase != ACPhase::Stopped) {
ACPhase nextPhase = doAutocal(m_autocalPhase);
// if we changed phase, reset the phase timer
if (m_autocalPhase != nextPhase) {
m_autocalTimer.reset();
m_autocalPhase = nextPhase;
}
} else
#endif /* EFI_TUNER_STUDIO */
{
TBase::update();
}
}
void autoCalibrateTps() override {
// Only auto calibrate throttles
if (TBase::getFunction() == DC_Throttle1 || TBase::getFunction() == DC_Throttle2) {
m_autocalPhase = ACPhase::Start;
}
}
ACPhase doAutocal(ACPhase phase) {
// Don't allow if engine is running!
if (Sensor::getOrZero(SensorType::Rpm) > 0) {
return ACPhase::Stopped;
}
auto motor = TBase::getMotor();
if (!motor) {
return ACPhase::Stopped;
}
auto myFunction = TBase::getFunction();
switch (phase) {
case ACPhase::Start:
// Open the throttle
motor->set(0.5f);
motor->enable();
return ACPhase::Open;
case ACPhase::Open:
if (m_autocalTimer.hasElapsedMs(1000)) {
// Capture open position
m_primaryMax = Sensor::getRaw(functionToTpsSensorPrimary(myFunction));
m_secondaryMax = Sensor::getRaw(functionToTpsSensorSecondary(myFunction));
// Next: close the throttle
motor->set(-0.5f);
return ACPhase::Close;
}
break;
case ACPhase::Close:
if (m_autocalTimer.hasElapsedMs(1000)) {
// Capture closed position
m_primaryMin = Sensor::getRaw(functionToTpsSensorPrimary(myFunction));
m_secondaryMin = Sensor::getRaw(functionToTpsSensorSecondary(myFunction));
// Disable the motor, we're done
motor->disable("autotune");
// Check that the calibrate actually moved the throttle
if (std::abs(m_primaryMax - m_primaryMin) < 0.5f) {
firmwareError(ObdCode::OBD_TPS_Configuration, "Auto calibrate failed, check your wiring!\r\nClosed voltage: %.1fv Open voltage: %.1fv", m_primaryMin, m_primaryMax);
return ACPhase::Stopped;
}
// Next: start transmitting results
engine->outputChannels.calibrationMode = (uint8_t)functionToCalModePriMax(myFunction);
engine->outputChannels.calibrationValue = convertVoltageTo10bitADC(m_primaryMax);
return ACPhase::TransmitPrimaryMax;
}
break;
case ACPhase::TransmitPrimaryMax:
if (m_autocalTimer.hasElapsedMs(500)) {
engine->outputChannels.calibrationMode = (uint8_t)functionToCalModePriMin(myFunction);
engine->outputChannels.calibrationValue = convertVoltageTo10bitADC(m_primaryMin);
return ACPhase::TransmitPrimaryMin;
}
break;
case ACPhase::TransmitPrimaryMin:
if (m_autocalTimer.hasElapsedMs(500)) {
engine->outputChannels.calibrationMode = (uint8_t)functionToCalModeSecMax(myFunction);
engine->outputChannels.calibrationValue = convertVoltageTo10bitADC(m_secondaryMax);
return ACPhase::TransmitSecondaryMax;
}
break;
case ACPhase::TransmitSecondaryMax:
if (m_autocalTimer.hasElapsedMs(500)) {
engine->outputChannels.calibrationMode = (uint8_t)functionToCalModeSecMin(myFunction);
engine->outputChannels.calibrationValue = convertVoltageTo10bitADC(m_secondaryMin);
return ACPhase::TransmitSecondaryMin;
}
break;
case ACPhase::TransmitSecondaryMin:
if (m_autocalTimer.hasElapsedMs(500)) {
// Done!
engine->outputChannels.calibrationMode = (uint8_t)TsCalMode::None;
return ACPhase::Stopped;
}
break;
case ACPhase::Stopped: break;
}
// by default, stay in the same phase
return phase;
}
private:
ACPhase m_autocalPhase = ACPhase::Stopped;
Timer m_autocalTimer;
float m_primaryMax;
float m_secondaryMax;
float m_primaryMin;
float m_secondaryMin;
};
// real implementation (we mock for some unit tests)
static EtbImpl<EtbController1> etb1;
static EtbImpl<EtbController2> etb2;
static_assert(ETB_COUNT == 2);
static EtbController* etbControllers[] = { &etb1, &etb2 };
void etbPidReset() {
for (int i = 0 ; i < ETB_COUNT; i++) {
if (auto controller = engine->etbControllers[i]) {
controller->reset();
}
}
}
void etbAutocal(size_t throttleIndex) {
if (throttleIndex >= ETB_COUNT) {
return;
}
if (auto etb = engine->etbControllers[throttleIndex]) {
etb->autoCalibrateTps();
}
}
/**
* 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 = 5;
setLinearCurve(config->pedalToTpsPedalBins, /*from*/0, /*to*/100, 1);
setLinearCurve(config->pedalToTpsRpmBins, /*from*/0, /*to*/8000, 1);
for (int pedalIndex = 0; pedalIndex < PEDAL_TO_TPS_SIZE; pedalIndex++) {
for (int rpmIndex = 0; rpmIndex < PEDAL_TO_TPS_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
-100, 100, // min/max
0 // alignment fill
};
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);
}
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;
}
}
void doInitElectronicThrottle() {
bool hasPedal = Sensor::hasSensor(SensorType::AcceleratorPedalPrimary);
#if EFI_UNIT_TEST
printf("doInitElectronicThrottle %s\n", boolToString(hasPedal));
#endif // EFI_UNIT_TEST
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(engineConfiguration->etbIo[i], i, engineConfiguration->etb_use_two_wires);
auto controller = engine->etbControllers[i];
if (!controller) {
continue;
}
auto pid = getPidForDcFunction(func);
bool dcConfigured = controller->init(func, motor, pid, &pedal2tpsMap, hasPedal);
anyEtbConfigured |= dcConfigured && controller->isEtbMode();
}
if (!anyEtbConfigured) {
// It's not valid to have a PPS without any ETBs - check that at least one ETB was enabled along with the pedal
if (hasPedal) {
firmwareError(ObdCode::OBD_PCM_Processor_Fault, "A pedal position sensor was configured, but no electronic throttles are configured.");
}
}
if (anyEtbConfigured) {
validateParam(engineConfiguration->etb.minValue < 0, "ETB PID min must be negative");
validateParam(engineConfiguration->etb.maxValue > 0, "ETB PID max must be positive");
}
#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 */
}
void initElectronicThrottle() {
if (hasFirmwareError()) {
return;
}
for (int i = 0; i < ETB_COUNT; i++) {
engine->etbControllers[i] = etbControllers[i];
}
pedal2tpsMap.init(config->pedalToTpsTable, config->pedalToTpsPedalBins, config->pedalToTpsRpmBins);
doInitElectronicThrottle();
}
void setEtbIdlePosition(percent_t pos) {
for (int i = 0; i < ETB_COUNT; i++) {
if (auto etb = engine->etbControllers[i]) {
etb->setIdlePosition(pos);
}
}
}
void setEtbWastegatePosition(percent_t pos) {
for (int i = 0; i < ETB_COUNT; i++) {
if (auto etb = engine->etbControllers[i]) {
etb->setWastegatePosition(pos);
}
}
}
void setEtbLuaAdjustment(percent_t pos) {
for (int i = 0; i < ETB_COUNT; i++) {
if (auto etb = engine->etbControllers[i]) {
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.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 */
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
}