diff --git a/firmware/controllers/actuators/vvt.cpp b/firmware/controllers/actuators/vvt.cpp new file mode 100644 index 0000000000..b18c188273 --- /dev/null +++ b/firmware/controllers/actuators/vvt.cpp @@ -0,0 +1,153 @@ +/* + * @file vvt.cpp + * + * @date Jun 26, 2016 + * @author Andrey Belomutskiy, (c) 2012-2020 + */ + +#include "local_version_holder.h" +#include "allsensors.h" +#include "vvt.h" + +#include "tunerstudio_outputs.h" +#include "fsio_impl.h" +#include "engine_math.h" +#include "pin_repository.h" + +#define NO_PIN_PERIOD 500 + +#if defined(HAS_OS_ACCESS) +#error "Unexpected OS ACCESS HERE" +#endif /* HAS_OS_ACCESS */ + +EXTERN_ENGINE; + +static fsio8_Map3D_u8t vvtTable1("vvt#1"); +static fsio8_Map3D_u8t vvtTable2("vvt#2"); + +static Logging *logger; + +void VvtController::init(int index, const ValueProvider3D* targetMap) { + this->index = index; + m_pid.initPidClass(&CONFIG(auxPid[index])); + m_targetMap = targetMap; +} + +int VvtController::getPeriodMs() { + return isBrainPinValid(engineConfiguration->auxPidPins[index]) ? + GET_PERIOD_LIMITED(&engineConfiguration->auxPid[index]) : NO_PIN_PERIOD; +} + +void VvtController::PeriodicTask() { + if (engine->auxParametersVersion.isOld(engine->getGlobalConfigurationVersion())) { + m_pid.reset(); + } + + update(); +} + +expected VvtController::observePlant() const { + return engine->triggerCentral.getVVTPosition(); +} + +expected VvtController::getSetpoint() const { + int rpm = GET_RPM(); + float load = getFuelingLoad(PASS_ENGINE_PARAMETER_SIGNATURE); + return m_targetMap->getValue(rpm, load); +} + +expected VvtController::getOpenLoop(angle_t target) const { + // TODO: could we do VVT open loop? + return 0; +} + +expected VvtController::getClosedLoop(angle_t setpoint, angle_t observation) { + float retVal = m_pid.getOutput(setpoint, observation); + + if (engineConfiguration->isVerboseAuxPid1) { + scheduleMsg(logger, "aux duty: %.2f/value=%.2f/p=%.2f/i=%.2f/d=%.2f int=%.2f", retVal, observation, + m_pid.getP(), m_pid.getI(), m_pid.getD(), m_pid.getIntegration()); + } + +#if EFI_TUNER_STUDIO + if (engineConfiguration->debugMode == DBG_AUX_PID_1) { + m_pid.postState(&tsOutputChannels); + tsOutputChannels.debugIntField3 = (int)(10 * setpoint); + } +#endif /* EFI_TUNER_STUDIO */ + + return retVal; +} + +void VvtController::setOutput(expected outputValue) { + float rpm = GET_RPM(); + + // todo: make this configurable? + bool enabledAtCurrentRpm = rpm > engineConfiguration->cranking.rpm; + + if (outputValue && enabledAtCurrentRpm) { + m_pwm.setSimplePwmDutyCycle(PERCENT_TO_DUTY(outputValue.Value)); + } else { + m_pwm.setSimplePwmDutyCycle(0); + + // we need to avoid accumulating iTerm while engine is not running + m_pid.reset(); + } +} + +#if EFI_AUX_PID + +static VvtController instances[CAM_INPUTS_COUNT]; + +static void turnAuxPidOn(int index) { + if (!isBrainPinValid(engineConfiguration->auxPidPins[index])) { + return; + } + + startSimplePwmExt(&instances[index].m_pwm, "Aux PID", + &engine->executor, + engineConfiguration->auxPidPins[index], + &instances[index].m_pin, + // todo: do we need two separate frequencies? + engineConfiguration->auxPidFrequency[0], 0.1); +} + +void startVvtControlPins() { + for (int i = 0;i vvtTable1, config->vvtTable1LoadBins, + config->vvtTable1RpmBins); + vvtTable2.init(config->vvtTable2, config->vvtTable2LoadBins, + config->vvtTable2RpmBins); + + + logger = sharedLogger; + + for (int i = 0;i < CAM_INPUTS_COUNT;i++) { + INJECT_ENGINE_REFERENCE(&instances[i]); + + // TODO: this is wrong for cams 3/4 + int indexInBank = i % CAMS_PER_BANK; + auto targetMap = indexInBank == 0 ? &vvtTable1 : &vvtTable2; + instances[i].init(i, targetMap); + } + + startVvtControlPins(); + + for (int i = 0;i < CAM_INPUTS_COUNT;i++) { + instances[i].Start(); + } +} + +#endif diff --git a/firmware/controllers/actuators/vvt.h b/firmware/controllers/actuators/vvt.h new file mode 100644 index 0000000000..6f9943dfb3 --- /dev/null +++ b/firmware/controllers/actuators/vvt.h @@ -0,0 +1,50 @@ +/* + * @file vvt.h + * + * @date Jun 26, 2016 + * @author Andrey Belomutskiy, (c) 2012-2020 + */ + +#pragma once + +#include "engine_ptr.h" +#include "periodic_task.h" +#include "closed_loop_controller.h" +#include "pwm_generator_logic.h" +#include "pid.h" + +class Logging; +class ValueProvider3D; + +void initAuxPid(Logging *sharedLogger); +void startVvtControlPins(); +void stopVvtControlPins(); + +class VvtController : public PeriodicTimerController, public ClosedLoopController { +public: + DECLARE_ENGINE_PTR; + + void init(int index, const ValueProvider3D* targetMap); + + // PeriodicTimerController implementation + int getPeriodMs() override; + void PeriodicTask() override; + + // ClosedLoopController implementation + expected observePlant() const override; + + expected getSetpoint() const override; + expected getOpenLoop(angle_t target) const override; + expected getClosedLoop(angle_t setpoint, angle_t observation) override; + void setOutput(expected outputValue) override; + +private: + Pid m_pid; + const ValueProvider3D* m_targetMap = nullptr; + int index = 0; + +public: + // todo: encapsulate or inject these + SimplePwm m_pwm; + OutputPin m_pin; +}; diff --git a/firmware/controllers/actuators/vvt_pid.cpp b/firmware/controllers/actuators/vvt_pid.cpp deleted file mode 100644 index aede8414f4..0000000000 --- a/firmware/controllers/actuators/vvt_pid.cpp +++ /dev/null @@ -1,147 +0,0 @@ -/* - * @file vvt_pid.cpp - * - * This class is a copy-paste of alternator_controller.cpp TODO: do something about it? extract more common logic? - * - * @date Jun 26, 2016 - * @author Andrey Belomutskiy, (c) 2012-2020 - */ - -#include "local_version_holder.h" -#include "allsensors.h" -#include "vvt_pid.h" - -#if EFI_AUX_PID -#include "pwm_generator_logic.h" -#include "tunerstudio_outputs.h" -#include "fsio_impl.h" -#include "engine_math.h" -#include "pin_repository.h" -#include "periodic_task.h" - -#define NO_PIN_PERIOD 500 - -#if defined(HAS_OS_ACCESS) -#error "Unexpected OS ACCESS HERE" -#endif /* HAS_OS_ACCESS */ - -EXTERN_ENGINE; - -static fsio8_Map3D_u8t vvtTable1("vvt#1"); -static fsio8_Map3D_u8t vvtTable2("vvt#2"); - - -// todo: this is to some extent a copy-paste of alternator_controller. maybe same loop -// for all PIDs? - -static Logging *logger; - -class AuxPidController : public PeriodicTimerController { -public: - - SimplePwm auxPidPwm; - OutputPin auxOutputPin; - - void init(int index) { - this->index = index; - auxPid.initPidClass(&persistentState.persistentConfiguration.engineConfiguration.auxPid[index]); - int camIndex = index % CAMS_PER_BANK; - table = camIndex == 0 ? &vvtTable1 : &vvtTable2; - } - - int getPeriodMs() override { - return isBrainPinValid(engineConfiguration->auxPidPins[index]) ? - GET_PERIOD_LIMITED(&engineConfiguration->auxPid[index]) : NO_PIN_PERIOD; - } - - void PeriodicTask() override { - if (engine->auxParametersVersion.isOld(engine->getGlobalConfigurationVersion())) { - auxPid.reset(); - - } - - float rpm = GET_RPM(); - - // todo: make this configurable? - bool enabledAtCurrentRpm = rpm > engineConfiguration->cranking.rpm; - - if (!enabledAtCurrentRpm) { - // we need to avoid accumulating iTerm while engine is not running - auxPid.reset(); - return; - } - - - float value = engine->triggerCentral.getVVTPosition(); - float targetValue = table->getValue(rpm, getFuelingLoad(PASS_ENGINE_PARAMETER_SIGNATURE)); - - percent_t pwm = auxPid.getOutput(targetValue, value); - if (engineConfiguration->isVerboseAuxPid1) { - scheduleMsg(logger, "aux duty: %.2f/value=%.2f/p=%.2f/i=%.2f/d=%.2f int=%.2f", pwm, value, - auxPid.getP(), auxPid.getI(), auxPid.getD(), auxPid.getIntegration()); - } - - - if (engineConfiguration->debugMode == DBG_AUX_PID_1) { -#if EFI_TUNER_STUDIO - auxPid.postState(&tsOutputChannels); - tsOutputChannels.debugIntField3 = (int)(10 * targetValue); -#endif /* EFI_TUNER_STUDIO */ - } - - auxPidPwm.setSimplePwmDutyCycle(PERCENT_TO_DUTY(pwm)); - } -private: - Pid auxPid; - int index = 0; - ValueProvider3D *table = nullptr; -}; - -static AuxPidController instances[CAM_INPUTS_COUNT]; - -static void turnAuxPidOn(int index) { - if (!isBrainPinValid(engineConfiguration->auxPidPins[index])) { - return; - } - - startSimplePwmExt(&instances[index].auxPidPwm, "Aux PID", - &engine->executor, - engineConfiguration->auxPidPins[index], - &instances[index].auxOutputPin, - // todo: do we need two separate frequencies? - engineConfiguration->auxPidFrequency[0], 0.1); -} - -void startVvtControlPins() { - for (int i = 0;i vvtTable1, config->vvtTable1LoadBins, - config->vvtTable1RpmBins); - vvtTable2.init(config->vvtTable2, config->vvtTable2LoadBins, - config->vvtTable2RpmBins); - - - logger = sharedLogger; - - for (int i = 0;i < CAM_INPUTS_COUNT;i++) { - instances[i].init(i); - } - - startVvtControlPins(); - for (int i = 0;i < CAM_INPUTS_COUNT;i++) { - instances[i].Start(); - } -} - -#endif diff --git a/firmware/controllers/actuators/vvt_pid.h b/firmware/controllers/actuators/vvt_pid.h deleted file mode 100644 index 95cad4753e..0000000000 --- a/firmware/controllers/actuators/vvt_pid.h +++ /dev/null @@ -1,15 +0,0 @@ -/* - * @file aux_pid.h - * - * @date Jun 26, 2016 - * @author Andrey Belomutskiy, (c) 2012-2020 - */ - -#pragma once - -#include "global.h" - -void initAuxPid(Logging *sharedLogger); -void startVvtControlPins(); -void stopVvtControlPins(); - diff --git a/firmware/controllers/controllers.mk b/firmware/controllers/controllers.mk index f07274dd0f..d014482b29 100644 --- a/firmware/controllers/controllers.mk +++ b/firmware/controllers/controllers.mk @@ -18,7 +18,7 @@ CONTROLLERS_SRC_CPP = \ $(CONTROLLERS_DIR)/actuators/idle_hardware.cpp \ $(CONTROLLERS_DIR)/actuators/idle_thread.cpp \ $(CONTROLLERS_DIR)/actuators/pwm_tester.cpp \ - $(CONTROLLERS_DIR)/actuators/algo/vvt_pid.cpp \ + $(CONTROLLERS_DIR)/actuators/vvt.cpp \ $(CONTROLLERS_DIR)/actuators/gppwm/gppwm_channel.cpp \ $(CONTROLLERS_DIR)/actuators/gppwm/gppwm.cpp \ $(CONTROLLERS_DIR)/gauges/tachometer.cpp \ diff --git a/firmware/controllers/engine_controller.cpp b/firmware/controllers/engine_controller.cpp index 144e0a4a0f..4e495db4c2 100644 --- a/firmware/controllers/engine_controller.cpp +++ b/firmware/controllers/engine_controller.cpp @@ -50,7 +50,7 @@ #include "spark_logic.h" #include "aux_valves.h" #include "accelerometer.h" -#include "actuators/vvt_pid.h" +#include "vvt.h" #include "perf_trace.h" #include "boost_control.h" #include "launch_control.h" diff --git a/firmware/hw_layer/hardware.cpp b/firmware/hw_layer/hardware.cpp index c87386671b..0f5b2de475 100644 --- a/firmware/hw_layer/hardware.cpp +++ b/firmware/hw_layer/hardware.cpp @@ -47,7 +47,7 @@ #include "trigger_central.h" #include "svnversion.h" #include "engine_configuration.h" -#include "vvt_pid.h" +#include "vvt.h" #include "perf_trace.h" #include "trigger_emulator_algo.h" #include "boost_control.h" diff --git a/unit_tests/tests/test_vvt.cpp b/unit_tests/tests/test_vvt.cpp new file mode 100644 index 0000000000..02d502b5b5 --- /dev/null +++ b/unit_tests/tests/test_vvt.cpp @@ -0,0 +1,45 @@ +#include "engine_test_helper.h" +#include "vvt.h" +#include "mocks.h" + +using ::testing::StrictMock; +using ::testing::Return; + +TEST(Vvt, setpoint) { + WITH_ENGINE_TEST_HELPER(TEST_ENGINE); + + // Set up a mock target map + StrictMock targetMap; + EXPECT_CALL(targetMap, getValue(4321, 55)) + .WillOnce(Return(20)); + + // Mock necessary inputs + engine->engineState.fuelingLoad = 55; + engine->rpmCalculator.mockRpm = 4321; + + VvtController dut; + INJECT_ENGINE_REFERENCE(&dut); + dut.init(0, &targetMap); + + // Test dut + EXPECT_EQ(20, dut.getSetpoint().value_or(0)); +} + +TEST(Vvt, observePlant) { + WITH_ENGINE_TEST_HELPER(TEST_ENGINE); + + engine->triggerCentral.vvtPosition[0][0] = 23; + + VvtController dut; + INJECT_ENGINE_REFERENCE(&dut); + dut.init(0, nullptr); + + EXPECT_EQ(23, dut.observePlant().value_or(0)); +} + +TEST(Vvt, openLoop) { + VvtController dut; + + // No open loop for now + EXPECT_EQ(dut.getOpenLoop(10), 0); +} diff --git a/unit_tests/tests/tests.mk b/unit_tests/tests/tests.mk index d7b0d9fd1c..2c57161061 100644 --- a/unit_tests/tests/tests.mk +++ b/unit_tests/tests/tests.mk @@ -32,6 +32,7 @@ TESTS_SRC_CPP = \ tests/test_idle_controller.cpp \ tests/test_issue_898.cpp \ tests/test_etb.cpp \ + tests/test_vvt.cpp \ tests/test_launch.cpp \ tests/test_fuel_map.cpp \ tests/ignition_injection/test_fuel_wall_wetting.cpp \