rusefi/wideband
1
0
Fork 0
mirror of https://github.com/mck1117/wideband.git synced 2026-09-30 09:57:06 -04:00
Code Issues Packages Projects Releases Wiki Activity

format pump, heater

Browse Source
This commit is contained in:
Matthew Kennedy
2026-04-10 13:30:00 -07:00
parent ed53f5c7e9
commit 899e48daa6
6 changed files with 148 additions and 151 deletions
Download Patch File Download Diff File Expand all files Collapse all files

226
firmware/heater_control.cpp
View File

@@ -5,12 +5,11 @@
using namespace wbo;
static const PidConfig heaterPidConfig =
{
.kP = 0.3f, // kP
.kI = 0.3f, // kI
.kD = 0.01f, // kD
.clamp = 3.0f, // Integrator clamp (volts)
static const PidConfig heaterPidConfig = {
.kP = 0.3f, // kP
.kI = 0.3f, // kI
.kD = 0.01f, // kD
.clamp = 3.0f, // Integrator clamp (volts)
};
HeaterControllerBase::HeaterControllerBase(int ch, int preheatTimeSec, int warmupTimeSec)
@@ -51,7 +50,10 @@ HeaterState HeaterControllerBase::GetHeaterState() const
return heaterState;
}
HeaterState HeaterControllerBase::GetNextState(HeaterState currentState, HeaterAllow heaterAllowState, float heaterSupplyVoltage, float sensorTemp)
HeaterState HeaterControllerBase::GetNextState(HeaterState currentState,
HeaterAllow heaterAllowState,
float heaterSupplyVoltage,
float sensorTemp)
{
bool heaterAllowed = heaterAllowState == HeaterAllow::Allowed;
@@ -84,77 +86,77 @@ HeaterState HeaterControllerBase::GetNextState(HeaterState currentState, HeaterA
switch (currentState)
{
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;
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;
// Reset the timer for the warmup phase
m_warmupTimer.reset();
// Reset the timer for the warmup phase
m_warmupTimer.reset();
SetStatus(ch, Status::Warmup);
return HeaterState::WarmupRamp;
}
#endif
SetStatus(ch, Status::Warmup);
return HeaterState::WarmupRamp;
}
#endif
// If preheat timeout, or sensor is already hot (engine running?)
if (m_preheatTimer.hasElapsedSec(m_preheatTimeSec) || sensorTemp > closedLoopTemp)
{
// If enough time has elapsed, start the ramp
// Start the ramp at 7 volts
rampVoltage = 7;
// If preheat timeout, or sensor is already hot (engine running?)
if (m_preheatTimer.hasElapsedSec(m_preheatTimeSec) || sensorTemp > closedLoopTemp)
{
// If enough time has elapsed, start the ramp
// Start the ramp at 7 volts
rampVoltage = 7;
// Reset the timer for the warmup phase
m_warmupTimer.reset();
// Reset the timer for the warmup phase
m_warmupTimer.reset();
SetStatus(ch, Status::Warmup);
return HeaterState::WarmupRamp;
}
SetStatus(ch, Status::Warmup);
return HeaterState::WarmupRamp;
}
// Stay in preheat - wait for time to elapse
break;
case HeaterState::WarmupRamp:
if (sensorTemp > closedLoopTemp)
{
SetStatus(ch, Status::RunningClosedLoop);
return HeaterState::ClosedLoop;
}
else if (m_warmupTimer.hasElapsedSec(m_warmupTimeSec))
{
SetStatus(ch, Status::SensorDidntHeat);
return HeaterState::Stopped;
}
// Stay in preheat - wait for time to elapse
break;
case HeaterState::WarmupRamp:
if (sensorTemp > closedLoopTemp)
{
SetStatus(ch, Status::RunningClosedLoop);
return HeaterState::ClosedLoop;
}
else if (m_warmupTimer.hasElapsedSec(m_warmupTimeSec))
{
SetStatus(ch, Status::SensorDidntHeat);
return HeaterState::Stopped;
}
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)
if (sensorTemp <= overheatTemp)
{
m_overheatTimer.reset();
}
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)
if (sensorTemp <= overheatTemp)
{
m_overheatTimer.reset();
}
if (sensorTemp >= underheatTemp)
{
m_underheatTimer.reset();
}
if (sensorTemp >= underheatTemp)
{
m_underheatTimer.reset();
}
if (m_overheatTimer.hasElapsedSec(0.5f))
{
SetStatus(ch, Status::SensorOverheat);
return HeaterState::Stopped;
}
else if (m_underheatTimer.hasElapsedSec(0.5f))
{
SetStatus(ch, Status::SensorUnderheat);
return HeaterState::Stopped;
}
if (m_overheatTimer.hasElapsedSec(0.5f))
{
SetStatus(ch, Status::SensorOverheat);
return HeaterState::Stopped;
}
else if (m_underheatTimer.hasElapsedSec(0.5f))
{
SetStatus(ch, Status::SensorUnderheat);
return HeaterState::Stopped;
}
break;
case HeaterState::Stopped:
break;
break;
case HeaterState::Stopped: break;
}
return currentState;
@@ -164,28 +166,28 @@ float HeaterControllerBase::GetVoltageForState(HeaterState state, float sensorEs
{
switch (state)
{
case HeaterState::Preheat:
// Max allowed during condensation phase (preheat) is 2v
return 2.0f;
case HeaterState::WarmupRamp:
if (rampVoltage < 12)
{
// 0.4 volt per second, divided by battery voltage and update rate
constexpr float rampRateVoltPerSecond = 0.4f;
constexpr float heaterFrequency = 1000.0f / HEATER_CONTROL_PERIOD;
rampVoltage += (rampRateVoltPerSecond / heaterFrequency);
}
case HeaterState::Preheat:
// Max allowed during condensation phase (preheat) is 2v
return 2.0f;
case HeaterState::WarmupRamp:
if (rampVoltage < 12)
{
// 0.4 volt per second, divided by battery voltage and update rate
constexpr float rampRateVoltPerSecond = 0.4f;
constexpr float heaterFrequency = 1000.0f / HEATER_CONTROL_PERIOD;
rampVoltage += (rampRateVoltPerSecond / heaterFrequency);
}
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
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 - m_pid.GetOutput(m_targetEsr, sensorEsr);
case HeaterState::Stopped:
// Something has gone wrong, turn off the heater.
return 0;
// TODO: heater PID should operate on temperature, not ESR
return 7.5f - m_pid.GetOutput(m_targetEsr, sensorEsr);
case HeaterState::Stopped:
// Something has gone wrong, turn off the heater.
return 0;
}
// should be unreachable
@@ -198,27 +200,28 @@ void HeaterControllerBase::Update(const ISampler& sampler, HeaterAllow heaterAll
float sensorEsr = sampler.GetSensorInternalResistance();
float sensorTemperature = sampler.GetSensorTemperature();
#ifdef BOARD_HAS_VOLTAGE_SENSE
float heaterSupplyVoltage = GetSupplyVoltage();
#else // not BOARD_HAS_VOLTAGE_SENSE
// If we haven't heard from the ECU, use the internally sensed
// battery voltage instead of voltage over CAN.
float heaterSupplyVoltage = heaterAllowState == HeaterAllow::Unknown
? sampler.GetInternalHeaterVoltage()
: GetRemoteBatteryVoltage();
#endif
#ifdef BOARD_HAS_VOLTAGE_SENSE
float heaterSupplyVoltage = GetSupplyVoltage();
#else // not BOARD_HAS_VOLTAGE_SENSE
// If we haven't heard from the ECU, use the internally sensed
// battery voltage instead of voltage over CAN.
float heaterSupplyVoltage =
heaterAllowState == HeaterAllow::Unknown ? sampler.GetInternalHeaterVoltage() : GetRemoteBatteryVoltage();
#endif
// Run the state machine
heaterState = GetNextState(heaterState, heaterAllowState, heaterSupplyVoltage, sensorTemperature);
float heaterVoltage = GetVoltageForState(heaterState, sensorEsr);
// Limit to 12 volts
if (heaterVoltage > 12) {
if (heaterVoltage > 12)
{
heaterVoltage = 12;
}
// Very low supply voltage -> avoid divide by zero or very high duty
if (heaterSupplyVoltage < 3) {
if (heaterSupplyVoltage < 3)
{
heaterSupplyVoltage = 12;
}
@@ -226,15 +229,17 @@ void HeaterControllerBase::Update(const ISampler& sampler, HeaterAllow heaterAll
float voltageRatio = (heaterSupplyVoltage < 1.0f) ? 0 : heaterVoltage / heaterSupplyVoltage;
float duty = voltageRatio * voltageRatio;
#ifdef HEATER_MAX_DUTY
#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) {
if ((cycle % 10) == 0)
{
if (duty > HEATER_MAX_DUTY)
{
duty = HEATER_MAX_DUTY;
}
}
#endif
#endif
// Protect the sensor in case of very high voltage
if (heaterSupplyVoltage >= 23)
@@ -249,15 +254,12 @@ void HeaterControllerBase::Update(const ISampler& sampler, HeaterAllow heaterAll
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";
switch (state)
{
case HeaterState::Preheat: return "Preheat";
case HeaterState::WarmupRamp: return "WarmupRamp";
case HeaterState::ClosedLoop: return "ClosedLoop";
case HeaterState::Stopped: return "Stopped";
}
return "Unknown";

3
firmware/heater_control.h
View File

@@ -43,7 +43,8 @@ public:
bool GetIsHeatingEnabled(HeaterAllow heaterAllowState, float batteryVoltage);
HeaterState GetNextState(HeaterState currentState, HeaterAllow haeterAllowState, float batteryVoltage, float sensorTemp);
HeaterState
GetNextState(HeaterState currentState, HeaterAllow haeterAllowState, float batteryVoltage, float sensorTemp);
float GetVoltageForState(HeaterState state, float sensorEsr);
private:

48
firmware/heater_thread.cpp
View File

@@ -12,20 +12,18 @@ static const PWMConfig heaterPwmConfig = {
.frequency = 400'000,
.period = 1024,
.callback = nullptr,
.channels = {
{PWM_OUTPUT_ACTIVE_HIGH | PWM_COMPLEMENTARY_OUTPUT_ACTIVE_LOW, nullptr},
{PWM_OUTPUT_ACTIVE_HIGH | PWM_COMPLEMENTARY_OUTPUT_ACTIVE_LOW, nullptr},
{PWM_OUTPUT_ACTIVE_HIGH | PWM_COMPLEMENTARY_OUTPUT_ACTIVE_LOW, nullptr},
{PWM_OUTPUT_ACTIVE_HIGH | PWM_COMPLEMENTARY_OUTPUT_ACTIVE_LOW, nullptr}
},
.channels = {{PWM_OUTPUT_ACTIVE_HIGH | PWM_COMPLEMENTARY_OUTPUT_ACTIVE_LOW, nullptr},
{PWM_OUTPUT_ACTIVE_HIGH | PWM_COMPLEMENTARY_OUTPUT_ACTIVE_LOW, nullptr},
{PWM_OUTPUT_ACTIVE_HIGH | PWM_COMPLEMENTARY_OUTPUT_ACTIVE_LOW, nullptr},
{PWM_OUTPUT_ACTIVE_HIGH | PWM_COMPLEMENTARY_OUTPUT_ACTIVE_LOW, nullptr}},
.cr2 = 0,
#if STM32_PWM_USE_ADVANCED
.bdtr = 0,
#endif
.dier = 0
};
.dier = 0};
class HeaterController : public HeaterControllerBase {
class HeaterController : public HeaterControllerBase
{
public:
HeaterController(int ch, int pwm_ch)
: HeaterControllerBase(ch, HEATER_PREHEAT_TIME, HEATER_WARMUP_TIMEOUT)
@@ -33,30 +31,26 @@ public:
{
}
void SetDuty(float duty) const override
{
heaterPwm.SetDuty(pwm_ch, duty);
}
void SetDuty(float duty) const override { heaterPwm.SetDuty(pwm_ch, duty); }
// TODO: private:
// TODO: private:
public:
const uint8_t pwm_ch;
};
HeaterController heaterControllers[AFR_CHANNELS] =
{
{ 0, HEATER_PWM_CHANNEL_0 },
HeaterController heaterControllers[AFR_CHANNELS] = {
{0, HEATER_PWM_CHANNEL_0},
#if AFR_CHANNELS >= 2
{ 1, HEATER_PWM_CHANNEL_1 },
{1, HEATER_PWM_CHANNEL_1},
#endif
#if AFR_CHANNELS >= 3
{ 2, HEATER_PWM_CHANNEL_2 },
{2, HEATER_PWM_CHANNEL_2},
#endif
#if AFR_CHANNELS >= 4
{ 3, HEATER_PWM_CHANNEL_3 },
{3, HEATER_PWM_CHANNEL_3},
#endif
};
@@ -80,16 +74,10 @@ static void HeaterThread(void*)
auto& h = heaterControllers[i];
switch (GetSensorType())
{
case SensorType::LSU42:
h.Configure(730, 80);
break;
case SensorType::LSUADV:
h.Configure(785, 300);
break;
case SensorType::LSU49:
default:
h.Configure(780, 300);
break;
case SensorType::LSU42: h.Configure(730, 80); break;
case SensorType::LSUADV: h.Configure(785, 300); break;
case SensorType::LSU49:
default: h.Configure(780, 300); break;
}
}

10
firmware/pid.cpp
View File

@@ -13,8 +13,14 @@ float Pid::GetOutput(float setpoint, float observation)
m_lastError = error;
// Clamp to +- 1
if (m_integrator > m_config.clamp) m_integrator = m_config.clamp;
if (m_integrator < -m_config.clamp) m_integrator = -m_config.clamp;
if (m_integrator > m_config.clamp)
{
m_integrator = m_config.clamp;
}
if (m_integrator < -m_config.clamp)
{
m_integrator = -m_config.clamp;
}
// Multiply by gains and sum
return m_config.kP * error + m_integrator + m_config.kD * dEdt;

2
firmware/pid.h
View File

@@ -11,7 +11,7 @@ struct PidConfig
class Pid
{
public:
Pid(const PidConfig& config, float periodMs)
Pid(const PidConfig& config, float periodMs)
: m_config(config)
, m_periodSec(1e-3 * periodMs)
{

10
firmware/pump_control.cpp
View File

@@ -7,7 +7,8 @@
#include "ch.h"
struct pump_control_state {
struct pump_control_state
{
Pid pumpPid;
};
@@ -18,8 +19,7 @@ PidConfig pumpPidConfig = {
.clamp = 10,
};
static struct pump_control_state state[AFR_CHANNELS] =
{
static struct pump_control_state state[AFR_CHANNELS] = {
{
Pid(pumpPidConfig, PUMP_CONTROL_PERIOD),
},
@@ -52,11 +52,11 @@ static void PumpThread(void*)
{
chRegSetThreadName("Pump");
while(true)
while (true)
{
for (int ch = 0; ch < AFR_CHANNELS; ch++)
{
pump_control_state &s = state[ch];
pump_control_state& s = state[ch];
const auto& sampler = GetSampler(ch);
const auto& heater = GetHeaterController(ch);