diff --git a/firmware/heater_control.cpp b/firmware/heater_control.cpp index b1f6689..2753cc5 100644 --- a/firmware/heater_control.cpp +++ b/firmware/heater_control.cpp @@ -117,12 +117,22 @@ HeaterState HeaterControllerBase::GetNextState(HeaterState currentState, HeaterA break; case HeaterState::ClosedLoop: // Check that the sensor's ESR is acceptable for normal operation - if (sensorTemp > overheatTemp) + if (sensorTemp <= overheatTemp) + { + m_overheatTimer.reset(); + } + + if (sensorTemp >= underheatTemp) + { + m_underheatTimer.reset(); + } + + if (m_overheatTimer.hasElapsedSec(0.5f)) { SetFault(ch, Fault::SensorOverheat); return HeaterState::Stopped; } - else if (sensorTemp < underheatTemp) + else if (m_underheatTimer.hasElapsedSec(0.5f)) { SetFault(ch, Fault::SensorUnderheat); return HeaterState::Stopped; diff --git a/firmware/heater_control.h b/firmware/heater_control.h index 02149a2..4b7a017 100644 --- a/firmware/heater_control.h +++ b/firmware/heater_control.h @@ -76,6 +76,12 @@ private: Timer m_preheatTimer; Timer m_warmupTimer; + // Stores the time since a non-over/underheat condition + // If the timer reaches a threshold, an over/underheat has + // occured + Timer m_underheatTimer; + Timer m_overheatTimer; + static const int batteryStabTimeCounter = HEATER_BATTERY_STAB_TIME / HEATER_CONTROL_PERIOD; };