From 17acd065d3f16b9703da7539dc65db606b725d57 Mon Sep 17 00:00:00 2001 From: Matthew Kennedy Date: Wed, 15 Nov 2023 17:42:37 -0800 Subject: [PATCH] cherry-pick timer from ECU and use it for sequencing logic (#289) * enable timestamp api * add timer class * use unsigned integer for timestamps * heater uses timers * timer mocking and test adjustment --- firmware/boards/f0_module/chconf.h | 2 +- firmware/boards/f1_dual/chconf.h | 2 +- firmware/boards/f1_dual_rev1/chconf.h | 2 +- firmware/boards/f1_rev2/chconf.h | 2 +- firmware/boards/f1_rev3/chconf.h | 2 +- firmware/heater_control.cpp | 21 ++--- firmware/heater_control.h | 18 ++--- firmware/heater_thread.cpp | 2 +- firmware/util/timer.cpp | 110 ++++++++++++++++++++++++++ firmware/util/timer.h | 41 ++++++++++ firmware/wideband.mk | 1 + firmware/wideband_config.h | 4 +- test/Makefile | 3 + test/tests/test_heater.cpp | 33 +++++--- 14 files changed, 203 insertions(+), 40 deletions(-) create mode 100644 firmware/util/timer.cpp create mode 100644 firmware/util/timer.h diff --git a/firmware/boards/f0_module/chconf.h b/firmware/boards/f0_module/chconf.h index 1c18933..8e1fb71 100644 --- a/firmware/boards/f0_module/chconf.h +++ b/firmware/boards/f0_module/chconf.h @@ -173,7 +173,7 @@ typedef int pid_t; * @note The default is @p TRUE. */ #if !defined(CH_CFG_USE_TIMESTAMP) -#define CH_CFG_USE_TIMESTAMP FALSE +#define CH_CFG_USE_TIMESTAMP TRUE #endif /** diff --git a/firmware/boards/f1_dual/chconf.h b/firmware/boards/f1_dual/chconf.h index 9229796..8ffc63c 100644 --- a/firmware/boards/f1_dual/chconf.h +++ b/firmware/boards/f1_dual/chconf.h @@ -173,7 +173,7 @@ typedef int pid_t; * @note The default is @p TRUE. */ #if !defined(CH_CFG_USE_TIMESTAMP) -#define CH_CFG_USE_TIMESTAMP FALSE +#define CH_CFG_USE_TIMESTAMP TRUE #endif /** diff --git a/firmware/boards/f1_dual_rev1/chconf.h b/firmware/boards/f1_dual_rev1/chconf.h index 9229796..8ffc63c 100644 --- a/firmware/boards/f1_dual_rev1/chconf.h +++ b/firmware/boards/f1_dual_rev1/chconf.h @@ -173,7 +173,7 @@ typedef int pid_t; * @note The default is @p TRUE. */ #if !defined(CH_CFG_USE_TIMESTAMP) -#define CH_CFG_USE_TIMESTAMP FALSE +#define CH_CFG_USE_TIMESTAMP TRUE #endif /** diff --git a/firmware/boards/f1_rev2/chconf.h b/firmware/boards/f1_rev2/chconf.h index 204d457..32dddc8 100644 --- a/firmware/boards/f1_rev2/chconf.h +++ b/firmware/boards/f1_rev2/chconf.h @@ -173,7 +173,7 @@ typedef int pid_t; * @note The default is @p TRUE. */ #if !defined(CH_CFG_USE_TIMESTAMP) -#define CH_CFG_USE_TIMESTAMP FALSE +#define CH_CFG_USE_TIMESTAMP TRUE #endif /** diff --git a/firmware/boards/f1_rev3/chconf.h b/firmware/boards/f1_rev3/chconf.h index abea810..5e6a781 100644 --- a/firmware/boards/f1_rev3/chconf.h +++ b/firmware/boards/f1_rev3/chconf.h @@ -173,7 +173,7 @@ typedef int pid_t; * @note The default is @p TRUE. */ #if !defined(CH_CFG_USE_TIMESTAMP) -#define CH_CFG_USE_TIMESTAMP FALSE +#define CH_CFG_USE_TIMESTAMP TRUE #endif /** diff --git a/firmware/heater_control.cpp b/firmware/heater_control.cpp index eec4523..36ded4e 100644 --- a/firmware/heater_control.cpp +++ b/firmware/heater_control.cpp @@ -5,8 +5,10 @@ using namespace wbo; -HeaterControllerBase::HeaterControllerBase(int ch) +HeaterControllerBase::HeaterControllerBase(int ch, int preheatTimeSec, int warmupTimeSec) : ch(ch) + , m_preheatTimeSec(preheatTimeSec) + , m_warmupTimeSec(warmupTimeSec) { } @@ -14,6 +16,9 @@ void HeaterControllerBase::Configure(float targetTempC, float targetEsr) { m_targetTempC = targetTempC; m_targetEsr = targetEsr; + + m_preheatTimer.reset(); + m_warmupTimer.reset(); } bool HeaterControllerBase::IsRunningClosedLoop() const @@ -55,7 +60,7 @@ HeaterState HeaterControllerBase::GetNextState(HeaterState currentState, HeaterA if (!heaterAllowed) { // ECU hasn't allowed preheat yet, reset timer, and force preheat state - timeCounter = preheatTimeCounter; + m_preheatTimer.reset(); return HeaterState::Preheat; } @@ -66,17 +71,15 @@ HeaterState HeaterControllerBase::GetNextState(HeaterState currentState, HeaterA switch (currentState) { case HeaterState::Preheat: - timeCounter--; - // If preheat timeout, or sensor is already hot (engine running?) - if (timeCounter <= 0 || sensorTemp > closedLoopTemp) + if (m_preheatTimer.hasElapsedSec(m_preheatTimeSec) || sensorTemp > closedLoopTemp) { // If enough time has elapsed, start the ramp // Start the ramp at 4 volts rampVoltage = 4; - // Next phase times out at 15 seconds - timeCounter = HEATER_WARMUP_TIMEOUT / HEATER_CONTROL_PERIOD; + // Reset the timer for the warmup phase + m_warmupTimer.reset(); return HeaterState::WarmupRamp; } @@ -84,13 +87,11 @@ HeaterState HeaterControllerBase::GetNextState(HeaterState currentState, HeaterA // Stay in preheat - wait for time to elapse break; case HeaterState::WarmupRamp: - timeCounter--; - if (sensorTemp > closedLoopTemp) { return HeaterState::ClosedLoop; } - else if (timeCounter == 0) + else if (m_warmupTimer.hasElapsedSec(m_warmupTimeSec)) { SetFault(ch, Fault::SensorDidntHeat); return HeaterState::Stopped; diff --git a/firmware/heater_control.h b/firmware/heater_control.h index 9ae3455..78540ba 100644 --- a/firmware/heater_control.h +++ b/firmware/heater_control.h @@ -6,6 +6,7 @@ #include "can.h" #include "pid.h" +#include "timer.h" enum class HeaterState { @@ -29,7 +30,7 @@ struct IHeaterController class HeaterControllerBase : public IHeaterController { public: - HeaterControllerBase(int ch); + HeaterControllerBase(int ch, int preheatTimeSec, int warmupTimeSec); void Configure(float targetTempC, float targetEsr); void Update(const ISampler& sampler, HeaterAllow heaterAllowState) override; @@ -42,11 +43,6 @@ public: HeaterState GetNextState(HeaterState currentState, HeaterAllow haeterAllowState, float batteryVoltage, float sensorTemp); float GetVoltageForState(HeaterState state, float sensorEsr); - int GetTimeCounter() const - { - return timeCounter; - } - private: Pid heaterPid = { @@ -57,7 +53,6 @@ private: HEATER_CONTROL_PERIOD }; - int timeCounter = preheatTimeCounter; int batteryStabTime = batteryStabTimeCounter; float rampVoltage = 0; float heaterVoltage = 0; @@ -69,11 +64,14 @@ private: float m_targetEsr = 0; float m_targetTempC = 0; -// TODO: private: -public: const uint8_t ch; - static const int preheatTimeCounter = HEATER_PREHEAT_TIME / HEATER_CONTROL_PERIOD; + const int m_preheatTimeSec; + const int m_warmupTimeSec; + + Timer m_preheatTimer; + Timer m_warmupTimer; + static const int batteryStabTimeCounter = HEATER_BATTERY_STAB_TIME / HEATER_CONTROL_PERIOD; }; diff --git a/firmware/heater_thread.cpp b/firmware/heater_thread.cpp index 19bab10..084abe0 100644 --- a/firmware/heater_thread.cpp +++ b/firmware/heater_thread.cpp @@ -28,7 +28,7 @@ static const PWMConfig heaterPwmConfig = { class HeaterController : public HeaterControllerBase { public: HeaterController(int ch, int pwm_ch) - : HeaterControllerBase(ch) + : HeaterControllerBase(ch, HEATER_PREHEAT_TIME, HEATER_WARMUP_TIMEOUT) , pwm_ch(pwm_ch) { } diff --git a/firmware/util/timer.cpp b/firmware/util/timer.cpp new file mode 100644 index 0000000..b30ab89 --- /dev/null +++ b/firmware/util/timer.cpp @@ -0,0 +1,110 @@ +#include +#include "timer.h" + +#define US_PER_SECOND_F 1000000.0 + +Timer::Timer() { + init(); +} + +#ifdef MOCK_TIMER + +// in mock land, ticks == microseconds +#define TIME_US2I(us) (us) +#define TIME_I2US(ticks) (ticks) + +/*static*/ int64_t Timer::mockTimeStamp = 0; +int64_t Timer::getTimestamp() const { + return Timer::mockTimeStamp; +} + +/*static*/ void Timer::setMockTime(int64_t stamp) { + Timer::mockTimeStamp = stamp; +} + +#else +#include "ch.hpp" + +int64_t Timer::getTimestamp() const { + // Ensure that our timestamp type is compatible with the one ChibiOS returns + static_assert(sizeof(int64_t) == sizeof(systimestamp_t)); + + return chVTGetTimeStamp(); +} +#endif // MOCK_TIMER + +void Timer::reset() { + reset(getTimestamp()); +} + +void Timer::reset(int64_t stamp) { + m_lastReset = stamp; +} + +void Timer::init() { + // Use not-quite-minimum value to avoid overflow + m_lastReset = INT64_MIN / 8; +} + +bool Timer::hasElapsedSec(float seconds) const { + return hasElapsedMs(seconds * 1000); +} + +bool Timer::hasElapsedMs(float milliseconds) const { + return hasElapsedUs(milliseconds * 1000); +} + +bool Timer::hasElapsedUs(float microseconds) const { + auto delta = getTimestamp() - m_lastReset; + + // If larger than 32 bits, timer has certainly expired + if (delta >= UINT32_MAX) { + return true; + } + + auto delta32 = (uint32_t)delta; + + return delta32 > TIME_US2I(microseconds); +} + + +float Timer::getElapsedSeconds() const { + return getElapsedSeconds(getTimestamp()); +} + +float Timer::getElapsedSeconds(int64_t stamp) const { + return 1 / US_PER_SECOND_F * getElapsedUs(stamp); +} + +float Timer::getElapsedUs() const { + return getElapsedUs(getTimestamp()); +} + +float Timer::getElapsedUs(int64_t stamp) const { + auto deltaNt = stamp - m_lastReset; + + // Yes, things can happen slightly in the future if we get a lucky interrupt between + // the timestamp and this subtraction, that updates m_lastReset to what's now "the future", + // resulting in a negative delta. + if (deltaNt < 0) { + return 0; + } + + if (deltaNt > UINT32_MAX - 1) { + deltaNt = UINT32_MAX - 1; + } + + auto delta32 = (uint32_t)deltaNt; + + return TIME_I2US(delta32); +} + +float Timer::getElapsedSecondsAndReset() { + auto stamp = getTimestamp(); + + float result = getElapsedSeconds(stamp); + + reset(stamp); + + return result; +} diff --git a/firmware/util/timer.h b/firmware/util/timer.h new file mode 100644 index 0000000..2aeb258 --- /dev/null +++ b/firmware/util/timer.h @@ -0,0 +1,41 @@ +#pragma once + +/** + * Helper class with "has X amount of time elapsed since most recent reset" methods + * Brand new instances have most recent reset time far in the past, i.e. "hasElapsed" is true for any reasonable range + */ +class Timer final { +public: + Timer(); + // returns timer to the most original-as-constructed state + void init(); + + void reset(); + + bool hasElapsedSec(float seconds) const; + bool hasElapsedMs(float ms) const; + bool hasElapsedUs(float us) const; + + // Return the elapsed time since the last reset. + // If the elapsed time is longer than 2^32 timer tick counts, + // then a time period representing 2^32 counts will be returned. + float getElapsedSeconds() const; + float getElapsedUs() const; + + // Perform an atomic update and returning the delta between + // now and the last reset + float getElapsedSecondsAndReset(); + + static void setMockTime(int64_t stamp); + +private: + int64_t getTimestamp() const; + + void reset(int64_t stamp); + float getElapsedSeconds(int64_t stamp) const; + float getElapsedUs(int64_t stamp) const; + + int64_t m_lastReset; + + static int64_t mockTimeStamp; +}; diff --git a/firmware/wideband.mk b/firmware/wideband.mk index be91c75..5996976 100644 --- a/firmware/wideband.mk +++ b/firmware/wideband.mk @@ -2,3 +2,4 @@ WIDEBANDSRC = \ $(FIRMWARE_DIR)/pid.cpp \ $(FIRMWARE_DIR)/sampling.cpp \ $(FIRMWARE_DIR)/heater_control.cpp \ + $(FIRMWARE_DIR)/util/timer.cpp \ diff --git a/firmware/wideband_config.h b/firmware/wideband_config.h index 0ca79a3..ea59590 100644 --- a/firmware/wideband_config.h +++ b/firmware/wideband_config.h @@ -40,8 +40,8 @@ // ******************************* #define HEATER_CONTROL_PERIOD 50 -#define HEATER_PREHEAT_TIME 5000 -#define HEATER_WARMUP_TIMEOUT 60000 +#define HEATER_PREHEAT_TIME 5 +#define HEATER_WARMUP_TIMEOUT 60 #define HEATER_BATTERY_STAB_TIME 500 // minimal battery voltage to start heating without CAN command diff --git a/test/Makefile b/test/Makefile index 757c230..327dc55 100644 --- a/test/Makefile +++ b/test/Makefile @@ -39,6 +39,7 @@ INCDIR += \ $(RUSEFI_LIB_INC) \ $(FIRMWARE_DIR) \ $(FIRMWARE_DIR)/boards \ + $(FIRMWARE_DIR)/util \ # User may want to pass in a forced value for SANITIZE ifeq ($(SANITIZE),) @@ -77,6 +78,8 @@ ifeq ($(USE_CPPOPT),) USE_CPPOPT = -std=c++17 -fno-rtti -fno-use-cxa-atexit endif +USE_CPPOPT += -DMOCK_TIMER + # Enable address sanitizer for C++ files, but not on Windows since x86_64-w64-mingw32-g++ doesn't support it. # only c++ because lua does some things asan doesn't like, but don't actually cause overruns. ifeq ($(SANITIZE),yes) diff --git a/test/tests/test_heater.cpp b/test/tests/test_heater.cpp index c52321f..d3ad703 100644 --- a/test/tests/test_heater.cpp +++ b/test/tests/test_heater.cpp @@ -5,7 +5,7 @@ struct MockHeater : public HeaterControllerBase { - MockHeater() : HeaterControllerBase(0) { } + MockHeater() : HeaterControllerBase(0, 5, 10) { } MOCK_METHOD(void, SetDuty, (float), (const, override)); }; @@ -55,20 +55,26 @@ TEST(HeaterStateOutput, Cases) TEST(HeaterStateMachine, PreheatToWarmupTimeout) { MockHeater dut; + Timer::setMockTime(0); dut.Configure(780, 300); - for (size_t i = 0; i < HeaterControllerBase::preheatTimeCounter - 1; i++) - { - EXPECT_EQ(HeaterState::Preheat, dut.GetNextState(HeaterState::Preheat, HeaterAllow::Allowed, 12, 500)); - } + // For a while it should stay in preheat + Timer::setMockTime(1e6); + EXPECT_EQ(HeaterState::Preheat, dut.GetNextState(HeaterState::Preheat, HeaterAllow::Allowed, 12, 500)); + Timer::setMockTime(2e6); + EXPECT_EQ(HeaterState::Preheat, dut.GetNextState(HeaterState::Preheat, HeaterAllow::Allowed, 12, 500)); + Timer::setMockTime(4.9e6); + EXPECT_EQ(HeaterState::Preheat, dut.GetNextState(HeaterState::Preheat, HeaterAllow::Allowed, 12, 500)); // Timer expired, transition to warmup ramp + Timer::setMockTime(5.1e6); EXPECT_EQ(HeaterState::WarmupRamp, dut.GetNextState(HeaterState::Preheat, HeaterAllow::Allowed, 12, 500)); } TEST(HeaterStateMachine, PreheatToWarmupAlreadyWarm) { MockHeater dut; + Timer::setMockTime(0); dut.Configure(780, 300); // Preheat for a little while @@ -84,6 +90,7 @@ TEST(HeaterStateMachine, PreheatToWarmupAlreadyWarm) TEST(HeaterStateMachine, WarmupToClosedLoop) { MockHeater dut; + Timer::setMockTime(0); dut.Configure(780, 300); // Warm up for a little while @@ -99,17 +106,19 @@ TEST(HeaterStateMachine, WarmupToClosedLoop) TEST(HeaterStateMachine, WarmupTimeout) { MockHeater dut; + Timer::setMockTime(0); dut.Configure(780, 300); - size_t timeoutPeriod = dut.GetTimeCounter(); - - // Warm up for a little while - for (size_t i = 0; i < timeoutPeriod - 1; i++) - { - EXPECT_EQ(HeaterState::WarmupRamp, dut.GetNextState(HeaterState::WarmupRamp, HeaterAllow::Allowed, 12, 500)) << "i = " << i; - } + // For a while it should stay in warmup + Timer::setMockTime(1e6); + EXPECT_EQ(HeaterState::WarmupRamp, dut.GetNextState(HeaterState::WarmupRamp, HeaterAllow::Allowed, 12, 500)); + Timer::setMockTime(2e6); + EXPECT_EQ(HeaterState::WarmupRamp, dut.GetNextState(HeaterState::WarmupRamp, HeaterAllow::Allowed, 12, 500)); + Timer::setMockTime(9.9e6); + EXPECT_EQ(HeaterState::WarmupRamp, dut.GetNextState(HeaterState::WarmupRamp, HeaterAllow::Allowed, 12, 500)); // Warmup times out, sensor transitions to stopped + Timer::setMockTime(10.1e6); EXPECT_EQ(HeaterState::Stopped, dut.GetNextState(HeaterState::WarmupRamp, HeaterAllow::Allowed, 12, 500)); }