From ff5b3ac3bbc83d84b76a37a773d82c1deab616b0 Mon Sep 17 00:00:00 2001 From: Matthew Kennedy Date: Mon, 13 Nov 2023 13:30:23 -0800 Subject: [PATCH] split heater thread and heater logic --- firmware/Makefile | 3 +- firmware/heater_control.cpp | 157 +++++------------------------------- firmware/heater_control.h | 12 ++- firmware/heater_thread.cpp | 120 +++++++++++++++++++++++++++ firmware/wideband.mk | 2 + test/Makefile | 1 + test/test_stubs.cpp | 11 +++ test/tests/test_heater.cpp | 11 ++- 8 files changed, 173 insertions(+), 144 deletions(-) create mode 100644 firmware/heater_thread.cpp create mode 100644 test/test_stubs.cpp diff --git a/firmware/Makefile b/firmware/Makefile index 0cb2dd8..4101e87 100644 --- a/firmware/Makefile +++ b/firmware/Makefile @@ -153,7 +153,6 @@ CPPSRC = $(ALLCPPSRC) \ pwm.cpp \ dac.cpp \ pump_dac.cpp \ - pid.cpp \ pump_control.cpp \ max3185x.cpp \ uart.cpp \ @@ -165,7 +164,7 @@ CPPSRC = $(ALLCPPSRC) \ console/binary/tunerstudio_io_serial.cpp \ console/binary/tunerstudio_commands.cpp \ sampling_thread.cpp \ - heater_control.cpp \ + heater_thread.cpp \ main.cpp # List ASM source files here. diff --git a/firmware/heater_control.cpp b/firmware/heater_control.cpp index 693d069..585a1b5 100644 --- a/firmware/heater_control.cpp +++ b/firmware/heater_control.cpp @@ -1,103 +1,37 @@ #include "heater_control.h" -#include "ch.h" -#include "hal.h" - -#include "port.h" #include "fault.h" -#include "pwm.h" #include "sampling.h" -struct sensorHeaterParams { - float targetTemp; - float targetESR; -}; - -static const struct sensorHeaterParams heaterParams49 = { - .targetTemp = 780, - .targetESR = 300, -}; - -static const struct sensorHeaterParams heaterParams42 = { - .targetTemp = 730, - .targetESR = 80, -}; - -static const struct sensorHeaterParams heaterParamsAdv = { - .targetTemp = 785, - .targetESR = 300, -}; - -static const sensorHeaterParams *getHeaterParams(SensorType type) { - switch (type) { - case SensorType::LSU42: - return &heaterParams42; - case SensorType::LSUADV: - return &heaterParamsAdv; - case SensorType::LSU49: - default: - return &heaterParams49; - } -} - using namespace wbo; -// 400khz / 1024 = 390hz PWM -static Pwm heaterPwm(HEATER_PWM_DEVICE); -static const PWMConfig heaterPwmConfig = { - .frequency = 400'000, - .period = 1024, - .callback = nullptr, - .channels = { - {PWM_OUTPUT_ACTIVE_HIGH, nullptr}, - {PWM_OUTPUT_ACTIVE_HIGH, nullptr}, - {PWM_OUTPUT_ACTIVE_HIGH, nullptr}, - {PWM_OUTPUT_ACTIVE_HIGH, nullptr} - }, - .cr2 = 0, -#if STM32_PWM_USE_ADVANCED - .bdtr = 0, -#endif - .dier = 0 -}; - -static const struct sensorHeaterParams *heater; - -HeaterController::HeaterController(int ch, int pwm_ch) - : ch(ch), pwm_ch(pwm_ch) +HeaterControllerBase::HeaterControllerBase(int ch) + : ch(ch) { } -bool HeaterController::IsRunningClosedLoop() const +void HeaterControllerBase::Configure(float targetTempC, float targetEsr) +{ + m_targetTempC = targetTempC; + m_targetEsr = targetEsr; +} + +bool HeaterControllerBase::IsRunningClosedLoop() const { return heaterState == HeaterState::ClosedLoop; } -float HeaterController::GetHeaterEffectiveVoltage() const +float HeaterControllerBase::GetHeaterEffectiveVoltage() const { return heaterVoltage; } -HeaterState HeaterController::GetHeaterState() const +HeaterState HeaterControllerBase::GetHeaterState() const { return heaterState; } -HeaterController heaterControllers[AFR_CHANNELS] = -{ - { 0, HEATER_PWM_CHANNEL_0 }, - -#if AFR_CHANNELS >= 2 - { 1, HEATER_PWM_CHANNEL_1 } -#endif -}; - -const IHeaterController& GetHeaterController(int ch) -{ - return heaterControllers[ch]; -} - -HeaterState HeaterController::GetNextState(HeaterAllow heaterAllowState, float batteryVoltage, float sensorTemp) +HeaterState HeaterControllerBase::GetNextState(HeaterAllow heaterAllowState, float batteryVoltage, float sensorTemp) { bool heaterAllowed = heaterAllowState == HeaterAllow::Allowed; @@ -124,9 +58,9 @@ HeaterState HeaterController::GetNextState(HeaterAllow heaterAllowState, float b return HeaterState::Preheat; } - float overheatTemp = heater->targetTemp + 100; - float closedLoopTemp = heater->targetTemp - 50; - float underheatTemp = heater->targetTemp - 100; + float overheatTemp = m_targetTempC + 100; + float closedLoopTemp = m_targetTempC - 50; + float underheatTemp = m_targetTempC - 100; switch (heaterState) { @@ -185,7 +119,7 @@ HeaterState HeaterController::GetNextState(HeaterAllow heaterAllowState, float b return heaterState; } -float HeaterController::GetVoltageForState(float heaterEsr) +float HeaterControllerBase::GetVoltageForState(float heaterEsr) { switch (heaterState) { @@ -207,7 +141,7 @@ float HeaterController::GetVoltageForState(float heaterEsr) // Negated because lower resistance -> hotter // TODO: heater PID should operate on temperature, not ESR - return 7.5f - heaterPid.GetOutput(heater->targetESR, heaterEsr); + return 7.5f - heaterPid.GetOutput(m_targetEsr, heaterEsr); case HeaterState::Stopped: case HeaterState::NoHeaterSupply: // Something has gone wrong, turn off the heater. @@ -218,7 +152,7 @@ float HeaterController::GetVoltageForState(float heaterEsr) return 0; } -void HeaterController::Update(const ISampler& sampler, HeaterAllow heaterAllowState) +void HeaterControllerBase::Update(const ISampler& sampler, HeaterAllow heaterAllowState) { // Read sensor state float heaterEsr = sampler.GetSensorInternalResistance(); @@ -258,60 +192,9 @@ void HeaterController::Update(const ISampler& sampler, HeaterAllow heaterAllowSt duty = 0; heaterVoltage = 0; } + // Pipe the output to the heater driver - heaterPwm.SetDuty(pwm_ch, duty); - heaterVoltage = heaterVoltage; -} - -static THD_WORKING_AREA(waHeaterThread, 256); -static void HeaterThread(void*) -{ - int i; - - chRegSetThreadName("Heater"); - - // Wait for temperature sensing to stabilize so we don't - // immediately think we overshot the target temperature - chThdSleepMilliseconds(1000); - - // Get sensor type and settings - heater = getHeaterParams(GetSensorType()); - - while (true) - { - auto heaterAllowState = GetHeaterAllowed(); - - for (i = 0; i < AFR_CHANNELS; i++) { - const auto& sampler = GetSampler(i); - auto& heater = heaterControllers[i]; - - heater.Update(sampler, heaterAllowState); - } - - // Loop at ~20hz - chThdSleepMilliseconds(HEATER_CONTROL_PERIOD); - } -} - -void StartHeaterControl() -{ - heaterPwm.Start(heaterPwmConfig); - heaterPwm.SetDuty(heaterControllers[0].pwm_ch, 0); -#if (AFR_CHANNELS > 1) - heaterPwm.SetDuty(heaterControllers[1].pwm_ch, 0); -#endif - - chThdCreateStatic(waHeaterThread, sizeof(waHeaterThread), NORMALPRIO + 1, HeaterThread, nullptr); -} - -float GetHeaterDuty(int ch) -{ - return heaterPwm.GetLastDuty(heaterControllers[ch].pwm_ch); -} - -HeaterState GetHeaterState(int ch) -{ - return heaterControllers[ch].GetHeaterState(); + SetDuty(duty); } const char* describeHeaterState(HeaterState state) diff --git a/firmware/heater_control.h b/firmware/heater_control.h index 0340d4a..a07a1ed 100644 --- a/firmware/heater_control.h +++ b/firmware/heater_control.h @@ -26,17 +26,19 @@ struct IHeaterController virtual HeaterState GetHeaterState() const = 0; }; -class HeaterController : public IHeaterController +class HeaterControllerBase : public IHeaterController { public: - HeaterController(int ch, int pwm_ch); - + HeaterControllerBase(int ch); + void Configure(float targetTempC, float targetEsr); void Update(const ISampler& sampler, HeaterAllow heaterAllowState) override; bool IsRunningClosedLoop() const override; float GetHeaterEffectiveVoltage() const override; HeaterState GetHeaterState() const override; + virtual void SetDuty(float duty) const = 0; + protected: HeaterState GetNextState(HeaterAllow haeterAllowState, float batteryVoltage, float sensorTemp); float GetVoltageForState(float heaterEsr); @@ -60,10 +62,12 @@ private: int cycle; #endif + float m_targetEsr = 0; + float m_targetTempC = 0; + // TODO: private: public: const uint8_t ch; - const uint8_t pwm_ch; static const int preheatTimeCounter = HEATER_PREHEAT_TIME / HEATER_CONTROL_PERIOD; static const int batteryStabTimeCounter = HEATER_BATTERY_STAB_TIME / HEATER_CONTROL_PERIOD; diff --git a/firmware/heater_thread.cpp b/firmware/heater_thread.cpp new file mode 100644 index 0000000..19bab10 --- /dev/null +++ b/firmware/heater_thread.cpp @@ -0,0 +1,120 @@ +#include "ch.h" +#include "hal.h" +#include "pwm.h" + +#include "heater_control.h" +#include "port.h" +#include "sampling.h" + +// 400khz / 1024 = 390hz PWM +static Pwm heaterPwm(HEATER_PWM_DEVICE); +static const PWMConfig heaterPwmConfig = { + .frequency = 400'000, + .period = 1024, + .callback = nullptr, + .channels = { + {PWM_OUTPUT_ACTIVE_HIGH, nullptr}, + {PWM_OUTPUT_ACTIVE_HIGH, nullptr}, + {PWM_OUTPUT_ACTIVE_HIGH, nullptr}, + {PWM_OUTPUT_ACTIVE_HIGH, nullptr} + }, + .cr2 = 0, +#if STM32_PWM_USE_ADVANCED + .bdtr = 0, +#endif + .dier = 0 +}; + +class HeaterController : public HeaterControllerBase { +public: + HeaterController(int ch, int pwm_ch) + : HeaterControllerBase(ch) + , pwm_ch(pwm_ch) + { + } + + void SetDuty(float duty) const override + { + heaterPwm.SetDuty(pwm_ch, duty); + } + +// TODO: private: +public: + const uint8_t pwm_ch; +}; + +HeaterController heaterControllers[AFR_CHANNELS] = +{ + { 0, HEATER_PWM_CHANNEL_0 }, + +#if AFR_CHANNELS >= 2 + { 1, HEATER_PWM_CHANNEL_1 } +#endif +}; + +const IHeaterController& GetHeaterController(int ch) +{ + return heaterControllers[ch]; +} + +static THD_WORKING_AREA(waHeaterThread, 256); +static void HeaterThread(void*) +{ + int i; + + chRegSetThreadName("Heater"); + + // Wait for temperature sensing to stabilize so we don't + // immediately think we overshot the target temperature + chThdSleepMilliseconds(1000); + + // Configure heater controllers for sensor type + for (i = 0; i < AFR_CHANNELS; i++) { + auto& h = heaterControllers[i]; + switch (GetSensorType()) { + case SensorType::LSU42: + return h.Configure(730, 80); + case SensorType::LSUADV: + return h.Configure(785, 300); + case SensorType::LSU49: + default: + return h.Configure(780, 300); + } + } + + while (true) + { + auto heaterAllowState = GetHeaterAllowed(); + + for (i = 0; i < AFR_CHANNELS; i++) { + const auto& sampler = GetSampler(i); + auto& heater = heaterControllers[i]; + + heater.Update(sampler, heaterAllowState); + } + + // Loop at ~20hz + chThdSleepMilliseconds(HEATER_CONTROL_PERIOD); + } +} + +void StartHeaterControl() +{ + heaterPwm.Start(heaterPwmConfig); + heaterPwm.SetDuty(heaterControllers[0].pwm_ch, 0); +#if (AFR_CHANNELS > 1) + heaterPwm.SetDuty(heaterControllers[1].pwm_ch, 0); +#endif + + chThdCreateStatic(waHeaterThread, sizeof(waHeaterThread), NORMALPRIO + 1, HeaterThread, nullptr); +} + +float GetHeaterDuty(int ch) +{ + return heaterPwm.GetLastDuty(heaterControllers[ch].pwm_ch); +} + +HeaterState GetHeaterState(int ch) +{ + return heaterControllers[ch].GetHeaterState(); +} diff --git a/firmware/wideband.mk b/firmware/wideband.mk index eb7c74b..be91c75 100644 --- a/firmware/wideband.mk +++ b/firmware/wideband.mk @@ -1,2 +1,4 @@ WIDEBANDSRC = \ + $(FIRMWARE_DIR)/pid.cpp \ $(FIRMWARE_DIR)/sampling.cpp \ + $(FIRMWARE_DIR)/heater_control.cpp \ diff --git a/test/Makefile b/test/Makefile index 1f9b685..757c230 100644 --- a/test/Makefile +++ b/test/Makefile @@ -27,6 +27,7 @@ CPPSRC += \ gtest-all.cpp \ gmock-all.cpp \ gtest_main.cpp \ + test_stubs.cpp \ tests/test_sampler.cpp \ tests/test_heater.cpp \ diff --git a/test/test_stubs.cpp b/test/test_stubs.cpp new file mode 100644 index 0000000..659e0b0 --- /dev/null +++ b/test/test_stubs.cpp @@ -0,0 +1,11 @@ +#include "fault.h" +#include "can.h" + +void SetFault(int, wbo::Fault) +{ +} + +float GetRemoteBatteryVoltage() +{ + return 0; +} diff --git a/test/tests/test_heater.cpp b/test/tests/test_heater.cpp index 271a4e9..8d159e6 100644 --- a/test/tests/test_heater.cpp +++ b/test/tests/test_heater.cpp @@ -1,6 +1,15 @@ #include +#include + +#include "heater_control.h" + +struct MockHeater : public HeaterControllerBase { + MockHeater() : HeaterControllerBase(0) { } + + MOCK_METHOD(void, SetDuty, (float), (const, override)); +}; TEST(Heater, Basic) { - + MockHeater dut; }