diff --git a/firmware/controllers/actuators/electronic_throttle.cpp b/firmware/controllers/actuators/electronic_throttle.cpp index 596fb8f3fd..13863cf7ff 100644 --- a/firmware/controllers/actuators/electronic_throttle.cpp +++ b/firmware/controllers/actuators/electronic_throttle.cpp @@ -98,6 +98,8 @@ static pedal2tps_t pedal2tpsMap("Pedal2Tps"); EXTERN_ENGINE; +constexpr float etbPeriodSeconds = 1.0f / ETB_LOOP_FREQUENCY; + static bool startupPositionError = false; #define STARTUP_NEUTRAL_POSITION_ERROR_THRESHOLD 5 @@ -205,6 +207,9 @@ bool EtbController::init(etb_function_e function, DcMotor *motor, pid_s *pidPara m_pid.initPidClass(pidParameters); m_pedalMap = pedalMap; + // Ignore 3% position error before complaining + m_errorAccumulator.init(3.0f, etbPeriodSeconds); + reset(); return true; @@ -444,8 +449,23 @@ expected EtbController::getClosedLoop(percent_t target, percent_t obs && m_function == ETB_Throttle1) { return getClosedLoopAutotune(observation); } else { + // Check that we're not over the error limit + float errorIntegral = m_errorAccumulator.accumulate(target - observation); + +#if EFI_TUNER_STUDIO + if (m_function == ETB_Throttle1 && CONFIG(debugMode) == DBG_ETB_LOGIC) { + tsOutputChannels.debugFloatField3 = errorIntegral; + } +#endif // EFI_TUNER_STUDIO + + // Allow up to 10 percent-seconds of error + if (errorIntegral > 10.0f) { + // TODO: figure out how to handle uncalibrated ETB + //ENGINE(limpManager).etbProblem(); + } + // Normal case - use PID to compute closed loop part - return m_pid.getOutput(target, observation, 1.0f / ETB_LOOP_FREQUENCY); + return m_pid.getOutput(target, observation, etbPeriodSeconds); } } diff --git a/firmware/controllers/actuators/electronic_throttle_impl.h b/firmware/controllers/actuators/electronic_throttle_impl.h index bed39a21e8..8d2da53f45 100644 --- a/firmware/controllers/actuators/electronic_throttle_impl.h +++ b/firmware/controllers/actuators/electronic_throttle_impl.h @@ -12,6 +12,7 @@ #include "sensor.h" #include "pid.h" +#include "error_accumulator.h" /** * Hard code ETB update speed. @@ -74,6 +75,7 @@ private: DcMotor *m_motor = nullptr; Pid m_pid; bool m_shouldResetPid = false; + ErrorAccumulator m_errorAccumulator; // Pedal -> target map const ValueProvider3D* m_pedalMap = nullptr;