wideband/firmware/heater_control.cpp

217 lines
6.4 KiB
C++
Raw Normal View History

2020-10-30 01:53:54 -07:00
#include "heater_control.h"
2020-12-10 18:32:41 -08:00
#include "fault.h"
2020-10-30 01:53:54 -07:00
#include "sampling.h"
2022-01-01 21:10:55 -08:00
using namespace wbo;
2023-11-13 13:30:23 -08:00
HeaterControllerBase::HeaterControllerBase(int ch)
: ch(ch)
{
2023-11-13 12:55:48 -08:00
}
2023-11-13 13:30:23 -08:00
void HeaterControllerBase::Configure(float targetTempC, float targetEsr)
2023-11-13 12:55:48 -08:00
{
2023-11-13 13:30:23 -08:00
m_targetTempC = targetTempC;
m_targetEsr = targetEsr;
2023-11-13 12:55:48 -08:00
}
2023-06-23 13:50:01 -07:00
2023-11-13 13:30:23 -08:00
bool HeaterControllerBase::IsRunningClosedLoop() const
2023-11-13 12:55:48 -08:00
{
2023-11-13 13:30:23 -08:00
return heaterState == HeaterState::ClosedLoop;
2023-11-13 12:55:48 -08:00
}
2023-11-13 13:30:23 -08:00
float HeaterControllerBase::GetHeaterEffectiveVoltage() const
2023-11-13 12:55:48 -08:00
{
2023-11-13 13:30:23 -08:00
return heaterVoltage;
2023-11-13 12:55:48 -08:00
}
2023-06-23 13:50:01 -07:00
2023-11-13 13:30:23 -08:00
HeaterState HeaterControllerBase::GetHeaterState() const
2023-06-23 13:50:01 -07:00
{
2023-11-13 13:30:23 -08:00
return heaterState;
}
2023-11-13 13:56:03 -08:00
HeaterState HeaterControllerBase::GetNextState(HeaterState currentState, HeaterAllow heaterAllowState, float batteryVoltage, float sensorTemp)
2020-10-30 02:40:18 -07:00
{
bool heaterAllowed = heaterAllowState == HeaterAllow::Allowed;
// Check battery voltage for thresholds only if there is still no command over CAN
if (heaterAllowState == HeaterAllow::Unknown)
{
// measured voltage too low to auto-start heating
if (batteryVoltage < HEATER_BATTETY_OFF_VOLTAGE)
{
2023-06-23 13:50:01 -07:00
batteryStabTime = batteryStabTimeCounter;
}
// measured voltage is high enougth to auto-start heating, wait some time to stabilize
2023-06-23 13:50:01 -07:00
if ((batteryVoltage > HEATER_BATTERY_ON_VOLTAGE) && (batteryStabTime > 0))
{
2023-06-23 13:50:01 -07:00
batteryStabTime--;
}
2023-06-23 13:50:01 -07:00
heaterAllowed = batteryStabTime == 0;
}
2021-07-15 21:50:22 -07:00
if (!heaterAllowed)
{
// ECU hasn't allowed preheat yet, reset timer, and force preheat state
2023-06-23 13:50:01 -07:00
timeCounter = preheatTimeCounter;
2021-07-15 21:50:22 -07:00
return HeaterState::Preheat;
}
2023-11-13 13:30:23 -08:00
float overheatTemp = m_targetTempC + 100;
float closedLoopTemp = m_targetTempC - 50;
float underheatTemp = m_targetTempC - 100;
2023-11-13 13:56:03 -08:00
switch (currentState)
2020-10-30 02:40:18 -07:00
{
case HeaterState::Preheat:
2023-06-23 13:50:01 -07:00
timeCounter--;
2020-10-30 02:40:18 -07:00
2020-12-13 17:24:33 -08:00
// If preheat timeout, or sensor is already hot (engine running?)
2023-06-23 13:50:01 -07:00
if (timeCounter <= 0 || sensorTemp > closedLoopTemp)
2020-10-30 02:40:18 -07:00
{
// If enough time has elapsed, start the ramp
2021-07-12 15:24:20 -07:00
// Start the ramp at 4 volts
2023-06-23 13:50:01 -07:00
rampVoltage = 4;
2020-12-10 18:32:41 -08:00
// Next phase times out at 15 seconds
2023-06-23 13:50:01 -07:00
timeCounter = HEATER_WARMUP_TIMEOUT / HEATER_CONTROL_PERIOD;
2020-12-10 18:32:41 -08:00
2020-10-30 02:40:18 -07:00
return HeaterState::WarmupRamp;
}
2020-10-31 14:54:50 -07:00
// Stay in preheat - wait for time to elapse
break;
2020-10-30 02:40:18 -07:00
case HeaterState::WarmupRamp:
if (sensorTemp > closedLoopTemp)
2020-10-30 02:40:18 -07:00
{
return HeaterState::ClosedLoop;
}
2023-06-23 13:50:01 -07:00
else if (timeCounter == 0)
2020-12-10 18:32:41 -08:00
{
2023-06-23 13:50:01 -07:00
SetFault(ch, Fault::SensorDidntHeat);
2020-12-10 18:32:41 -08:00
return HeaterState::Stopped;
}
2023-06-23 13:50:01 -07:00
timeCounter--;
2020-10-31 14:54:50 -07:00
break;
2020-12-10 18:32:41 -08:00
case HeaterState::ClosedLoop:
2021-11-04 15:07:47 -07:00
// Check that the sensor's ESR is acceptable for normal operation
if (sensorTemp > overheatTemp)
2020-12-10 22:08:00 -08:00
{
2023-06-23 13:50:01 -07:00
SetFault(ch, Fault::SensorOverheat);
2020-12-11 15:10:04 -08:00
return HeaterState::Stopped;
2020-12-10 22:08:00 -08:00
}
else if (sensorTemp < underheatTemp)
2020-12-11 15:46:03 -08:00
{
2023-06-23 13:50:01 -07:00
SetFault(ch, Fault::SensorUnderheat);
2020-12-11 15:46:03 -08:00
return HeaterState::Stopped;
}
2020-12-10 22:08:00 -08:00
2020-12-10 18:32:41 -08:00
break;
case HeaterState::Stopped:
case HeaterState::NoHeaterSupply:
/* nop */
break;
2020-10-30 02:40:18 -07:00
}
2020-10-31 14:54:50 -07:00
2023-06-23 13:50:01 -07:00
return heaterState;
2020-10-30 02:40:18 -07:00
}
2023-11-13 13:56:03 -08:00
float HeaterControllerBase::GetVoltageForState(HeaterState state, float sensorEsr)
2020-10-30 02:40:18 -07:00
{
2023-11-13 13:56:03 -08:00
switch (state)
2020-10-30 02:40:18 -07:00
{
2021-07-15 21:48:24 -07:00
case HeaterState::Preheat:
// Max allowed during condensation phase (preheat) is 2v
return 1.5f;
2020-10-30 02:40:18 -07:00
case HeaterState::WarmupRamp:
2023-06-23 13:50:01 -07:00
if (rampVoltage < 10)
2020-10-30 02:40:18 -07:00
{
2021-07-09 23:15:24 -07:00
// 0.3 volt per second, divided by battery voltage and update rate
constexpr float rampRateVoltPerSecond = 0.3f;
2021-04-26 17:40:06 -07:00
constexpr float heaterFrequency = 1000.0f / HEATER_CONTROL_PERIOD;
2023-06-23 13:50:01 -07:00
rampVoltage += (rampRateVoltPerSecond / heaterFrequency);
2020-10-30 02:40:18 -07:00
}
2023-06-23 13:50:01 -07:00
return rampVoltage;
2020-10-30 02:40:18 -07:00
case HeaterState::ClosedLoop:
2021-07-12 15:46:10 -07:00
// "nominal" heater voltage is 7.5v, so apply correction around that point (instead of relying on integrator so much)
2020-10-31 17:34:36 -07:00
// Negated because lower resistance -> hotter
// TODO: heater PID should operate on temperature, not ESR
2023-11-13 13:56:03 -08:00
return 7.5f - heaterPid.GetOutput(m_targetEsr, sensorEsr);
2020-12-10 18:32:41 -08:00
case HeaterState::Stopped:
case HeaterState::NoHeaterSupply:
2021-07-12 15:24:20 -07:00
// Something has gone wrong, turn off the heater.
2020-12-10 18:32:41 -08:00
return 0;
2020-10-30 02:40:18 -07:00
}
2020-12-10 21:05:02 -08:00
// should be unreachable
return 0;
2020-10-30 02:40:18 -07:00
}
2023-11-13 13:30:23 -08:00
void HeaterControllerBase::Update(const ISampler& sampler, HeaterAllow heaterAllowState)
2023-06-23 13:50:01 -07:00
{
// Read sensor state
2023-11-13 13:56:03 -08:00
float sensorEsr = sampler.GetSensorInternalResistance();
2023-06-23 13:50:01 -07:00
float sensorTemperature = sampler.GetSensorTemperature();
// If we haven't heard from the ECU, use the internally sensed
// battery voltage instead of voltage over CAN.
float batteryVoltage = heaterAllowState == HeaterAllow::Unknown
? sampler.GetInternalBatteryVoltage()
: GetRemoteBatteryVoltage();
// Run the state machine
2023-11-13 13:56:03 -08:00
heaterState = GetNextState(heaterState, heaterAllowState, batteryVoltage, sensorTemperature);
float heaterVoltage = GetVoltageForState(heaterState, sensorEsr);
2023-06-23 13:50:01 -07:00
// Limit to 11 volts
if (heaterVoltage > 11) {
heaterVoltage = 11;
}
// duty = (V_eff / V_batt) ^ 2
float voltageRatio = heaterVoltage / batteryVoltage;
float duty = voltageRatio * voltageRatio;
#ifdef HEATER_MAX_DUTY
2023-06-23 14:41:40 -07:00
cycle++;
2023-06-23 13:50:01 -07:00
// limit PWM each 10th cycle (2 time per second) to measure heater supply voltage throuth "Heater-"
2023-06-23 14:41:40 -07:00
if ((cycle % 10) == 0) {
2023-06-23 13:50:01 -07:00
if (duty > HEATER_MAX_DUTY) {
duty = HEATER_MAX_DUTY;
}
}
#endif
if (batteryVoltage >= 23)
{
duty = 0;
heaterVoltage = 0;
}
2021-06-03 21:03:41 -07:00
2023-11-13 13:30:23 -08:00
// Pipe the output to the heater driver
SetDuty(duty);
}
const char* describeHeaterState(HeaterState state)
{
switch (state) {
case HeaterState::Preheat:
return "Preheat";
case HeaterState::WarmupRamp:
return "WarmupRamp";
case HeaterState::ClosedLoop:
return "ClosedLoop";
case HeaterState::Stopped:
return "Stopped";
case HeaterState::NoHeaterSupply:
return "NoHeaterSupply";
}
return "Unknown";
}