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

@@ -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
 
 /**

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
 
 /**

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
 
 /**

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
 
 /**

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
 
 /**

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;

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;
 };
 

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)
     {
     }

firmware/util/timer.cpp

@@ -0,0 +1,110 @@
+#include <cstdint>
+#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;
+}

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;
+};

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 \

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

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)

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++)
-    {
+    // 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));
 }