diff --git a/firmware/controllers/actuators/electronic_throttle.cpp b/firmware/controllers/actuators/electronic_throttle.cpp index 7ae34557e9..47d5d19409 100644 --- a/firmware/controllers/actuators/electronic_throttle.cpp +++ b/firmware/controllers/actuators/electronic_throttle.cpp @@ -99,7 +99,11 @@ static bool startupPositionError = false; #define STARTUP_NEUTRAL_POSITION_ERROR_THRESHOLD 5 -static SensorType indexToTpsSensor(size_t index) { +static SensorType indexToTpsSensor(size_t index, bool volkswagenEtbIdle) { + if (volkswagenEtbIdle) { + return SensorType::Tps2; + } + switch(index) { case 0: return SensorType::Tps1; default: return SensorType::Tps2; @@ -127,7 +131,8 @@ static percent_t currentEtbDuty; // 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)) -void EtbController::init(DcMotor *motor, int ownIndex, pid_s *pidParameters, const ValueProvider3D* pedalMap) { +void EtbController::init(SensorType positionSensor, DcMotor *motor, int ownIndex, pid_s *pidParameters, const ValueProvider3D* pedalMap) { + m_positionSensor = positionSensor; m_motor = motor; m_myIndex = ownIndex; m_pid.initPidClass(pidParameters); @@ -149,7 +154,7 @@ void EtbController::showStatus(Logging* logger) { } expected EtbController::observePlant() const { - return Sensor::get(indexToTpsSensor(m_myIndex)); + return Sensor::get(m_positionSensor); } void EtbController::setIdlePosition(percent_t pos) { @@ -162,6 +167,12 @@ expected EtbController::getSetpoint() const { return unexpected; } + // 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 (CONFIG(volkswagenEtbIdle)) { + return m_idlePosition; + } + // If the pedal map hasn't been set, we can't provide a setpoint. if (!m_pedalMap) { return unexpected; @@ -743,7 +754,11 @@ void doInitElectronicThrottle(DECLARE_ENGINE_PARAMETER_SIGNATURE) { pedal2tpsMap.init(config->pedalToTpsTable, config->pedalToTpsPedalBins, config->pedalToTpsRpmBins); - engine->etbActualCount = Sensor::hasSensor(SensorType::Tps2) ? 2 : 1; + if (CONFIG(volkswagenEtbIdle)) { + engine->etbActualCount = 1; + } else { + engine->etbActualCount = Sensor::hasSensor(SensorType::Tps2) ? 2 : 1; + } for (int i = 0 ; i < engine->etbActualCount; i++) { auto motor = initDcMotor(i, CONFIG(etb_use_two_wires) PASS_ENGINE_PARAMETER_SUFFIX); @@ -751,7 +766,8 @@ void doInitElectronicThrottle(DECLARE_ENGINE_PARAMETER_SIGNATURE) { // If this motor is actually set up, init the etb if (motor) { - engine->etbControllers[i]->init(motor, i, &engineConfiguration->etb, &pedal2tpsMap); + auto positionSensor = indexToTpsSensor(i, CONFIG(volkswagenEtbIdle)); + engine->etbControllers[i]->init(positionSensor, motor, i, &engineConfiguration->etb, &pedal2tpsMap); INJECT_ENGINE_REFERENCE(engine->etbControllers[i]); } } diff --git a/firmware/controllers/actuators/electronic_throttle.h b/firmware/controllers/actuators/electronic_throttle.h index 5c120aac1e..e4b1ca666e 100644 --- a/firmware/controllers/actuators/electronic_throttle.h +++ b/firmware/controllers/actuators/electronic_throttle.h @@ -19,6 +19,7 @@ #include "engine.h" #include "closed_loop_controller.h" #include "expected.h" +#include "sensor.h" class DcMotor; class Logging; @@ -26,7 +27,7 @@ class Logging; class IEtbController : public ClosedLoopController { public: DECLARE_ENGINE_PTR; - virtual void init(DcMotor *motor, int ownIndex, pid_s *pidParameters, const ValueProvider3D* pedalMap) = 0; + virtual void init(SensorType positionSensor, DcMotor *motor, int ownIndex, pid_s *pidParameters, const ValueProvider3D* pedalMap) = 0; virtual void reset() = 0; virtual void setIdlePosition(percent_t pos) = 0; virtual void start() = 0; @@ -35,7 +36,7 @@ public: class EtbController : public IEtbController { public: - void init(DcMotor *motor, int ownIndex, pid_s *pidParameters, const ValueProvider3D* pedalMap) override; + void init(SensorType positionSensor, DcMotor *motor, int ownIndex, pid_s *pidParameters, const ValueProvider3D* pedalMap) override; void setIdlePosition(percent_t pos) override; void reset() override; void start() override {} @@ -74,6 +75,7 @@ protected: private: int m_myIndex = 0; + SensorType m_positionSensor = SensorType::Invalid; DcMotor *m_motor = nullptr; Pid m_pid; bool m_shouldResetPid = false;