From 579219e8cce47e3d6ffed58547dfeb8fa1faa0ff Mon Sep 17 00:00:00 2001 From: Matthew Kennedy Date: Fri, 22 Nov 2019 17:53:54 -0800 Subject: [PATCH] inject interfaces to ETB, renaming (#1026) * inject interfaces, renaming * format * null check * that was a dumb typo * fix indent --- .../actuators/electronic_throttle.cpp | 298 +++++++++--------- .../actuators/electronic_throttle.h | 10 +- 2 files changed, 157 insertions(+), 151 deletions(-) diff --git a/firmware/controllers/actuators/electronic_throttle.cpp b/firmware/controllers/actuators/electronic_throttle.cpp index a020ad34e0..a7c96316ef 100644 --- a/firmware/controllers/actuators/electronic_throttle.cpp +++ b/firmware/controllers/actuators/electronic_throttle.cpp @@ -105,7 +105,7 @@ static bool startupPositionError = false; #define STARTUP_NEUTRAL_POSITION_ERROR_THRESHOLD 5 -class EtbControl { +class EtbHardware { private: OutputPin m_pinEnable; OutputPin m_pinDir1; @@ -118,9 +118,7 @@ private: SimplePwm etbPwmUp; public: - DECLARE_ENGINE_PTR; - - EtbControl() : etbPwmUp("etbUp"), dcMotor(&m_pwmEnable, &m_pwmDir1, &m_pwmDir2) {} + EtbHardware() : etbPwmUp("etbUp"), dcMotor(&m_pwmEnable, &m_pwmDir1, &m_pwmDir2) {} TwoPinDcMotor dcMotor; @@ -135,7 +133,9 @@ public: // since we have pointer magic here we cannot simply have value parameter pin_output_mode_e *pinEnableMode, brain_pin_e pinDir1, - brain_pin_e pinDir2) { + brain_pin_e pinDir2, + ExecutorInterface* executor, + int frequency) { dcMotor.setType(useTwoWires ? TwoPinDcMotor::ControlType::PwmDirectionPins : TwoPinDcMotor::ControlType::PwmEnablePin); m_pinEnable.initPin("ETB Enable", pinEnable, pinEnableMode); @@ -143,29 +143,29 @@ public: m_pinDir2.initPin("ETB Dir 2", pinDir2); // Clamp to >100hz - int freq = maxI(100, engineConfiguration->etbFreq); + int clampedFrequency = maxI(100, frequency); // no need to complicate event queue with ETB PWM in unit tests #if ! EFI_UNIT_TEST startSimplePwm(&m_pwmEnable, "ETB Enable", - &engine->executor, + executor, &m_pinEnable, - freq, + clampedFrequency, 0, (pwm_gen_callback*)applyPinState); startSimplePwm(&m_pwmDir1, "ETB Dir 1", - &engine->executor, + executor, &m_pinDir1, - freq, + clampedFrequency, 0, (pwm_gen_callback*)applyPinState); startSimplePwm(&m_pwmDir2, "ETB Dir 2", - &engine->executor, + executor, &m_pinDir2, - freq, + clampedFrequency, 0, (pwm_gen_callback*)applyPinState); #endif /* EFI_UNIT_TEST */ @@ -174,7 +174,7 @@ public: #define ETB_COUNT 2 -static EtbControl etbControls[ETB_COUNT]; +static EtbHardware etbHardware[ETB_COUNT]; extern percent_t mockPedalPosition; @@ -187,152 +187,157 @@ static percent_t currentEtbDuty; // this macro clamps both positive and negative percentages from about -100% to 100% #define ETB_PERCENT_TO_DUTY(X) (maxF(minF((X * 0.01), ETB_DUTY_LIMIT - 0.01), 0.01 - ETB_DUTY_LIMIT)) - EtbController::EtbController(EtbControl *etb) { - this->etb = etb; - } +EtbController::EtbController(DcMotor *motor) +: m_motor(motor) +{ +} - int EtbController::getPeriodMs() { - return GET_PERIOD_LIMITED(&engineConfiguration->etb); +int EtbController::getPeriodMs() { + return GET_PERIOD_LIMITED(&engineConfiguration->etb); +} + +void EtbController::PeriodicTask() { + // set debug_mode 17 + if (engineConfiguration->debugMode == DBG_ELECTRONIC_THROTTLE_PID) { +#if EFI_TUNER_STUDIO + etbPid.postState(&tsOutputChannels); + tsOutputChannels.debugIntField5 = engine->engineState.etbFeedForward; +#endif /* EFI_TUNER_STUDIO */ + } else if (engineConfiguration->debugMode == DBG_ELECTRONIC_THROTTLE_EXTRA) { +#if EFI_TUNER_STUDIO + // set debug_mode 29 + tsOutputChannels.debugFloatField1 = directPwmValue; +#endif /* EFI_TUNER_STUDIO */ } - void EtbController::PeriodicTask() { - // set debug_mode 17 - if (engineConfiguration->debugMode == DBG_ELECTRONIC_THROTTLE_PID) { + if (!m_motor) { + return; + } + + if (startupPositionError) { + m_motor->set(0); + return; + } + + if (shouldResetPid) { + etbPid.reset(); + shouldResetPid = false; + } + + if (!cisnan(directPwmValue)) { + m_motor->set(directPwmValue); + return; + } + + if (boardConfiguration->pauseEtbControl) { + m_motor->set(0); + return; + } + + percent_t actualThrottlePosition = getTPS(PASS_ENGINE_PARAMETER_SIGNATURE); + + if (engine->etbAutoTune) { + autoTune.input = actualThrottlePosition; + bool result = autoTune.Runtime(&logger); + + tuneWorkingPid.updateFactors(autoTune.output, 0, 0); + + float value = tuneWorkingPid.getOutput(50, actualThrottlePosition); + scheduleMsg(&logger, "AT input=%f output=%f PID=%f", autoTune.input, + autoTune.output, + value); + scheduleMsg(&logger, "AT PID=%f", value); + m_motor->set(ETB_PERCENT_TO_DUTY(value)); + + if (result) { + scheduleMsg(&logger, "GREAT NEWS! %f/%f/%f", autoTune.GetKp(), autoTune.GetKi(), autoTune.GetKd()); + } + + return; + } + + + percent_t pedalPosition = getPedalPosition(PASS_ENGINE_PARAMETER_SIGNATURE); + + int rpm = GET_RPM(); + engine->engineState.targetFromTable = pedal2tpsMap.getValue(rpm / RPM_1_BYTE_PACKING_MULT, pedalPosition); + percent_t etbIdleAddition = CONFIGB(useETBforIdleControl) ? engine->engineState.idle.etbIdleAddition : 0; + percent_t targetPosition = engine->engineState.targetFromTable + etbIdleAddition; + + if (engineConfiguration->debugMode == DBG_ETB_LOGIC) { #if EFI_TUNER_STUDIO - etbPid.postState(&tsOutputChannels); - tsOutputChannels.debugIntField5 = engine->engineState.etbFeedForward; + tsOutputChannels.debugFloatField1 = engine->engineState.targetFromTable; + tsOutputChannels.debugFloatField2 = engine->engineState.idle.etbIdleAddition; #endif /* EFI_TUNER_STUDIO */ - } else if (engineConfiguration->debugMode == DBG_ELECTRONIC_THROTTLE_EXTRA) { -#if EFI_TUNER_STUDIO - // set debug_mode 29 - tsOutputChannels.debugFloatField1 = directPwmValue; -#endif /* EFI_TUNER_STUDIO */ - } + } - if (startupPositionError) { - etb->dcMotor.set(0); - return; - } + engine->engineState.etbFeedForward = interpolate2d("etbb", targetPosition, engineConfiguration->etbBiasBins, engineConfiguration->etbBiasValues); - if (shouldResetPid) { - etbPid.reset(); - shouldResetPid = false; - } + etbPid.iTermMin = engineConfiguration->etb_iTermMin; + etbPid.iTermMax = engineConfiguration->etb_iTermMax; - if (!cisnan(directPwmValue)) { - etb->dcMotor.set(directPwmValue); - return; - } + currentEtbDuty = engine->engineState.etbFeedForward + + etbPid.getOutput(targetPosition, actualThrottlePosition); - if (boardConfiguration->pauseEtbControl) { - etb->dcMotor.set(0); - return; - } + m_motor->set(ETB_PERCENT_TO_DUTY(currentEtbDuty)); - percent_t actualThrottlePosition = getTPS(PASS_ENGINE_PARAMETER_SIGNATURE); + if (engineConfiguration->isVerboseETB) { + etbPid.showPidStatus(&logger, "ETB"); + } - if (engine->etbAutoTune) { - autoTune.input = actualThrottlePosition; - bool result = autoTune.Runtime(&logger); - - tuneWorkingPid.updateFactors(autoTune.output, 0, 0); - - float value = tuneWorkingPid.getOutput(50, actualThrottlePosition); - scheduleMsg(&logger, "AT input=%f output=%f PID=%f", autoTune.input, - autoTune.output, - value); - scheduleMsg(&logger, "AT PID=%f", value); - etb->dcMotor.set(ETB_PERCENT_TO_DUTY(value)); - - if (result) { - scheduleMsg(&logger, "GREAT NEWS! %f/%f/%f", autoTune.GetKp(), autoTune.GetKi(), autoTune.GetKd()); - } - - return; - } - - - percent_t pedalPosition = getPedalPosition(PASS_ENGINE_PARAMETER_SIGNATURE); - - int rpm = GET_RPM(); - engine->engineState.targetFromTable = pedal2tpsMap.getValue(rpm / RPM_1_BYTE_PACKING_MULT, pedalPosition); - percent_t etbIdleAddition = CONFIGB(useETBforIdleControl) ? engine->engineState.idle.etbIdleAddition : 0; - percent_t targetPosition = engine->engineState.targetFromTable + etbIdleAddition; - - if (engineConfiguration->debugMode == DBG_ETB_LOGIC) { -#if EFI_TUNER_STUDIO - tsOutputChannels.debugFloatField1 = engine->engineState.targetFromTable; - tsOutputChannels.debugFloatField2 = engine->engineState.idle.etbIdleAddition; -#endif /* EFI_TUNER_STUDIO */ - } - - engine->engineState.etbFeedForward = interpolate2d("etbb", targetPosition, engineConfiguration->etbBiasBins, engineConfiguration->etbBiasValues); - - etbPid.iTermMin = engineConfiguration->etb_iTermMin; - etbPid.iTermMax = engineConfiguration->etb_iTermMax; - - currentEtbDuty = engine->engineState.etbFeedForward + - etbPid.getOutput(targetPosition, actualThrottlePosition); - - etb->dcMotor.set(ETB_PERCENT_TO_DUTY(currentEtbDuty)); - - if (engineConfiguration->isVerboseETB) { - etbPid.showPidStatus(&logger, "ETB"); - } - - DISPLAY_STATE(Engine) + DISPLAY_STATE(Engine) DISPLAY(DISPLAY_IF(hasEtbPedalPositionSensor)) - DISPLAY_TEXT(Electronic_Throttle); - DISPLAY_SENSOR(TPS) - DISPLAY_TEXT(eol); + DISPLAY_TEXT(Electronic_Throttle); + DISPLAY_SENSOR(TPS) + DISPLAY_TEXT(eol); - DISPLAY_TEXT(Pedal); - DISPLAY_SENSOR(PPS); - DISPLAY(DISPLAY_CONFIG(throttlePedalPositionAdcChannel)); - DISPLAY_TEXT(eol); + DISPLAY_TEXT(Pedal); + DISPLAY_SENSOR(PPS); + DISPLAY(DISPLAY_CONFIG(throttlePedalPositionAdcChannel)); + DISPLAY_TEXT(eol); - DISPLAY_TEXT(Feed_forward); - DISPLAY(DISPLAY_FIELD(etbFeedForward)); - DISPLAY_TEXT(eol); + DISPLAY_TEXT(Feed_forward); + DISPLAY(DISPLAY_FIELD(etbFeedForward)); + DISPLAY_TEXT(eol); - DISPLAY_STATE(ETB_pid) - DISPLAY_TEXT(input); - DISPLAY(DISPLAY_FIELD(input)); - DISPLAY_TEXT(Output); - DISPLAY(DISPLAY_FIELD(output)); - DISPLAY_TEXT(iTerm); - DISPLAY(DISPLAY_FIELD(iTerm)); - DISPLAY_TEXT(eol); - DISPLAY(DISPLAY_FIELD(errorAmplificationCoef)); - DISPLAY(DISPLAY_FIELD(previousError)); - DISPLAY_TEXT(eol); + DISPLAY_STATE(ETB_pid) + DISPLAY_TEXT(input); + DISPLAY(DISPLAY_FIELD(input)); + DISPLAY_TEXT(Output); + DISPLAY(DISPLAY_FIELD(output)); + DISPLAY_TEXT(iTerm); + DISPLAY(DISPLAY_FIELD(iTerm)); + DISPLAY_TEXT(eol); + DISPLAY(DISPLAY_FIELD(errorAmplificationCoef)); + DISPLAY(DISPLAY_FIELD(previousError)); + DISPLAY_TEXT(eol); - DISPLAY_TEXT(Settings); - DISPLAY(DISPLAY_CONFIG(ETB_PFACTOR)); - DISPLAY(DISPLAY_CONFIG(ETB_IFACTOR)); - DISPLAY(DISPLAY_CONFIG(ETB_DFACTOR)); - DISPLAY_TEXT(eol); - DISPLAY(DISPLAY_CONFIG(ETB_OFFSET)); - DISPLAY(DISPLAY_CONFIG(ETB_PERIODMS)); - DISPLAY_TEXT(eol); - DISPLAY(DISPLAY_CONFIG(ETB_MINVALUE)); - DISPLAY(DISPLAY_CONFIG(ETB_MAXVALUE)); + DISPLAY_TEXT(Settings); + DISPLAY(DISPLAY_CONFIG(ETB_PFACTOR)); + DISPLAY(DISPLAY_CONFIG(ETB_IFACTOR)); + DISPLAY(DISPLAY_CONFIG(ETB_DFACTOR)); + DISPLAY_TEXT(eol); + DISPLAY(DISPLAY_CONFIG(ETB_OFFSET)); + DISPLAY(DISPLAY_CONFIG(ETB_PERIODMS)); + DISPLAY_TEXT(eol); + DISPLAY(DISPLAY_CONFIG(ETB_MINVALUE)); + DISPLAY(DISPLAY_CONFIG(ETB_MAXVALUE)); /* DISPLAY_ELSE */ - DISPLAY_TEXT(No_Pedal_Sensor); + DISPLAY_TEXT(No_Pedal_Sensor); /* DISPLAY_ENDIF */ #if EFI_TUNER_STUDIO - // 312 - tsOutputChannels.etbTarget = targetPosition; - // 316 - tsOutputChannels.etb1DutyCycle = currentEtbDuty; - // 320 - // Error is positive if the throttle needs to open further - tsOutputChannels.etb1Error = targetPosition - actualThrottlePosition; + // 312 + tsOutputChannels.etbTarget = targetPosition; + // 316 + tsOutputChannels.etb1DutyCycle = currentEtbDuty; + // 320 + // Error is positive if the throttle needs to open further + tsOutputChannels.etb1Error = targetPosition - actualThrottlePosition; #endif /* EFI_TUNER_STUDIO */ - } +} -EtbController etbController(&etbControls[0]); +EtbController etbController(&etbHardware[0].dcMotor); /** * set_etb_duty X @@ -348,7 +353,7 @@ void setThrottleDutyCycle(percent_t level) { float dc = ETB_PERCENT_TO_DUTY(level); directPwmValue = dc; for (int i = 0 ; i < ETB_COUNT; i++) { - etbControls[i].dcMotor.set(dc); + etbHardware[i].dcMotor.set(dc); } scheduleMsg(&logger, "duty ETB duty=%f", dc); } @@ -377,7 +382,7 @@ static void showEthInfo(void) { scheduleMsg(&logger, "dir2=%s", hwPortname(CONFIGB(etb1.directionPin2))); for (int i = 0 ; i < ETB_COUNT; i++) { - EtbControl *etb = &etbControls[i]; + EtbHardware *etb = &etbHardware[i]; scheduleMsg(&logger, "%d: dir=%d DC=%f", i, etb->dcMotor.isOpenDirection(), etb->dcMotor.get()); } @@ -392,7 +397,7 @@ static void setEtbFrequency(int frequency) { engineConfiguration->etbFreq = frequency; for (int i = 0 ; i < ETB_COUNT; i++) { - etbControls[i].setFrequency(frequency); + etbHardware[i].setFrequency(frequency); } } @@ -400,7 +405,7 @@ static void etbReset() { scheduleMsg(&logger, "etbReset"); for (int i = 0 ; i < ETB_COUNT; i++) { - etbControls[i].dcMotor.set(0); + etbHardware[i].dcMotor.set(0); } etbPid.reset(); @@ -535,12 +540,14 @@ void onConfigurationChangeElectronicThrottleCallback(engine_configuration_s *pre void startETBPins(DECLARE_ENGINE_PARAMETER_SIGNATURE) { // controlPinMode is a strange feature - it's simply because I am short on 5v I/O on Frankenso with Miata NB2 test mule - etbControls[0].start( + etbHardware[0].start( CONFIG(etb1_use_two_wires), CONFIGB(etb1.controlPin1), &CONFIGB(etb1.controlPinMode), CONFIGB(etb1.directionPin1), - CONFIGB(etb1.directionPin2) + CONFIGB(etb1.directionPin2), + &ENGINE(executor), + CONFIG(etbFreq) ); } @@ -611,9 +618,6 @@ void initElectronicThrottle(DECLARE_ENGINE_PARAMETER_SIGNATURE) { etbPid.initPidClass(&engineConfiguration->etb); - for (int i = 0 ; i < ETB_COUNT; i++) { - INJECT_ENGINE_REFERENCE(etbControls[i]); - } INJECT_ENGINE_REFERENCE(etbController); pedal2tpsMap.init(config->pedalToTpsTable, config->pedalToTpsPedalBins, config->pedalToTpsRpmBins); @@ -646,7 +650,7 @@ void initElectronicThrottle(DECLARE_ENGINE_PARAMETER_SIGNATURE) { if (engineConfiguration->etbCalibrationOnStart) { for (int i = 0 ; i < ETB_COUNT; i++) { - EtbControl *etb = &etbControls[i]; + EtbHardware *etb = &etbHardware[i]; etb->dcMotor.set(70); chThdSleep(600); diff --git a/firmware/controllers/actuators/electronic_throttle.h b/firmware/controllers/actuators/electronic_throttle.h index ebf7dfad04..583b4df08a 100644 --- a/firmware/controllers/actuators/electronic_throttle.h +++ b/firmware/controllers/actuators/electronic_throttle.h @@ -14,16 +14,18 @@ #include "engine.h" #include "periodic_task.h" -class EtbControl; +class DcMotor; -class EtbController : public PeriodicTimerController { +class EtbController final : public PeriodicTimerController { public: DECLARE_ENGINE_PTR; - EtbController(EtbControl *etb); - EtbControl *etb = nullptr; + EtbController(DcMotor *etb); int getPeriodMs() override; void PeriodicTask() override; + +private: + DcMotor *m_motor; }; void initElectronicThrottle(DECLARE_ENGINE_PARAMETER_SIGNATURE);