Formalized and cleaned up heater state machine

Author: xtitoris

firmware/heater_control.cpp

@@ -7,6 +7,9 @@
 
 using namespace wbo;
 
+#define HEATER_OVERVOLTAGE_THRESHOLD 23.0f
+#define MAX_HEATER_VOLTAGE 12.0f
+
 static const PidConfig heaterPidConfig =
 {
     .kP = 0.3f,      // kP
@@ -26,11 +29,14 @@ void HeaterControllerBase::Configure(float targetTempC, float targetEsr, struct
     m_targetTempC = targetTempC;
     m_targetEsr = targetEsr;
     m_configuration = configuration;
+    m_retryTime = 0;
+
+    m_stateTimer.reset();
 
-    m_preheatTimer.reset();
-    m_warmupTimer.reset();
     m_heaterStableTimer.reset();
-    m_closedLoopStableTimer.reset();
+    m_undervoltTimer.reset();
+    m_underheatTimer.reset();
+    m_overheatTimer.reset();
 }
 
 bool HeaterControllerBase::IsRunningClosedLoop() const
@@ -53,87 +59,143 @@ HeaterState HeaterControllerBase::GetHeaterState() const
     return heaterState;
 }
 
+HeaterState HeaterControllerBase::changeState(HeaterState newState, Status status)
+{
+    m_stateTimer.reset();
+    SetStatus(ch, status);
+    return newState;
+}
+
+HeaterState HeaterControllerBase::stopWithRetry(Status status, int retryTimeSec)
+{
+    m_retryTime = retryTimeSec;
+    return changeState(HeaterState::Stopped, status);
+}
+
 HeaterState HeaterControllerBase::GetNextState(HeaterState currentState, HeaterAllow heaterAllowState, float heaterSupplyVoltage, float sensorTemp)
 {
-    bool heaterAllowed = heaterAllowState == HeaterAllow::Allowed;
+    const float overheatTemp = m_targetTempC + 100;
+    const float closedLoopTemp = m_targetTempC - 30;
+    const float underheatTemp = m_targetTempC - 100;
+
+    // Common checks that apply to all running states
+    // This is here to avoid repeating these checks in every state - if any of these conditions are met, we want to stop heating immediately
+    // There should be no code outside of specific states otherwise to keep the state machine logic clear and maintainable
 
-    // Check battery voltage for thresholds only if there is still no command over CAN
-    if (heaterAllowState == HeaterAllow::Unknown)
+    if (heaterSupplyVoltage > HEATER_OVERVOLTAGE_THRESHOLD)
     {
-        // measured voltage too low to auto-start heating
-        if (heaterSupplyVoltage < m_configuration->HeaterSupplyOffVoltage)
-        {
-            m_heaterStableTimer.reset();
-        }
-        else if (heaterSupplyVoltage > m_configuration->HeaterSupplyOnVoltage)
-        {
-            // measured voltage is high enougth to auto-start heating, wait some time to stabilize
-            heaterAllowed = m_heaterStableTimer.hasElapsedSec(HEATER_BATTERY_STAB_TIME);
-        }
+        // If voltage is dangerously high, immediately stop heating to protect the sensor
+        return stopWithRetry(Status::SensorOvervoltage, HEATER_OVERVOLT_RETRY_TIMEOUT);
     }
 
-    if (!heaterAllowed)
+    // If voltage is back to normal, reset undervoltage timer
+    if (heaterSupplyVoltage > m_configuration->HeaterSupplyOffVoltage)
     {
-        // ECU hasn't allowed preheat yet, reset timer, and force preheat state
-        m_preheatTimer.reset();
-        SetStatus(ch, Status::Preheat);
-        return HeaterState::Preheat;
+        m_undervoltTimer.reset();
     }
 
-    float overheatTemp = m_targetTempC + 100;
-    float closedLoopTemp = m_targetTempC - 30;
-    float underheatTemp = m_targetTempC - 100;
+    // Voltage has been too low for too long, stop heating and set fault
+    if (m_undervoltTimer.hasElapsedSec(0.5f))
+    {
+        return stopWithRetry(Status::SensorUndervoltage, HEATER_UNDERVOLT_RETRY_TIMEOUT);
+    }
 
     switch (currentState)
     {
+        case HeaterState::Stopped:
+        {
+            // If retry timer is running, wait until it elapses before allowing to start preheat again
+            if ((m_retryTime) && !(m_stateTimer.hasElapsedSec(m_retryTime)))
+            {
+                break;
+            }
+            // Disable retry timer
+            m_retryTime = 0;
+
+            if (heaterAllowState == HeaterAllow::NotAllowed)
+            {
+                // ECU has explicitly disallowed heating, stay stopped
+                break;
+            }
+
+            // If we haven't received any CAN message about whether heating is allowed or not
+            // we should wait until heater supply voltage stabilizes above threshold before allowing to start heating
+            if (heaterAllowState == HeaterAllow::Unknown)
+            {
+                // Reset voltage_stable timer if voltage is too low
+                if (heaterSupplyVoltage < m_configuration->HeaterSupplyOnVoltage)
+                {
+                    m_heaterStableTimer.reset();
+                }
+
+                if (!m_heaterStableTimer.hasElapsedSec(HEATER_VOLTAGE_STAB_TIME))
+                {
+                    // Continue to wait for voltage to stabilize before allowing heating
+                    break;
+                }
+            }
+
+            // Otherwise, start preheat
+            return changeState(HeaterState::Preheat, Status::Preheat);
+            break;
+        }
         case HeaterState::Preheat:
-            #ifdef HEATER_FAST_HEATING_THRESHOLD_T
-            if (sensorTemp >= HEATER_FAST_HEATING_THRESHOLD_T) {
-                // if sensor is already hot - we can start from higher heater voltage
-                rampVoltage = 9;
+        {
+            bool startRamp = false;
 
-                // Reset the timer for the warmup phase
-                m_warmupTimer.reset();
+            rampVoltage = 7.0f;
 
-                SetStatus(ch, Status::Warmup);
-                return HeaterState::WarmupRamp;
+            #ifdef HEATER_FAST_HEATING_THRESHOLD_T
+            // If the sensor is already above a certain temperature, skip preheat and go straight to warmup ramp
+            if (sensorTemp > HEATER_FAST_HEATING_THRESHOLD_T)
+            {
+                // Start ramp at higher voltage to speed up heating for cases like hot restarts where sensor is already warm
+                rampVoltage = 9.0f;
+                startRamp = true;
             }
+
             #endif
 
-            // If preheat timeout, or sensor is already hot (engine running?)
-            if (m_preheatTimer.hasElapsedSec(m_configuration->PreheatTimeSec) || sensorTemp > closedLoopTemp)
+            // If the sensor is already hot (engine running?), skip preheat and go straight to warmup ramp
+            if (sensorTemp > closedLoopTemp)
             {
-                // If enough time has elapsed, start the ramp
-                // Start the ramp at 7 volts
-                rampVoltage = 7;
+                startRamp = true;
+            }
 
-                // Reset the timer for the warmup phase
-                m_warmupTimer.reset();
+            // If enough time has elapsed in preheat (condensation phase has passed), start warmup ramp
+            if (m_stateTimer.hasElapsedSec(m_configuration->PreheatTimeSec))
+            {
+                startRamp = true;
+            }
 
-                SetStatus(ch, Status::Warmup);
-                return HeaterState::WarmupRamp;
+            if (startRamp)
+            {
+                // Reset the timer for the warmup phase
+                return changeState(HeaterState::WarmupRamp, Status::Warmup);
             }
 
-            // Stay in preheat - wait for time to elapse
+            // Stay in preheat
             break;
+        }
         case HeaterState::WarmupRamp:
+        {
+            // Already hot enough, start closed loop
             if (sensorTemp > closedLoopTemp)
             {
-                m_closedLoopStableTimer.reset();
-                SetStatus(ch, Status::RunningClosedLoop);
-                return HeaterState::ClosedLoop;
+                return changeState(HeaterState::ClosedLoop, Status::RunningClosedLoop);
             }
-            else if (m_warmupTimer.hasElapsedSec(HEATER_WARMUP_TIMEOUT))
+
+            // If we've been trying to warm up for too long without reaching the closed loop threshold,
+            // something is wrong (like heater not working), so stop and set fault
+            if (m_stateTimer.hasElapsedSec(HEATER_WARMUP_TIMEOUT))
             {
-                SetStatus(ch, Status::SensorDidntHeat);
-                // retry after timeout
-                m_retryTime = HEATER_DIDNOTHEAT_RETRY_TIMEOUT;
-                m_retryTimer.reset();
-                return HeaterState::Stopped;
+                return stopWithRetry(Status::SensorDidntHeat, HEATER_DIDNOTHEAT_RETRY_TIMEOUT);
             }
 
             break;
+        }
         case HeaterState::ClosedLoop:
+        {
             // Over/under heat timers track how long it's been since
             // temperature was within normal range (then we abort if
             // it's been too long out of range)
@@ -147,47 +209,46 @@ HeaterState HeaterControllerBase::GetNextState(HeaterState currentState, HeaterA
                 m_underheatTimer.reset();
             }
 
-            if (m_closedLoopStableTimer.hasElapsedSec(HEATER_CLOSED_LOOP_STAB_TIME)) {
-                if (m_overheatTimer.hasElapsedSec(0.5f))
-                {
-                    SetStatus(ch, Status::SensorOverheat);
-                    // retry after timeout
-                    m_retryTime = HEATER_OVERHEAT_RETRY_TIMEOUT;
-                    m_retryTimer.reset();
-                    return HeaterState::Stopped;
-                }
-                else if (m_underheatTimer.hasElapsedSec(0.5f))
-                {
-                    SetStatus(ch, Status::SensorUnderheat);
-                    // retry after timeout
-                    m_retryTime = HEATER_UNDERHEAT_RETRY_TIMEOUT;
-                    m_retryTimer.reset();
-                    return HeaterState::Stopped;
-                }
-            } else {
+            // Check for overheat befor checking for closed loop stable time
+            // if we're overheating, we want to stop as soon as possible, even if we haven't been in closed loop for very long
+            if (m_overheatTimer.hasElapsedSec(0.5f))
+            {
+                return stopWithRetry(Status::SensorOverheat, HEATER_OVERHEAT_RETRY_TIMEOUT);
+            }
+
+            
+            if (!m_stateTimer.hasElapsedSec(HEATER_CLOSED_LOOP_STAB_TIME))
+            {
                 // give some time for stabilization...
                 // looks like heavy ramped Ipump affects sensorTemp measure
                 // and right after switch to closed loop sensorTemp drops below underhead threshold
+                break;
             }
 
-            break;
-        case HeaterState::Stopped:
-            if ((m_retryTime) && (m_retryTimer.hasElapsedSec(m_retryTime))) {
-                return HeaterState::Preheat;
+
+            else if (m_underheatTimer.hasElapsedSec(0.5f))
+            {
+                return stopWithRetry(Status::SensorUnderheat, HEATER_UNDERHEAT_RETRY_TIMEOUT);
             }
+
             break;
+        }
     }
 
     return currentState;
 }
 
 float HeaterControllerBase::GetVoltageForState(HeaterState state, float sensorEsr)
 {
+    float heaterVoltage = 0;
+
     switch (state)
     {
         case HeaterState::Preheat:
             // Max allowed during condensation phase (preheat) is 2v
-            return 2.0f;
+            heaterVoltage = 2.0f;
+            break;
+
         case HeaterState::WarmupRamp:
             if (rampVoltage < 12)
             {
@@ -197,20 +258,29 @@ float HeaterControllerBase::GetVoltageForState(HeaterState state, float sensorEs
                 rampVoltage += (rampRateVoltPerSecond / heaterFrequency);
             }
 
-            return rampVoltage;
+            heaterVoltage = rampVoltage;
+            break;
+
         case HeaterState::ClosedLoop:
             // "nominal" heater voltage is 7.5v, so apply correction around that point (instead of relying on integrator so much)
             // Negated because lower resistance -> hotter
 
             // TODO: heater PID should operate on temperature, not ESR
-            return 7.5f - m_pid.GetOutput(m_targetEsr, sensorEsr);
+            heaterVoltage = 7.5f - m_pid.GetOutput(m_targetEsr, sensorEsr);
+            break;
+
         case HeaterState::Stopped:
             // Something has gone wrong, turn off the heater.
-            return 0;
+            heaterVoltage = 0;
+            break;
     }
 
-    // should be unreachable
-    return 0;
+    // Limit to MAX_HEATER_VOLTAGE as per specification
+    if (heaterVoltage > MAX_HEATER_VOLTAGE) {
+        heaterVoltage = MAX_HEATER_VOLTAGE;
+    }
+
+    return heaterVoltage;
 }
 
 void HeaterControllerBase::Update(const ISampler& sampler, HeaterAllow heaterAllowState)
@@ -219,6 +289,9 @@ void HeaterControllerBase::Update(const ISampler& sampler, HeaterAllow heaterAll
     float sensorEsr = sampler.GetSensorInternalResistance();
     float sensorTemperature = sampler.GetSensorTemperature();
 
+    // TODO: Clean this up, looks like a mess
+    // Move supply voltage reading logic into port so that it's cleaner and more testable
+    // Also unify voltage reading logic across the codebase, so that everyting is consistent
     #if defined(HEATER_INPUT_DIVIDER)
         // if board has ability to measure heater supply localy - use it
         float heaterSupplyVoltage = sampler.GetInternalHeaterVoltage();
@@ -231,37 +304,25 @@ void HeaterControllerBase::Update(const ISampler& sampler, HeaterAllow heaterAll
 
     // Run the state machine
     heaterState = GetNextState(heaterState, heaterAllowState, heaterSupplyVoltage, sensorTemperature);
-    float heaterVoltage = GetVoltageForState(heaterState, sensorEsr);
+    heaterVoltage = GetVoltageForState(heaterState, sensorEsr);
 
-    // Limit to 12 volts
-    if (heaterVoltage > 12) {
-        heaterVoltage = 12;
-    }
-
-    // Very low supply voltage -> avoid divide by zero or very high duty
-    if (heaterSupplyVoltage < 3) {
-        heaterSupplyVoltage = 12;
-    }
-
-    // duty = (V_eff / V_batt) ^ 2
-    float voltageRatio = (heaterSupplyVoltage < 1.0f) ? 0 : heaterVoltage / heaterSupplyVoltage;
-    float duty = voltageRatio * voltageRatio;
-
-    #ifdef HEATER_MAX_DUTY
-    cycle++;
-    // limit PWM each 10th cycle (2 time per second) to measure heater supply voltage throuth "Heater-"
-    if ((cycle % 10) == 0) {
-        if (duty > HEATER_MAX_DUTY) {
-            duty = HEATER_MAX_DUTY;
-        }
-    }
-    #endif
+    float duty = 0;
 
-    // Protect the sensor in case of very high voltage
-    if (heaterSupplyVoltage >= 23)
+    if (heaterVoltage > 0)
     {
-        duty = 0;
-        heaterVoltage = 0;
+        // duty = (V_eff / V_batt) ^ 2
+        float voltageRatio = heaterVoltage / heaterSupplyVoltage;
+        duty = voltageRatio * voltageRatio;
+
+        #ifdef HEATER_MAX_DUTY
+        cycle++;
+        // limit PWM each 10th cycle (2 time per second) to measure heater supply voltage through "Heater-"
+        if ((cycle % 10) == 0) {
+            if (duty > HEATER_MAX_DUTY) {
+                duty = HEATER_MAX_DUTY;
+            }
+        }
+        #endif
     }
 
     // Pipe the output to the heater driver