heater pid

firmware/Makefile

@@ -128,6 +128,7 @@ CPPSRC = $(ALLCPPSRC) \
           pump_dac.cpp \
           sampling.cpp \
           heater_control.cpp \
+          pid.cpp \
           main.cpp
 
 # List ASM source files here.

firmware/heater_control.cpp

@@ -1,9 +1,11 @@
 #include "heater_control.h"
+#include "wideband_config.h"
 
 #include "ch.h"
 #include "hal.h"
 #include "pwm.h"
 #include "sampling.h"
+#include "pid.h"
 
 // 400khz / 1024 = 390hz PWM
 Pwm heaterPwm(PWMD1, 0, 400'000, 1024);
@@ -48,6 +50,8 @@ static HeaterState GetNextState(HeaterState state, float sensorEsr)
     return state;
 }
 
+static Pid heaterPid(0.1f, 0, HEATER_CONTROL_PERIOD);
+
 static float GetDutyForState(HeaterState state, float heaterEsr)
 {
     switch (state)
@@ -62,12 +66,7 @@ static float GetDutyForState(HeaterState state, float heaterEsr)
 
             return rampDuty;
         case HeaterState::ClosedLoop:
-        {
-            // do something more intelligent here
-            float error = (heaterEsr - 250) / 100;
-
-            return error * 1.0f;
-        }
+            return heaterPid.GetOutput(HEATER_TARGET_ESR, heaterEsr);
     }
 }
 
@@ -90,7 +89,7 @@ static void HeaterThread(void*)
         heaterPwm.SetDuty(duty);
 
         // Loop at ~20hz
-        chThdSleepMilliseconds(50);
+        chThdSleepMilliseconds(HEATER_CONTROL_PERIOD);
     }
 }
 

firmware/pid.cpp

@@ -0,0 +1,13 @@
+#include "pid.h"
+
+float Pid::GetOutput(float setpoint, float observation)
+{
+    // TODO: is this backwards?
+    float error = setpoint - observation;
+
+    // Integrate error
+    m_integrator += error * m_period;
+
+    // Multiply by gains and sum
+    return m_kp * error + m_ki * m_integrator;
+}

firmware/pid.h

@@ -0,0 +1,22 @@
+#pragma once
+
+class Pid
+{
+public:
+    Pid(float kP, float kI, int periodMs) 
+        : m_period(periodMs / 1000.0f)
+        , m_kp(kP)
+        , m_ki(kI)
+    {
+    }
+
+    float GetOutput(float setpoint, float observation);
+
+private:
+    const float m_period;
+    const float m_kp;
+    const float m_ki;
+
+    float m_lastError;
+    float m_integrator;
+};

firmware/wideband_config.h

@@ -28,3 +28,10 @@
 // *******************************
 
 // todo
+
+
+// *******************************
+//    Heater controller config
+// *******************************
+#define HEATER_CONTROL_PERIOD 50
+#define HEATER_TARGET_ESR 300