rusefi
/
wideband
mirror of https://github.com/rusefi/wideband.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

class-ify HeaterController

This commit is contained in:
Matthew Kennedy 2023-06-23 13:50:01 -07:00
parent 324eae132a
commit a8ac2698c4
1 changed files with 113 additions and 113 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

226
firmware/heater_control.cpp
View File

@ -68,57 +68,53 @@ static constexpr int preheatTimeCounter = HEATER_PREHEAT_TIME / HEATER_CONTROL_P
static constexpr int batteryStabTimeCounter = HEATER_BATTERY_STAB_TIME / HEATER_CONTROL_PERIOD;
static const struct sensorHeaterParams *heater;
struct heater_state {
Pid heaterPid;
int timeCounter;
int batteryStabTime;
float rampVoltage;
float heaterVoltage;
HeaterState heaterState;
class HeaterController
{
public:
HeaterController(int ch, int pwm_ch)
: ch(ch), pwm_ch(pwm_ch)
{
}
void Update(const ISampler& sampler, HeaterAllow heaterAllowState);
protected:
HeaterState GetNextState(HeaterAllow haeterAllowState, float batteryVoltage, float sensorTemp);
float GetVoltageForState(float heaterEsr);
// private:
public:
Pid heaterPid =
{
0.3f, // kP
0.3f, // kI
0.01f, // kD
3.0f, // Integrator clamp (volts)
HEATER_CONTROL_PERIOD
};
int timeCounter = preheatTimeCounter;
int batteryStabTime = batteryStabTimeCounter;
float rampVoltage = 0;
float heaterVoltage = 0;
HeaterState heaterState = HeaterState::Preheat;
#ifdef HEATER_MAX_DUTY
int cycle;
#endif
uint8_t ch;
uint8_t pwm_ch;
const uint8_t ch;
const uint8_t pwm_ch;
};
static struct heater_state state[AFR_CHANNELS] =
HeaterController heaterControllers[AFR_CHANNELS] =
{
{
.heaterPid = Pid(
0.3f, // kP
0.3f, // kI
0.01f, // kD
3.0f, // Integrator clamp (volts)
HEATER_CONTROL_PERIOD
),
.timeCounter = preheatTimeCounter,
.batteryStabTime = batteryStabTimeCounter,
.rampVoltage = 0,
.heaterState = HeaterState::Preheat,
.ch = 0,
.pwm_ch = HEATER_PWM_CHANNEL_0,
},
#if (AFR_CHANNELS > 1)
{
.heaterPid = Pid(
0.3f, // kP
0.3f, // kI
0.01f, // kD
3.0f, // Integrator clamp (volts)
HEATER_CONTROL_PERIOD
),
.timeCounter = preheatTimeCounter,
.batteryStabTime = batteryStabTimeCounter,
.rampVoltage = 0,
.heaterState = HeaterState::Preheat,
.ch = 1,
.pwm_ch = HEATER_PWM_CHANNEL_1,
},
{ 0, HEATER_PWM_CHANNEL_0 },
#if AFR_CHANNELS >= 2
{ 1, HEATER_PWM_CHANNEL_1 }
#endif
};
static HeaterState GetNextState(struct heater_state &s, HeaterAllow heaterAllowState, float batteryVoltage, float sensorTemp)
HeaterState HeaterController::GetNextState(HeaterAllow heaterAllowState, float batteryVoltage, float sensorTemp)
{
bool heaterAllowed = heaterAllowState == HeaterAllow::Allowed;
@ -128,20 +124,20 @@ static HeaterState GetNextState(struct heater_state &s, HeaterAllow heaterAllowS
// measured voltage too low to auto-start heating
if (batteryVoltage < HEATER_BATTETY_OFF_VOLTAGE)
{
s.batteryStabTime = batteryStabTimeCounter;
batteryStabTime = batteryStabTimeCounter;
}
// measured voltage is high enougth to auto-start heating, wait some time to stabilize
if ((batteryVoltage > HEATER_BATTERY_ON_VOLTAGE) && (s.batteryStabTime > 0))
if ((batteryVoltage > HEATER_BATTERY_ON_VOLTAGE) && (batteryStabTime > 0))
{
s.batteryStabTime--;
batteryStabTime--;
}
heaterAllowed = s.batteryStabTime == 0;
heaterAllowed = batteryStabTime == 0;
}
if (!heaterAllowed)
{
// ECU hasn't allowed preheat yet, reset timer, and force preheat state
s.timeCounter = preheatTimeCounter;
timeCounter = preheatTimeCounter;
return HeaterState::Preheat;
}
@ -149,20 +145,20 @@ static HeaterState GetNextState(struct heater_state &s, HeaterAllow heaterAllowS
float closedLoopTemp = heater->targetTemp - 50;
float underheatTemp = heater->targetTemp - 100;
switch (s.heaterState)
switch (heaterState)
{
case HeaterState::Preheat:
s.timeCounter--;
timeCounter--;
// If preheat timeout, or sensor is already hot (engine running?)
if (s.timeCounter <= 0 || sensorTemp > closedLoopTemp)
if (timeCounter <= 0 || sensorTemp > closedLoopTemp)
{
// If enough time has elapsed, start the ramp
// Start the ramp at 4 volts
s.rampVoltage = 4;
rampVoltage = 4;
// Next phase times out at 15 seconds
s.timeCounter = HEATER_WARMUP_TIMEOUT / HEATER_CONTROL_PERIOD;
timeCounter = HEATER_WARMUP_TIMEOUT / HEATER_CONTROL_PERIOD;
return HeaterState::WarmupRamp;
}
@ -174,25 +170,25 @@ static HeaterState GetNextState(struct heater_state &s, HeaterAllow heaterAllowS
{
return HeaterState::ClosedLoop;
}
else if (s.timeCounter == 0)
else if (timeCounter == 0)
{
SetFault(s.ch, Fault::SensorDidntHeat);
SetFault(ch, Fault::SensorDidntHeat);
return HeaterState::Stopped;
}
s.timeCounter--;
timeCounter--;
break;
case HeaterState::ClosedLoop:
// Check that the sensor's ESR is acceptable for normal operation
if (sensorTemp > overheatTemp)
{
SetFault(s.ch, Fault::SensorOverheat);
SetFault(ch, Fault::SensorOverheat);
return HeaterState::Stopped;
}
else if (sensorTemp < underheatTemp)
{
SetFault(s.ch, Fault::SensorUnderheat);
SetFault(ch, Fault::SensorUnderheat);
return HeaterState::Stopped;
}
@ -203,32 +199,32 @@ static HeaterState GetNextState(struct heater_state &s, HeaterAllow heaterAllowS
break;
}
return s.heaterState;
return heaterState;
}
static float GetVoltageForState(struct heater_state &s, float heaterEsr)
float HeaterController::GetVoltageForState(float heaterEsr)
{
switch (s.heaterState)
switch (heaterState)
{
case HeaterState::Preheat:
// Max allowed during condensation phase (preheat) is 2v
return 1.5f;
case HeaterState::WarmupRamp:
if (s.rampVoltage < 10)
if (rampVoltage < 10)
{
// 0.3 volt per second, divided by battery voltage and update rate
constexpr float rampRateVoltPerSecond = 0.3f;
constexpr float heaterFrequency = 1000.0f / HEATER_CONTROL_PERIOD;
s.rampVoltage += (rampRateVoltPerSecond / heaterFrequency);
rampVoltage += (rampRateVoltPerSecond / heaterFrequency);
}
return s.rampVoltage;
return rampVoltage;
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 - s.heaterPid.GetOutput(heater->targetESR, heaterEsr);
return 7.5f - heaterPid.GetOutput(heater->targetESR, heaterEsr);
case HeaterState::Stopped:
// Something has gone wrong, turn off the heater.
return 0;
@ -238,6 +234,51 @@ static float GetVoltageForState(struct heater_state &s, float heaterEsr)
return 0;
}
void HeaterController::Update(const ISampler& sampler, HeaterAllow heaterAllowState)
{
// Read sensor state
float heaterEsr = sampler.GetSensorInternalResistance();
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
heaterState = GetNextState(heaterAllowState, batteryVoltage, sensorTemperature);
float heaterVoltage = GetVoltageForState(heaterEsr);
// 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
s.cycle++;
// limit PWM each 10th cycle (2 time per second) to measure heater supply voltage throuth "Heater-"
if ((s.cycle % 10) == 0) {
if (duty > HEATER_MAX_DUTY) {
duty = HEATER_MAX_DUTY;
}
}
#endif
if (batteryVoltage >= 23)
{
duty = 0;
heaterVoltage = 0;
}
// Pipe the output to the heater driver
heaterPwm.SetDuty(pwm_ch, duty);
heaterVoltage = heaterVoltage;
}
static THD_WORKING_AREA(waHeaterThread, 256);
static void HeaterThread(void*)
{
@ -258,50 +299,9 @@ static void HeaterThread(void*)
for (i = 0; i < AFR_CHANNELS; i++) {
const auto& sampler = GetSampler(i);
auto& heater = heaterControllers[i];
heater_state &s = state[i];
// Read sensor state
float heaterEsr = sampler.GetSensorInternalResistance();
float sensorTemperature = sampler.GetSensorTemperature();
// If we haven't heard from rusEFI, use the internally sensed
// battery voltage instead of voltage over CAN.
float batteryVoltage = heaterAllowState == HeaterAllow::Unknown
? sampler.GetInternalBatteryVoltage()
: GetRemoteBatteryVoltage();
// Run the state machine
s.heaterState = GetNextState(s, heaterAllowState, batteryVoltage, sensorTemperature);
float heaterVoltage = GetVoltageForState(s, heaterEsr);
// 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
s.cycle++;
// limit PWM each 10th cycle (2 time per second) to measure heater supply voltage throuth "Heater-"
if ((s.cycle % 10) == 0) {
if (duty > HEATER_MAX_DUTY) {
duty = HEATER_MAX_DUTY;
}
}
#endif
if (batteryVoltage >= 23)
{
duty = 0;
heaterVoltage = 0;
}
// Pipe the output to the heater driver
heaterPwm.SetDuty(s.pwm_ch, duty);
s.heaterVoltage = heaterVoltage;
heater.Update(sampler, heaterAllowState);
}
// Loop at ~20hz
@ -312,9 +312,9 @@ static void HeaterThread(void*)
void StartHeaterControl()
{
heaterPwm.Start(heaterPwmConfig);
heaterPwm.SetDuty(state[0].pwm_ch, 0);
heaterPwm.SetDuty(heaterControllers[0].pwm_ch, 0);
#if (AFR_CHANNELS > 1)
heaterPwm.SetDuty(state[1].pwm_ch, 0);
heaterPwm.SetDuty(heaterControllers[1].pwm_ch, 0);
#endif
chThdCreateStatic(waHeaterThread, sizeof(waHeaterThread), NORMALPRIO + 1, HeaterThread, nullptr);
@ -322,22 +322,22 @@ void StartHeaterControl()
bool IsRunningClosedLoop(int ch)
{
return state[ch].heaterState == HeaterState::ClosedLoop;
return heaterControllers[ch].heaterState == HeaterState::ClosedLoop;
}
float GetHeaterDuty(int ch)
{
return heaterPwm.GetLastDuty(state[ch].pwm_ch);
return heaterPwm.GetLastDuty(heaterControllers[ch].pwm_ch);
}
float GetHeaterEffVoltage(int ch)
{
return state[ch].heaterVoltage;
return heaterControllers[ch].heaterVoltage;
}
HeaterState GetHeaterState(int ch)
{
return state[ch].heaterState;
return heaterControllers[ch].heaterState;
}
const char* describeHeaterState(HeaterState state)