rusefi/firmware/controllers/electronic_throttle.cpp

411 lines
12 KiB
C++
Raw Normal View History

2015-07-10 06:01:56 -07:00
/**
* @file electronic_throttle.cpp
* @brief Electronic Throttle Module driver L298N
*
* todo: make this more universal if/when we get other hardware options
*
2019-02-10 16:13:04 -08:00
* Jan 2019 actually driven around the block but still need some work.
*
2017-01-27 18:04:23 -08:00
* Jan 2017 status:
2017-05-25 09:09:07 -07:00
* Electronic throttle body with it's spring is definitely not linear - both P and I factors of PID are required to get any results
2017-01-27 18:04:23 -08:00
* PID implementation tested on a bench only
* it is believed that more than just PID would be needed, as is this is probably
* not usable on a real vehicle. Needs to be tested :)
*
2017-05-25 09:09:07 -07:00
*
*
2017-05-24 20:53:07 -07:00
* ETB is controlled according to pedal position input (pedal position sensor is a potentiometer)
2019-02-10 16:52:06 -08:00
* pedal 0% means pedal not pressed / idle
2017-05-24 20:53:07 -07:00
* pedal 100% means pedal all the way down
2017-05-25 09:09:07 -07:00
* (not TPS - not the one you can calibrate in TunerStudio)
2017-05-24 20:53:07 -07:00
*
* At the moment we only control opening motor - while relying on ETB spring to move throttle butterfly
* back. Throttle position sensor inside ETB is used for closed-loop PID control of ETB.
*
2017-05-25 09:09:07 -07:00
* See also pid.cpp
*
2017-05-29 20:36:08 -07:00
* Relevant console commands:
*
2019-02-27 05:55:56 -08:00
* ETB_BENCH_ENGINE
* set engine_type 58
*
2017-05-29 20:36:08 -07:00
* enable verbose_etb
* disable verbose_etb
* ethinfo
2018-01-30 19:04:33 -08:00
* set mock_pedal_position X
2017-05-24 20:53:07 -07:00
*
2018-12-01 13:41:40 -08:00
*
* set debug_mode 17
* for PID outputs
*
2019-03-01 20:09:33 -08:00
* set etb_p X
* set etb_i X
* set etb_d X
*
*
2017-01-27 18:04:23 -08:00
* http://rusefi.com/forum/viewtopic.php?f=5&t=592
*
2015-07-10 06:01:56 -07:00
* @date Dec 7, 2013
2018-01-20 17:55:31 -08:00
* @author Andrey Belomutskiy, (c) 2012-2018
2015-07-10 06:01:56 -07:00
*
* 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/>.
*/
2018-09-16 19:26:57 -07:00
#include "global.h"
2015-07-10 06:01:56 -07:00
#include "electronic_throttle.h"
#include "tps.h"
#include "io_pins.h"
#include "engine_configuration.h"
#include "pwm_generator_logic.h"
#include "pid.h"
#include "engine_controller.h"
2019-02-10 16:52:06 -08:00
#include "PeriodicController.h"
2015-07-10 06:01:56 -07:00
#if EFI_ELECTRONIC_THROTTLE_BODY || defined(__DOXYGEN__)
2017-01-02 11:03:17 -08:00
#include "pin_repository.h"
#include "pwm_generator.h"
2019-02-27 14:54:25 -08:00
#include "DcMotor.h"
2017-09-21 20:21:03 -07:00
#include "pid_auto_tune.h"
#if EFI_TUNER_STUDIO || defined(__DOXYGEN__)
2017-05-22 12:31:03 -07:00
extern TunerStudioOutputChannels tsOutputChannels;
#endif /* EFI_TUNER_STUDIO */
2017-05-27 20:01:41 -07:00
static bool shouldResetPid = false;
2015-07-10 06:01:56 -07:00
2018-11-22 20:42:30 -08:00
static pid_s tuneWorkingPidSettings;
static Pid tuneWorkingPid(&tuneWorkingPidSettings);
2017-09-21 20:21:03 -07:00
static PID_AutoTune autoTune;
2015-07-10 06:01:56 -07:00
static LoggingWithStorage logger("ETB");
2019-02-10 16:52:06 -08:00
2015-07-10 06:01:56 -07:00
/**
* @brief Pulse-Width Modulation state
*/
/*CCM_OPTIONAL*/ static SimplePwm etbPwmUp("etbUp");
2018-10-21 14:45:14 -07:00
static float valueOverride = NAN;
2018-09-24 20:57:03 -07:00
/*
CCM_OPTIONAL static SimplePwm etbPwmDown("etbDown");
2018-09-24 20:57:03 -07:00
*/
/*CCM_OPTIONAL*/ static OutputPin outputDirectionOpen;
/*CCM_OPTIONAL*/ static OutputPin outputDirectionClose;
2015-07-10 06:01:56 -07:00
2019-02-27 14:54:25 -08:00
static TwoPinDcMotor dcMotor(&etbPwmUp, &outputDirectionOpen, &outputDirectionClose);
2017-05-27 20:01:41 -07:00
EXTERN_ENGINE;
2017-05-29 20:15:07 -07:00
static Pid pid(&engineConfiguration->etb);
2015-07-10 06:01:56 -07:00
2017-09-21 20:21:03 -07:00
static percent_t currentEtbDuty;
2015-07-10 06:01:56 -07:00
//static bool wasEtbBraking = false;
2015-07-10 17:01:36 -07:00
2019-03-01 20:09:33 -08:00
// looks like my H-bridge does not like 100% duty cycle and it just hangs up?
// so we use 99.9% to indicate that things are alive
#define PERCENT_TO_DUTY(X) (maxF(minI(X, 99.9), -99.9) / 100.0)
2019-02-27 14:54:25 -08:00
2019-03-01 20:09:33 -08:00
//#define PERCENT_TO_DUTY(X) ((X) / 100.0)
2018-11-25 20:13:03 -08:00
2019-02-10 16:52:06 -08:00
class EtbController : public PeriodicController<UTILITY_THREAD_STACK_SIZE> {
public:
EtbController() : PeriodicController("ETB") { }
private:
2019-02-17 15:00:41 -08:00
float feedForward = 0;
2019-02-10 16:52:06 -08:00
void PeriodicTask(efitime_t nowNt) override {
UNUSED(nowNt);
2019-02-10 20:54:41 -08:00
setPeriod(NOT_TOO_OFTEN(10 /* ms */, engineConfiguration->etb.periodMs));
2019-02-10 16:52:06 -08:00
2018-12-01 13:41:40 -08:00
// set debug_mode 17
2018-10-21 14:45:14 -07:00
if (engineConfiguration->debugMode == DBG_ELECTRONIC_THROTTLE_PID) {
#if EFI_TUNER_STUDIO || defined(__DOXYGEN__)
2018-10-21 14:45:14 -07:00
pid.postState(&tsOutputChannels);
2019-02-17 15:00:41 -08:00
tsOutputChannels.debugIntField5 = feedForward;
#endif /* EFI_TUNER_STUDIO */
2018-10-21 14:45:14 -07:00
} else if (engineConfiguration->debugMode == DBG_ELECTRONIC_THROTTLE_EXTRA) {
#if EFI_TUNER_STUDIO || defined(__DOXYGEN__)
2018-12-01 13:41:40 -08:00
// set debug_mode 29
2018-10-21 14:45:14 -07:00
tsOutputChannels.debugFloatField1 = valueOverride;
#endif /* EFI_TUNER_STUDIO */
2018-10-21 14:45:14 -07:00
}
2017-05-27 20:01:41 -07:00
if (shouldResetPid) {
pid.reset();
shouldResetPid = false;
}
2018-10-21 14:45:14 -07:00
if (!cisnan(valueOverride)) {
2019-02-27 14:54:25 -08:00
dcMotor.Set(valueOverride);
2019-02-10 16:52:06 -08:00
return;
2018-10-21 14:45:14 -07:00
}
2018-11-22 20:06:06 -08:00
percent_t actualThrottlePosition = getTPS();
2017-09-21 20:21:03 -07:00
if (engine->etbAutoTune) {
2018-11-22 20:06:06 -08:00
autoTune.input = actualThrottlePosition;
2018-11-22 20:54:11 -08:00
bool result = autoTune.Runtime(&logger);
2017-09-21 20:21:03 -07:00
2018-11-22 20:42:30 -08:00
tuneWorkingPid.updateFactors(autoTune.output, 0, 0);
2017-09-21 20:21:03 -07:00
2018-11-22 20:42:30 -08:00
float value = tuneWorkingPid.getValue(50, actualThrottlePosition);
2018-11-25 20:13:03 -08:00
scheduleMsg(&logger, "AT input=%f output=%f PID=%f", autoTune.input,
autoTune.output,
value);
scheduleMsg(&logger, "AT PID=%f", value);
2019-02-27 14:54:25 -08:00
dcMotor.Set(PERCENT_TO_DUTY(value));
2018-11-22 20:42:30 -08:00
2018-11-22 20:54:11 -08:00
if (result) {
2018-11-25 20:13:03 -08:00
scheduleMsg(&logger, "GREAT NEWS! %f/%f/%f", autoTune.GetKp(), autoTune.GetKi(), autoTune.GetKd());
2018-11-22 20:54:11 -08:00
}
2019-02-10 16:52:06 -08:00
return;
2017-09-21 20:21:03 -07:00
}
2017-05-27 20:01:41 -07:00
2017-11-16 11:44:34 -08:00
percent_t targetPosition = getPedalPosition(PASS_ENGINE_PARAMETER_SIGNATURE);
2015-07-10 06:01:56 -07:00
2019-02-17 15:00:41 -08:00
feedForward = interpolate2d("etbb", targetPosition, engineConfiguration->etbBiasBins, engineConfiguration->etbBiasValues, ETB_BIAS_CURVE_LENGTH);
2017-11-16 11:44:34 -08:00
2019-02-27 14:54:25 -08:00
currentEtbDuty = feedForward +
pid.getValue(targetPosition, actualThrottlePosition);
2015-07-10 06:01:56 -07:00
2019-02-27 14:54:25 -08:00
dcMotor.Set(PERCENT_TO_DUTY(currentEtbDuty));
2019-02-10 16:13:04 -08:00
/*
if (CONFIGB(etbDirectionPin2) != GPIO_UNASSIGNED) {
2018-09-24 20:57:03 -07:00
bool needEtbBraking = absF(targetPosition - actualThrottlePosition) < 3;
if (needEtbBraking != wasEtbBraking) {
scheduleMsg(&logger, "need ETB braking: %d", needEtbBraking);
wasEtbBraking = needEtbBraking;
}
outputDirectionClose.setValue(needEtbBraking);
2015-07-10 17:01:36 -07:00
}
2019-02-10 16:13:04 -08:00
*/
2017-05-25 05:57:03 -07:00
if (engineConfiguration->isVerboseETB) {
2017-05-29 20:15:07 -07:00
pid.showPidStatus(&logger, "ETB");
2017-05-25 05:57:03 -07:00
}
2015-07-10 06:01:56 -07:00
}
2019-02-10 16:52:06 -08:00
};
static EtbController etbController;
2015-07-10 06:01:56 -07:00
2018-09-24 20:57:03 -07:00
/**
2018-11-25 20:13:03 -08:00
* set_etb X
2018-10-21 14:45:14 -07:00
* manual duty cycle control without PID. Percent value from 0 to 100
2018-09-24 20:57:03 -07:00
*/
2018-10-21 14:45:14 -07:00
static void setThrottleDutyCycle(float level) {
2018-11-26 17:19:42 -08:00
scheduleMsg(&logger, "setting ETB duty=%f%%", level);
2018-11-26 17:40:24 -08:00
if (cisnan(level)) {
valueOverride = NAN;
return;
}
2015-07-10 06:01:56 -07:00
2018-11-25 20:13:03 -08:00
float dc = PERCENT_TO_DUTY(level);
2018-10-21 14:45:14 -07:00
valueOverride = dc;
2019-02-27 14:54:25 -08:00
dcMotor.Set(dc);
2018-11-26 17:19:42 -08:00
scheduleMsg(&logger, "duty ETB duty=%f", dc);
2015-07-10 06:01:56 -07:00
}
static void showEthInfo(void) {
static char pinNameBuffer[16];
2017-09-21 20:21:03 -07:00
scheduleMsg(&logger, "etbAutoTune=%d",
engine->etbAutoTune);
2018-01-23 09:05:14 -08:00
scheduleMsg(&logger, "throttlePedal=%.2f %.2f/%.2f @%s",
2017-05-29 20:36:08 -07:00
getPedalPosition(),
engineConfiguration->throttlePedalUpVoltage,
engineConfiguration->throttlePedalWOTVoltage,
getPinNameByAdcChannel("tPedal", engineConfiguration->throttlePedalPositionAdcChannel, pinNameBuffer));
2015-07-10 06:01:56 -07:00
2018-01-23 09:05:14 -08:00
scheduleMsg(&logger, "TPS=%.2f", getTPS());
2019-03-01 20:09:33 -08:00
scheduleMsg(&logger, "dir=%d", dcMotor.isOpenDirection());
2015-07-10 06:01:56 -07:00
2018-01-23 09:05:14 -08:00
scheduleMsg(&logger, "etbControlPin1=%s duty=%.2f freq=%d",
hwPortname(CONFIGB(etbControlPin1)),
2017-05-29 20:36:08 -07:00
currentEtbDuty,
engineConfiguration->etbFreq);
scheduleMsg(&logger, "close dir=%s", hwPortname(CONFIGB(etbDirectionPin2)));
2017-05-29 20:36:08 -07:00
pid.showPidStatus(&logger, "ETB");
2015-07-10 06:01:56 -07:00
}
2019-03-01 20:09:33 -08:00
/**
* set etb_p X
*/
2017-01-06 14:01:28 -08:00
void setEtbPFactor(float value) {
2015-11-11 20:01:18 -08:00
engineConfiguration->etb.pFactor = value;
2018-11-26 19:17:16 -08:00
pid.reset();
2015-07-10 06:01:56 -07:00
showEthInfo();
}
2019-03-01 20:09:33 -08:00
/**
* set etb_i X
*/
2017-01-06 14:01:28 -08:00
void setEtbIFactor(float value) {
2015-11-11 20:01:18 -08:00
engineConfiguration->etb.iFactor = value;
2018-11-26 19:17:16 -08:00
pid.reset();
2018-09-24 20:57:03 -07:00
showEthInfo();
}
2019-03-01 20:09:33 -08:00
/**
* set etb_d X
*/
2018-09-24 20:57:03 -07:00
void setEtbDFactor(float value) {
engineConfiguration->etb.dFactor = value;
2018-11-26 19:17:16 -08:00
pid.reset();
showEthInfo();
}
void setEtbOffset(int value) {
engineConfiguration->etb.offset = value;
pid.reset();
2015-07-10 06:01:56 -07:00
showEthInfo();
}
2019-02-27 06:57:03 -08:00
void setDefaultEtbParameters(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
engineConfiguration->throttlePedalUpVoltage = 0; // that's voltage, not ADC like with TPS
engineConfiguration->throttlePedalWOTVoltage = 6; // that's voltage, not ADC like with TPS
2015-11-11 20:01:18 -08:00
engineConfiguration->etb.pFactor = 1;
2019-02-27 06:57:03 -08:00
engineConfiguration->etb.iFactor = 0.05;
engineConfiguration->etb.dFactor = 0.0;
2019-02-10 19:47:49 -08:00
engineConfiguration->etb.periodMs = 100;
2017-05-29 19:08:55 -07:00
engineConfiguration->etbFreq = 300;
2019-03-02 11:00:32 -08:00
engineConfiguration->etb_iTermMin = -300;
engineConfiguration->etb_iTermMax = 300;
2018-09-24 20:57:03 -07:00
// CONFIGB(etbControlPin1) = GPIOE_4; // test board, matched default fuel pump relay
2015-07-10 06:01:56 -07:00
}
2017-05-30 18:56:56 -07:00
bool isETBRestartNeeded(void) {
/**
* We do not want any interruption in HW pin while adjusting other properties
*/
return engineConfiguration->bc.etbControlPin1 != activeConfiguration.bc.etbControlPin1 ||
engineConfiguration->bc.etbControlPin2 != activeConfiguration.bc.etbControlPin2 ||
engineConfiguration->bc.etbDirectionPin1 != activeConfiguration.bc.etbDirectionPin1 ||
engineConfiguration->bc.etbDirectionPin2 != activeConfiguration.bc.etbDirectionPin2;
}
2017-03-19 18:44:52 -07:00
void stopETBPins(void) {
2017-03-21 18:12:51 -07:00
unmarkPin(activeConfiguration.bc.etbControlPin1);
unmarkPin(activeConfiguration.bc.etbControlPin2);
unmarkPin(activeConfiguration.bc.etbDirectionPin1);
unmarkPin(activeConfiguration.bc.etbDirectionPin2);
2017-03-19 18:44:52 -07:00
}
2015-07-10 06:01:56 -07:00
2017-05-27 20:01:41 -07:00
void onConfigurationChangeElectronicThrottleCallback(engine_configuration_s *previousConfiguration) {
2017-05-28 11:22:43 -07:00
shouldResetPid = !pid.isSame(&previousConfiguration->etb);
2017-05-27 20:01:41 -07:00
}
2017-03-19 18:44:52 -07:00
void startETBPins(void) {
2017-05-29 19:08:55 -07:00
int freq = maxI(100, engineConfiguration->etbFreq);
2015-07-10 06:01:56 -07:00
// this line used for PWM
startSimplePwmExt(&etbPwmUp, "etb1",
&engine->executor,
CONFIGB(etbControlPin1),
2016-09-14 16:03:00 -07:00
&enginePins.etbOutput1,
2017-05-29 19:08:55 -07:00
freq,
2015-07-10 06:01:56 -07:00
0.80,
applyPinState);
2018-09-24 20:57:03 -07:00
/*
2015-07-10 06:01:56 -07:00
startSimplePwmExt(&etbPwmDown, "etb2",
CONFIGB(etbControlPin2),
2016-09-14 16:03:00 -07:00
&enginePins.etbOutput2,
2017-05-29 19:08:55 -07:00
freq,
2015-07-10 06:01:56 -07:00
0.80,
applyPinState);
2018-09-24 20:57:03 -07:00
*/
2019-02-10 16:13:04 -08:00
outputDirectionOpen.initPin("etb dir open", CONFIGB(etbDirectionPin1));
outputDirectionClose.initPin("etb dir close", CONFIGB(etbDirectionPin2));
2017-03-19 18:44:52 -07:00
}
2017-09-21 20:21:03 -07:00
static void setTempOutput(float value) {
autoTune.output = value;
}
2018-11-25 20:13:03 -08:00
/**
* set_etbat_step X
*/
static void setAutoStep(float value) {
autoTune.reset();
2018-11-22 20:06:06 -08:00
autoTune.SetOutputStep(value);
2017-09-21 20:21:03 -07:00
}
2018-11-25 20:13:03 -08:00
static void setAutoPeriod(int period) {
2019-02-10 19:47:49 -08:00
tuneWorkingPidSettings.periodMs = period;
2018-11-25 20:13:03 -08:00
autoTune.reset();
}
2018-11-26 18:40:41 -08:00
static void setAutoOffset(int offset) {
tuneWorkingPidSettings.offset = offset;
autoTune.reset();
}
2018-12-09 10:50:13 -08:00
void setDefaultEtbBiasCurve(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
engineConfiguration->etbBiasBins[0] = 0;
2019-03-02 11:00:32 -08:00
engineConfiguration->etbBiasBins[1] = 1;
engineConfiguration->etbBiasBins[2] = 2;
engineConfiguration->etbBiasBins[3] = 4;
engineConfiguration->etbBiasBins[4] = 7;
2018-12-09 10:50:13 -08:00
engineConfiguration->etbBiasBins[5] = 98;
engineConfiguration->etbBiasBins[6] = 99;
engineConfiguration->etbBiasBins[7] = 100;
engineConfiguration->etbBiasValues[0] = -100 / 255.0f * 100;
engineConfiguration->etbBiasValues[1] = -95 / 255.0f * 100;
engineConfiguration->etbBiasValues[2] = -80 / 255.0f * 100;
engineConfiguration->etbBiasValues[3] = 0 / 255.0f * 100;
engineConfiguration->etbBiasValues[4] = 115 / 255.0f * 100;
engineConfiguration->etbBiasValues[5] = 142 / 255.0f * 100;
engineConfiguration->etbBiasValues[6] = 142 / 255.0f * 100;
engineConfiguration->etbBiasValues[7] = 142 / 255.0f * 100;
}
2017-03-19 18:44:52 -07:00
void initElectronicThrottle(void) {
2017-05-29 19:08:55 -07:00
addConsoleAction("ethinfo", showEthInfo);
2017-03-19 18:44:52 -07:00
if (!hasPedalPositionSensor()) {
return;
}
2018-11-25 20:13:03 -08:00
autoTune.SetOutputStep(0.1);
2017-03-19 18:44:52 -07:00
startETBPins();
2015-07-10 17:01:36 -07:00
2018-12-01 13:41:40 -08:00
// manual duty cycle control without PID. Percent value from 0 to 100
2018-11-26 17:40:24 -08:00
addConsoleActionNANF("set_etb", setThrottleDutyCycle);
2017-09-21 20:21:03 -07:00
2018-11-22 20:42:30 -08:00
tuneWorkingPidSettings.pFactor = 1;
tuneWorkingPidSettings.iFactor = 0;
tuneWorkingPidSettings.dFactor = 0;
2018-11-25 20:13:03 -08:00
// tuneWorkingPidSettings.offset = 10; // todo: not hard-coded value
2019-02-10 19:47:49 -08:00
//todo tuneWorkingPidSettings.periodMs = 10;
2018-11-22 20:42:30 -08:00
tuneWorkingPidSettings.minValue = 0;
tuneWorkingPidSettings.maxValue = 100;
2019-02-10 19:47:49 -08:00
tuneWorkingPidSettings.periodMs = 100;
2018-11-22 20:42:30 -08:00
2019-03-01 20:09:33 -08:00
// this is useful once you do "enable etb_auto"
2018-11-25 20:13:03 -08:00
addConsoleActionF("set_etbat_output", setTempOutput);
addConsoleActionF("set_etbat_step", setAutoStep);
addConsoleActionI("set_etbat_period", setAutoPeriod);
2018-11-26 18:40:41 -08:00
addConsoleActionI("set_etbat_offset", setAutoOffset);
2015-07-10 06:01:56 -07:00
2018-11-26 19:17:16 -08:00
pid.reset();
2015-07-10 06:01:56 -07:00
2019-02-10 16:52:06 -08:00
etbController.Start();
2015-07-10 06:01:56 -07:00
}
#endif /* EFI_ELECTRONIC_THROTTLE_BODY */