From 07c964af7893069232ddbad283f03ff19167a46b Mon Sep 17 00:00:00 2001 From: rusefi Date: Sun, 10 Mar 2019 12:58:27 -0400 Subject: [PATCH] better Electronic Throttle body control #493 --- firmware/controllers/electronic_throttle.cpp | 81 ++++++++++++-------- firmware/controllers/electronic_throttle.h | 1 + firmware/controllers/system/efiGpio.cpp | 15 +++- firmware/controllers/system/efiGpio.h | 2 - 4 files changed, 59 insertions(+), 40 deletions(-) diff --git a/firmware/controllers/electronic_throttle.cpp b/firmware/controllers/electronic_throttle.cpp index 8580dbfd0e..4746ca9c18 100644 --- a/firmware/controllers/electronic_throttle.cpp +++ b/firmware/controllers/electronic_throttle.cpp @@ -74,6 +74,8 @@ #include "engine_controller.h" #include "PeriodicController.h" +#define ETB_MAX_COUNT 2 + #if EFI_ELECTRONIC_THROTTLE_BODY || defined(__DOXYGEN__) #include "pin_repository.h" #include "pwm_generator.h" @@ -90,20 +92,43 @@ static PID_AutoTune autoTune; static LoggingWithStorage logger("ETB"); -/** - * @brief Pulse-Width Modulation state - */ -/*CCM_OPTIONAL*/ static SimplePwm etbPwmUp("etbUp"); +EXTERN_ENGINE; + +class EtbControl { +public: + EtbControl() : etbPwmUp("etbUp"), dcMotor(&etbPwmUp, &outputDirectionOpen, &outputDirectionClose) {} + OutputPin outputDirectionOpen; + OutputPin outputDirectionClose; + OutputPin etbOutput; + SimplePwm etbPwmUp; + TwoPinDcMotor dcMotor; + void start(brain_pin_e controlPin, + brain_pin_e directionPin1, + brain_pin_e directionPin2) { + int freq = maxI(100, engineConfiguration->etbFreq); + // this line used for PWM + startSimplePwmExt(&etbPwmUp, "etb1", + &engine->executor, + controlPin, + &etbOutput, + freq, + 0.80, + applyPinState); + outputDirectionOpen.initPin("etb dir open", directionPin1); + outputDirectionClose.initPin("etb dir close", directionPin2); + } + + +}; + +static EtbControl etb1; + static float valueOverride = NAN; /* CCM_OPTIONAL static SimplePwm etbPwmDown("etbDown"); */ -/*CCM_OPTIONAL*/ static OutputPin outputDirectionOpen; -/*CCM_OPTIONAL*/ static OutputPin outputDirectionClose; -static TwoPinDcMotor dcMotor(&etbPwmUp, &outputDirectionOpen, &outputDirectionClose); -EXTERN_ENGINE; extern percent_t mockPedalPosition; static Pid pid(&engineConfiguration->etb); @@ -150,7 +175,7 @@ private: } if (!cisnan(valueOverride)) { - dcMotor.Set(valueOverride); + etb1.dcMotor.Set(valueOverride); return; } @@ -167,7 +192,7 @@ private: autoTune.output, value); scheduleMsg(&logger, "AT PID=%f", value); - dcMotor.Set(PERCENT_TO_DUTY(value)); + etb1.dcMotor.Set(PERCENT_TO_DUTY(value)); if (result) { scheduleMsg(&logger, "GREAT NEWS! %f/%f/%f", autoTune.GetKp(), autoTune.GetKi(), autoTune.GetKd()); @@ -195,7 +220,7 @@ private: currentEtbDuty = feedForward + pid.getValue(targetPosition, actualThrottlePosition); - dcMotor.Set(PERCENT_TO_DUTY(currentEtbDuty)); + etb1.dcMotor.Set(PERCENT_TO_DUTY(currentEtbDuty)); /* if (CONFIGB(etbDirectionPin2) != GPIO_UNASSIGNED) { bool needEtbBraking = absF(targetPosition - actualThrottlePosition) < 3; @@ -227,7 +252,7 @@ static void setThrottleDutyCycle(float level) { float dc = PERCENT_TO_DUTY(level); valueOverride = dc; - dcMotor.Set(dc); + etb1.dcMotor.Set(dc); scheduleMsg(&logger, "duty ETB duty=%f", dc); } @@ -244,7 +269,7 @@ static void showEthInfo(void) { getPinNameByAdcChannel("tPedal", engineConfiguration->throttlePedalPositionAdcChannel, pinNameBuffer)); scheduleMsg(&logger, "TPS=%.2f", getTPS()); - scheduleMsg(&logger, "dir=%d DC=%f", dcMotor.isOpenDirection(), dcMotor.Get()); + scheduleMsg(&logger, "dir=%d DC=%f", etb1.dcMotor.isOpenDirection(), etb1.dcMotor.Get()); scheduleMsg(&logger, "etbControlPin1=%s duty=%.2f freq=%d", hwPortname(CONFIGB(etb1.controlPin1)), @@ -269,7 +294,7 @@ static void etbReset() { scheduleMsg(&logger, "etbReset"); for (int i = 0;i < 5;i++) { // this is some crazy code to remind H-bridge that we are alive - dcMotor.BrakeGnd(); + etb1.dcMotor.BrakeGnd(); chThdSleepMilliseconds(10); } mockPedalPosition = MOCK_UNDEFINED; @@ -345,26 +370,10 @@ void onConfigurationChangeElectronicThrottleCallback(engine_configuration_s *pre void startETBPins(void) { - int freq = maxI(100, engineConfiguration->etbFreq); - - // this line used for PWM - startSimplePwmExt(&etbPwmUp, "etb1", - &engine->executor, - CONFIGB(etb1.controlPin1), - &enginePins.etbOutput1, - freq, - 0.80, - applyPinState); -/* - startSimplePwmExt(&etbPwmDown, "etb2", - CONFIGB(etbControlPin2), - &enginePins.etbOutput2, - freq, - 0.80, - applyPinState); -*/ - outputDirectionOpen.initPin("etb dir open", CONFIGB(etb1.directionPin1)); - outputDirectionClose.initPin("etb dir close", CONFIGB(etb1.directionPin2)); + etb1.start(CONFIGB(etb1.controlPin1), + CONFIGB(etb1.directionPin1), + CONFIGB(etb1.directionPin2) + ); } static void setTempOutput(float value) { @@ -418,6 +427,10 @@ void setDefaultEtbBiasCurve(DECLARE_ENGINE_PARAMETER_SIGNATURE) { engineConfiguration->etbBiasValues[7] = 25; } +void unregisterEtbPins() { + +} + void initElectronicThrottle(void) { addConsoleAction("ethinfo", showEthInfo); addConsoleAction("etbreset", etbReset); diff --git a/firmware/controllers/electronic_throttle.h b/firmware/controllers/electronic_throttle.h index b1b18bc201..064b7516af 100644 --- a/firmware/controllers/electronic_throttle.h +++ b/firmware/controllers/electronic_throttle.h @@ -20,5 +20,6 @@ bool isETBRestartNeeded(void); void stopETBPins(void); void startETBPins(void); void onConfigurationChangeElectronicThrottleCallback(engine_configuration_s *previousConfiguration); +void unregisterEtbPins(); #endif /* ELECTRONIC_THROTTLE_H_ */ diff --git a/firmware/controllers/system/efiGpio.cpp b/firmware/controllers/system/efiGpio.cpp index 5c89573955..2f06d2f0c5 100644 --- a/firmware/controllers/system/efiGpio.cpp +++ b/firmware/controllers/system/efiGpio.cpp @@ -15,6 +15,10 @@ #include "io_pins.h" #endif /* EFI_GPIO_HARDWARE */ +#if EFI_ELECTRONIC_THROTTLE_BODY +#include "electronic_throttle.h" +#endif /* EFI_ELECTRONIC_THROTTLE_BODY */ + EXTERN_ENGINE; #if EFI_ENGINE_SNIFFER || defined(__DOXYGEN__) @@ -88,6 +92,9 @@ bool EnginePins::stopPins() { } void EnginePins::unregisterPins() { +#if EFI_ELECTRONIC_THROTTLE_BODY + unregisterEtbPins(); +#endif /* EFI_ELECTRONIC_THROTTLE_BODY */ #if EFI_PROD_CODE || defined(__DOXYGEN__) fuelPumpRelay.unregisterOutput(activeConfiguration.bc.fuelPumpPin, engineConfiguration->bc.fuelPumpPin); fanRelay.unregisterOutput(activeConfiguration.bc.fanPin, engineConfiguration->bc.fanPin); @@ -97,10 +104,10 @@ void EnginePins::unregisterPins() { sdCsPin.unregisterOutput(activeConfiguration.bc.sdCardCsPin, engineConfiguration->bc.sdCardCsPin); accelerometerCs.unregisterOutput(activeConfiguration.LIS302DLCsPin, engineConfiguration->LIS302DLCsPin); - etbOutput1.unregisterOutput(activeConfiguration.bc.etb1.directionPin1, - engineConfiguration->bc.etb1.directionPin1); - etbOutput2.unregisterOutput(activeConfiguration.bc.etb1.directionPin2, - engineConfiguration->bc.etb1.directionPin2); +// etbOutput1.unregisterOutput(activeConfiguration.bc.etb1.directionPin1, +// engineConfiguration->bc.etb1.directionPin1); +// etbOutput2.unregisterOutput(activeConfiguration.bc.etb1.directionPin2, +// engineConfiguration->bc.etb1.directionPin2); checkEnginePin.unregisterOutput(activeConfiguration.bc.malfunctionIndicatorPin, engineConfiguration->bc.malfunctionIndicatorPin); dizzyOutput.unregisterOutput(activeConfiguration.dizzySparkOutputPin, diff --git a/firmware/controllers/system/efiGpio.h b/firmware/controllers/system/efiGpio.h index 165cd2b5d2..a22a7d8b43 100644 --- a/firmware/controllers/system/efiGpio.h +++ b/firmware/controllers/system/efiGpio.h @@ -132,8 +132,6 @@ public: NamedOutputPin tachOut; NamedOutputPin dizzyOutput; - OutputPin etbOutput1; - OutputPin etbOutput2; OutputPin fsioOutputs[FSIO_COMMAND_COUNT]; OutputPin triggerDecoderErrorPin; OutputPin hipCs;