Rewrite boost control in new closed loop framework (#1342)
* rewrite boost * test * fix build * tests * open loop test * test closed loop * fix names * typo * tests * mock boost setOutput * needs more virtual * fix boost logic * switch to TPS based open loop * fix test Co-authored-by: Matthew Kennedy <makenne@microsoft.com>
This commit is contained in:
Matthew Kennedy
GitHub
parent
20a95bdcf1
commit
74d13c391f
|
|
@ -18,9 +18,7 @@
|
|||
#include "io_pins.h"
|
||||
#include "engine_configuration.h"
|
||||
#include "pwm_generator_logic.h"
|
||||
#include "pid.h"
|
||||
#include "engine_controller.h"
|
||||
#include "periodic_task.h"
|
||||
#include "pin_repository.h"
|
||||
#include "pwm_generator_logic.h"
|
||||
#include "pid_auto_tune.h"
|
||||
|
|
@ -37,96 +35,129 @@ static Logging *logger;
|
|||
static boostOpenLoop_Map3D_t boostMapOpen("boostmapopen", 1);
|
||||
static boostOpenLoop_Map3D_t boostMapClosed("boostmapclosed", 1);
|
||||
static SimplePwm boostPwmControl("boost");
|
||||
static Pid boostControlPid;
|
||||
|
||||
static bool shouldResetPid = false;
|
||||
void BoostController::init(SimplePwm* pwm, const ValueProvider3D* openLoopMap, const ValueProvider3D* closedLoopTargetMap, pid_s* pidParams) {
|
||||
m_pwm = pwm;
|
||||
m_openLoopMap = openLoopMap;
|
||||
m_closedLoopTargetMap = closedLoopTargetMap;
|
||||
|
||||
m_pid.initPidClass(pidParams);
|
||||
}
|
||||
|
||||
|
||||
void BoostController::reset() {
|
||||
m_shouldResetPid = true;
|
||||
}
|
||||
|
||||
void BoostController::onConfigurationChange(pid_s* previousConfiguration) {
|
||||
if (!m_pid.isSame(previousConfiguration)) {
|
||||
m_shouldResetPid = true;
|
||||
}
|
||||
}
|
||||
|
||||
int BoostController::getPeriodMs() {
|
||||
return GET_PERIOD_LIMITED(&engineConfiguration->boostPid);
|
||||
}
|
||||
|
||||
expected<float> BoostController::observePlant() const {
|
||||
float map = getMap(PASS_ENGINE_PARAMETER_SIGNATURE);
|
||||
|
||||
if (cisnan(map)) {
|
||||
return unexpected;
|
||||
}
|
||||
|
||||
return map;
|
||||
}
|
||||
|
||||
expected<float> BoostController::getSetpoint() const {
|
||||
float rpm = GET_RPM();
|
||||
|
||||
auto tps = Sensor::get(SensorType::DriverThrottleIntent);
|
||||
|
||||
if (!tps) {
|
||||
return unexpected;
|
||||
}
|
||||
|
||||
if (!m_closedLoopTargetMap) {
|
||||
return unexpected;
|
||||
}
|
||||
|
||||
return m_closedLoopTargetMap->getValue(rpm / RPM_1_BYTE_PACKING_MULT, tps.Value / TPS_1_BYTE_PACKING_MULT) * LOAD_1_BYTE_PACKING_MULT;
|
||||
}
|
||||
|
||||
expected<percent_t> BoostController::getOpenLoop(float target) const {
|
||||
// Boost control open loop doesn't care about target - only MAP/RPM
|
||||
UNUSED(target);
|
||||
|
||||
float rpm = GET_RPM();
|
||||
auto tps = Sensor::get(SensorType::DriverThrottleIntent);
|
||||
|
||||
if (!tps) {
|
||||
return unexpected;
|
||||
}
|
||||
|
||||
if (!m_openLoopMap) {
|
||||
return unexpected;
|
||||
}
|
||||
|
||||
percent_t openLoop = m_openLoopMap->getValue(rpm / RPM_1_BYTE_PACKING_MULT, tps.Value / TPS_1_BYTE_PACKING_MULT) * LOAD_1_BYTE_PACKING_MULT;
|
||||
|
||||
#if EFI_TUNER_STUDIO
|
||||
extern TunerStudioOutputChannels tsOutputChannels;
|
||||
#endif /* EFI_TUNER_STUDIO */
|
||||
if (engineConfiguration->debugMode == DBG_BOOST) {
|
||||
tsOutputChannels.debugFloatField1 = openLoop;
|
||||
}
|
||||
#endif
|
||||
|
||||
static void pidReset(void) {
|
||||
boostControlPid.reset();
|
||||
return openLoop;
|
||||
}
|
||||
|
||||
class BoostControl: public PeriodicTimerController {
|
||||
DECLARE_ENGINE_PTR;
|
||||
|
||||
int getPeriodMs() override {
|
||||
return GET_PERIOD_LIMITED(&engineConfiguration->boostPid);
|
||||
expected<percent_t> BoostController::getClosedLoop(float target, float manifoldPressure) {
|
||||
// If we're in open loop only mode, make no closed loop correction.
|
||||
if (engineConfiguration->boostType != CLOSED_LOOP) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
void PeriodicTask() override {
|
||||
if (shouldResetPid) {
|
||||
pidReset();
|
||||
shouldResetPid = false;
|
||||
}
|
||||
// Reset PID if requested
|
||||
if (m_shouldResetPid) {
|
||||
m_pid.reset();
|
||||
m_shouldResetPid = false;
|
||||
}
|
||||
|
||||
float rpm = GET_RPM_VALUE;
|
||||
float mapValue = getMap(PASS_ENGINE_PARAMETER_SIGNATURE);
|
||||
// If the engine isn't running, don't correct.
|
||||
if (GET_RPM() == 0) {
|
||||
m_pid.reset();
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!engineConfiguration->isBoostControlEnabled)
|
||||
return;
|
||||
float closedLoop = m_pid.getOutput(target, manifoldPressure);
|
||||
|
||||
bool engineRunning = rpm > engineConfiguration->cranking.rpm;
|
||||
if (!engineRunning) {
|
||||
boostControlPid.reset();
|
||||
return;
|
||||
}
|
||||
|
||||
percent_t openLoopDuty = boostMapOpen.getValue(rpm / RPM_1_BYTE_PACKING_MULT, mapValue / LOAD_1_BYTE_PACKING_MULT) * LOAD_1_BYTE_PACKING_MULT;
|
||||
percent_t closedLoopDuty = 0;
|
||||
percent_t duty = openLoopDuty;
|
||||
|
||||
if (engineConfiguration->boostType == CLOSED_LOOP) {
|
||||
auto [valid, tps] = Sensor::get(SensorType::DriverThrottleIntent);
|
||||
|
||||
if (valid) {
|
||||
float targetBoost = boostMapClosed.getValue(rpm / RPM_1_BYTE_PACKING_MULT, tps / TPS_1_BYTE_PACKING_MULT) * LOAD_1_BYTE_PACKING_MULT;
|
||||
closedLoopDuty = openLoopDuty + boostControlPid.getOutput(targetBoost, mapValue);
|
||||
duty += closedLoopDuty;
|
||||
}
|
||||
}
|
||||
|
||||
boostControlPid.iTermMin = -50;
|
||||
boostControlPid.iTermMax = 50;
|
||||
|
||||
if (engineConfiguration->debugMode == DBG_BOOST) {
|
||||
#if EFI_TUNER_STUDIO
|
||||
boostControlPid.postState(&tsOutputChannels);
|
||||
tsOutputChannels.debugFloatField1 = openLoopDuty;
|
||||
tsOutputChannels.debugFloatField7 = closedLoopDuty;
|
||||
if (engineConfiguration->debugMode == DBG_BOOST) {
|
||||
m_pid.postState(&tsOutputChannels);
|
||||
tsOutputChannels.debugFloatField2 = closedLoop;
|
||||
}
|
||||
#endif /* EFI_TUNER_STUDIO */
|
||||
}
|
||||
|
||||
#if EFI_LAUNCH_CONTROL
|
||||
if (engine->setLaunchBoostDuty) {
|
||||
duty = engineConfiguration->launchBoostDuty;
|
||||
return closedLoop;
|
||||
}
|
||||
|
||||
void BoostController::setOutput(expected<float> output) {
|
||||
// TODO: hook up safe duty cycle
|
||||
float duty = PERCENT_TO_DUTY(output.value_or(/*CONFIG(boostControlSafeDutyCycle)*/ 0));
|
||||
|
||||
if (m_pwm) {
|
||||
m_pwm->setSimplePwmDutyCycle(duty);
|
||||
}
|
||||
#endif /* EFI_LAUNCH_CONTROL */
|
||||
|
||||
boostPwmControl.setSimplePwmDutyCycle(PERCENT_TO_DUTY(duty));
|
||||
}
|
||||
};
|
||||
|
||||
static BoostControl BoostController;
|
||||
|
||||
#if !EFI_UNIT_TEST
|
||||
void setBoostPFactor(float value) {
|
||||
engineConfiguration->boostPid.pFactor = value;
|
||||
boostControlPid.reset();
|
||||
}
|
||||
|
||||
void setBoostIFactor(float value) {
|
||||
engineConfiguration->boostPid.iFactor = value;
|
||||
boostControlPid.reset();
|
||||
void BoostController::PeriodicTask() {
|
||||
m_pid.iTermMin = -50;
|
||||
m_pid.iTermMax = 50;
|
||||
|
||||
update();
|
||||
}
|
||||
|
||||
void setBoostDFactor(float value) {
|
||||
engineConfiguration->boostPid.dFactor = value;
|
||||
boostControlPid.reset();
|
||||
}
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
BoostController boostController;
|
||||
|
||||
void setDefaultBoostParameters(DECLARE_CONFIG_PARAMETER_SIGNATURE) {
|
||||
engineConfiguration->isBoostControlEnabled = true;
|
||||
|
|
@ -141,22 +172,17 @@ void setDefaultBoostParameters(DECLARE_CONFIG_PARAMETER_SIGNATURE) {
|
|||
engineConfiguration->boostControlPinMode = OM_DEFAULT;
|
||||
|
||||
setLinearCurve(config->boostRpmBins, 0, 8000 / RPM_1_BYTE_PACKING_MULT, 1);
|
||||
setLinearCurve(config->boostMapBins, 0, 300 / LOAD_1_BYTE_PACKING_MULT, 1);
|
||||
for (int loadIndex = 0;loadIndex<BOOST_LOAD_COUNT;loadIndex++) {
|
||||
for (int rpmIndex = 0;rpmIndex<BOOST_RPM_COUNT;rpmIndex++) {
|
||||
config->boostTableOpenLoop[loadIndex][rpmIndex] = config->boostMapBins[loadIndex];
|
||||
}
|
||||
}
|
||||
|
||||
setLinearCurve(config->boostTpsBins, 0, 100 / TPS_1_BYTE_PACKING_MULT, 1);
|
||||
for (int loadIndex = 0;loadIndex<BOOST_LOAD_COUNT;loadIndex++) {
|
||||
for (int rpmIndex = 0;rpmIndex<BOOST_RPM_COUNT;rpmIndex++) {
|
||||
|
||||
for (int loadIndex = 0; loadIndex < BOOST_LOAD_COUNT; loadIndex++) {
|
||||
for (int rpmIndex = 0; rpmIndex < BOOST_RPM_COUNT; rpmIndex++) {
|
||||
config->boostTableOpenLoop[loadIndex][rpmIndex] = config->boostTpsBins[loadIndex];
|
||||
config->boostTableClosedLoop[loadIndex][rpmIndex] = config->boostTpsBins[loadIndex];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void turnBoostPidOn() {
|
||||
void startBoostPin() {
|
||||
#if !EFI_UNIT_TEST
|
||||
if (CONFIG(boostControlPin) == GPIO_UNASSIGNED){
|
||||
return;
|
||||
|
|
@ -174,18 +200,14 @@ static void turnBoostPidOn() {
|
|||
#endif /* EFI_UNIT_TEST */
|
||||
}
|
||||
|
||||
void startBoostPin(void) {
|
||||
turnBoostPidOn();
|
||||
}
|
||||
|
||||
void stopBoostPin(void) {
|
||||
void stopBoostPin() {
|
||||
#if !EFI_UNIT_TEST
|
||||
brain_pin_markUnused(activeConfiguration.boostControlPin);
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
}
|
||||
|
||||
void onConfigurationChangeBoostCallback(engine_configuration_s *previousConfiguration) {
|
||||
shouldResetPid = !boostControlPid.isSame(&previousConfiguration->boostPid);
|
||||
boostController.onConfigurationChange(&previousConfiguration->boostPid);
|
||||
}
|
||||
|
||||
void initBoostCtrl(Logging *sharedLogger DECLARE_ENGINE_PARAMETER_SUFFIX) {
|
||||
|
|
@ -195,15 +217,18 @@ void initBoostCtrl(Logging *sharedLogger DECLARE_ENGINE_PARAMETER_SUFFIX) {
|
|||
}
|
||||
#endif
|
||||
|
||||
boostControlPid.initPidClass(&engineConfiguration->boostPid);
|
||||
|
||||
logger = sharedLogger;
|
||||
boostMapOpen.init(config->boostTableOpenLoop, config->boostMapBins, config->boostRpmBins);
|
||||
|
||||
// Set up open & closed loop tables
|
||||
boostMapOpen.init(config->boostTableOpenLoop, config->boostTpsBins, config->boostRpmBins);
|
||||
boostMapClosed.init(config->boostTableClosedLoop, config->boostTpsBins, config->boostRpmBins);
|
||||
boostControlPid.reset();
|
||||
|
||||
// Set up boost controller instance
|
||||
boostController.init(&boostPwmControl, &boostMapOpen, &boostMapClosed, &engineConfiguration->boostPid);
|
||||
|
||||
#if !EFI_UNIT_TEST
|
||||
startBoostPin();
|
||||
BoostController.Start();
|
||||
boostController.Start();
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -8,13 +8,46 @@
|
|||
|
||||
#include "engine.h"
|
||||
#include "periodic_task.h"
|
||||
#include "closed_loop_controller.h"
|
||||
#include "pid.h"
|
||||
|
||||
void startBoostPin(void);
|
||||
void stopBoostPin(void);
|
||||
class SimplePwm;
|
||||
|
||||
class BoostController : public ClosedLoopController<float, percent_t>, public PeriodicTimerController {
|
||||
public:
|
||||
DECLARE_ENGINE_PTR;
|
||||
|
||||
void init(SimplePwm* pmw, const ValueProvider3D* openLoopMap, const ValueProvider3D* closedLoopTargetMap, pid_s* pidParams);
|
||||
|
||||
// PeriodicTimerController implementation
|
||||
int getPeriodMs() override;
|
||||
void PeriodicTask() override;
|
||||
void reset();
|
||||
|
||||
// Called when the configuration may have changed. Controller will
|
||||
// reset if necessary.
|
||||
void onConfigurationChange(pid_s* previousConfiguration);
|
||||
|
||||
// Helpers for individual parts of boost control
|
||||
expected<float> observePlant() const override;
|
||||
expected<float> getSetpoint() const override;
|
||||
|
||||
expected<percent_t> getOpenLoop(float target) const override;
|
||||
expected<percent_t> getClosedLoop(float target, float manifoldPressure) override;
|
||||
|
||||
void setOutput(expected<percent_t> outputValue) override;
|
||||
|
||||
private:
|
||||
bool m_shouldResetPid = false;
|
||||
Pid m_pid;
|
||||
|
||||
const ValueProvider3D* m_openLoopMap = nullptr;
|
||||
const ValueProvider3D* m_closedLoopTargetMap = nullptr;
|
||||
SimplePwm* m_pwm = nullptr;
|
||||
};
|
||||
|
||||
void startBoostPin();
|
||||
void stopBoostPin();
|
||||
void initBoostCtrl(Logging *sharedLogger DECLARE_ENGINE_PARAMETER_SUFFIX);
|
||||
void setBoostPFactor(float p);
|
||||
void setBoostIFactor(float i);
|
||||
void setBoostDFactor(float d);
|
||||
void setDefaultBoostParameters(DECLARE_CONFIG_PARAMETER_SIGNATURE);
|
||||
void showBoostInfo(void);
|
||||
void onConfigurationChangeBoostCallback(engine_configuration_s *previousConfiguration);
|
||||
|
|
|
|||
|
|
@ -1285,7 +1285,7 @@ float[AFTERSTART_DECAY_CURVE_SIZE] afterstartDecayTime;;"Seconds", 1, 0
|
|||
|
||||
|
||||
boost_table_t boostTableOpenLoop;
|
||||
uint8_t[BOOST_LOAD_COUNT] boostMapBins;;"", @@LOAD_1_BYTE_PACKING_MULT@@, 0, 0, 600.0, 1
|
||||
uint8_t[8] unused6312;
|
||||
uint8_t[BOOST_RPM_COUNT] boostRpmBins;;"RPM", @@RPM_1_BYTE_PACKING_MULT@@, 0, 0.0, 12000.0, 0
|
||||
|
||||
boost_table_t boostTableClosedLoop;
|
||||
|
|
|
|||
|
|
@ -746,7 +746,7 @@ fileVersion = { @@TS_FILE_VERSION@@ }
|
|||
|
||||
table = boostTableTbl, boostMapOpen, "", 1
|
||||
xBins = boostRpmBins, RPMValue
|
||||
yBins = boostMapBins, MAPValue
|
||||
yBins = boostTpsBins, TPSValue
|
||||
zBins = boostTableOpenLoop
|
||||
|
||||
table = boostTable2Tbl, boostMapClosed, "", 1
|
||||
|
|
|
|||
|
|
@ -0,0 +1,121 @@
|
|||
#include "boost_control.h"
|
||||
#include "engine_controller.h"
|
||||
#include "engine_test_helper.h"
|
||||
|
||||
#include "mocks.h"
|
||||
|
||||
using ::testing::_;
|
||||
using ::testing::StrictMock;
|
||||
|
||||
TEST(BoostControl, Setpoint) {
|
||||
MockVp3d targetMap;
|
||||
|
||||
// Just pass TPS input to output
|
||||
EXPECT_CALL(targetMap, getValue(_, _))
|
||||
.WillRepeatedly([](float xRpm, float tps) { return tps; });
|
||||
|
||||
WITH_ENGINE_TEST_HELPER(TEST_ENGINE);
|
||||
|
||||
BoostController bc;
|
||||
INJECT_ENGINE_REFERENCE(&bc);
|
||||
|
||||
// Should return unexpected without a pedal map cfg'd
|
||||
EXPECT_EQ(bc.getSetpoint(), unexpected);
|
||||
|
||||
// Now init with mock target map
|
||||
bc.init(nullptr, nullptr, &targetMap, nullptr);
|
||||
|
||||
// Should still return unxepected since TPS is invalid
|
||||
EXPECT_EQ(bc.getSetpoint(), unexpected);
|
||||
|
||||
// Configure TPS, should get passthru of tps value
|
||||
Sensor::setMockValue(SensorType::DriverThrottleIntent, 35.0f);
|
||||
EXPECT_FLOAT_EQ(bc.getSetpoint().value_or(-1), 35.0f);
|
||||
}
|
||||
|
||||
TEST(BoostControl, ObservePlant) {
|
||||
WITH_ENGINE_TEST_HELPER(TEST_ENGINE);
|
||||
|
||||
BoostController bc;
|
||||
INJECT_ENGINE_REFERENCE(&bc);
|
||||
|
||||
engine->mockMapValue = NAN;
|
||||
// Check that invalid MAP returns unexpected
|
||||
EXPECT_EQ(bc.observePlant(), unexpected);
|
||||
|
||||
// Test valid MAP value
|
||||
engine->mockMapValue = 150.0f;
|
||||
|
||||
EXPECT_FLOAT_EQ(bc.observePlant().value_or(0), 150.0f);
|
||||
}
|
||||
|
||||
TEST(BoostControl, OpenLoop) {
|
||||
MockVp3d openMap;
|
||||
|
||||
// Just pass MAP input to output
|
||||
EXPECT_CALL(openMap, getValue(_, _))
|
||||
.WillRepeatedly([](float xRpm, float tps) { return tps; });
|
||||
|
||||
WITH_ENGINE_TEST_HELPER(TEST_ENGINE);
|
||||
|
||||
BoostController bc;
|
||||
INJECT_ENGINE_REFERENCE(&bc);
|
||||
|
||||
// Without table set, should return unexpected
|
||||
EXPECT_EQ(bc.getOpenLoop(0), unexpected);
|
||||
|
||||
bc.init(nullptr, &openMap, nullptr, nullptr);
|
||||
|
||||
// Should pass TPS value thru
|
||||
Sensor::setMockValue(SensorType::DriverThrottleIntent, 47.0f);
|
||||
EXPECT_FLOAT_EQ(bc.getOpenLoop(0).value_or(-1), 47.0f);
|
||||
}
|
||||
|
||||
TEST(BoostControl, ClosedLoop) {
|
||||
WITH_ENGINE_TEST_HELPER(TEST_ENGINE);
|
||||
|
||||
BoostController bc;
|
||||
INJECT_ENGINE_REFERENCE(&bc);
|
||||
|
||||
pid_s pidCfg = {
|
||||
1, 0, 0, // P controller, easier to test
|
||||
0, // no offset
|
||||
5, // 5ms period
|
||||
-100, 100 // min/max output
|
||||
};
|
||||
|
||||
bc.init(nullptr, nullptr, nullptr, &pidCfg);
|
||||
|
||||
// Enable closed loop
|
||||
CONFIG(boostType) = CLOSED_LOOP;
|
||||
|
||||
// At 0 RPM, closed loop is disabled
|
||||
ENGINE(rpmCalculator.mockRpm) = 0;
|
||||
EXPECT_EQ(0, bc.getClosedLoop(150, 100).value_or(-1000));
|
||||
|
||||
// With RPM, we should get an output
|
||||
ENGINE(rpmCalculator.mockRpm) = 1000;
|
||||
// Actual is below target -> positive output
|
||||
EXPECT_FLOAT_EQ(50, bc.getClosedLoop(150, 100).value_or(-1000));
|
||||
// Actual is above target -> negative output
|
||||
EXPECT_FLOAT_EQ(-25.0f, bc.getClosedLoop(150, 175).value_or(-1000));
|
||||
|
||||
// Disabling closed loop should return 0
|
||||
CONFIG(boostType) = OPEN_LOOP;
|
||||
EXPECT_FLOAT_EQ(0, bc.getClosedLoop(150, 175).value_or(-1000));
|
||||
}
|
||||
|
||||
TEST(BoostControl, SetOutput) {
|
||||
StrictMock<MockPwm> pwm;
|
||||
BoostController bc;
|
||||
|
||||
EXPECT_CALL(pwm, setSimplePwmDutyCycle(0.25f));
|
||||
|
||||
// Don't crash if not init'd (don't deref null ptr m_pwm)
|
||||
EXPECT_NO_THROW(bc.setOutput(25.0f));
|
||||
|
||||
// Init with mock PWM device
|
||||
bc.init(&pwm, nullptr, nullptr, nullptr);
|
||||
|
||||
bc.setOutput(25.0f);
|
||||
}
|
||||
|
|
@ -49,6 +49,7 @@ TESTS_SRC_CPP = \
|
|||
tests/sensor/redundant.cpp \
|
||||
tests/sensor/test_sensor_init.cpp \
|
||||
tests/test_closed_loop_controller.cpp \
|
||||
tests/test_boost.cpp \
|
||||
tests/test_gppwm.cpp \
|
||||
tests/test_fuel_math.cpp \
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue