class-ify HeaterController

firmware/heater_control.cpp

@@ -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 {
+class HeaterController
-    Pid heaterPid;
+{
-    int timeCounter;
+public:
-    int batteryStabTime;
+    HeaterController(int ch, int pwm_ch)
-    float rampVoltage;
+        : ch(ch), pwm_ch(pwm_ch)
-    float heaterVoltage;
+    {
-    HeaterState heaterState;
+    }
+
+    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;
+    const uint8_t ch;
-    uint8_t pwm_ch;
+    const uint8_t pwm_ch;
 };
 
-static struct heater_state state[AFR_CHANNELS] =
+HeaterController heaterControllers[AFR_CHANNELS] =
 {
-    {
+    { 0, HEATER_PWM_CHANNEL_0 },
-        .heaterPid = Pid(
+
-            0.3f,      // kP
+#if AFR_CHANNELS >= 2
-            0.3f,      // kI
+    { 1, HEATER_PWM_CHANNEL_1 }
-            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,
-    },
 #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];
+            heater.Update(sampler, heaterAllowState);
-
-            // 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;
         }
 
         // 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)