Pinecil v2 tune via PID (#1827)

* Start PWM after adc irq fully done

* Filter len 4

* Use comparitor 2 on timer for wrap around

* Update IRQ.cpp

* Tip measurements are uint16_t

Update BSP.cpp

Update BSP.cpp

* WiP PID

move pid tuning to config

Update PIDThread.cpp

* Handle PWM Timer gitchy comparitor

* Tuning

* Dampen with Kd

* Cleaning up

* Use TemperatureType_t for getTipTemp()

* Add small rolling average to user GUI temp to reduce flicker

* Trigger PID when adc is skipped (will use old values)

source/Core/BSP/BSP_Power.h

@@ -19,9 +19,8 @@ void power_check();
 // Returns the tip resistance in x10 ohms, so 7.5 = 75; 14=140 etc
 uint8_t getTipResistanceX10();
 
-uint8_t getTipThermalMass();
+uint16_t getTipThermalMass();
-uint8_t getTipInertia();
+uint16_t getTipInertia();
-
 
 #ifdef __cplusplus
 }

source/Core/BSP/MHP30/BSP.cpp

@@ -472,7 +472,7 @@ uint64_t getDeviceID() {
 
 uint8_t preStartChecksDone() { return 1; }
 
-uint8_t getTipThermalMass() { return TIP_THERMAL_MASS; }
+uint16_t getTipThermalMass() { return TIP_THERMAL_MASS; }
-uint8_t getTipInertia() { return TIP_THERMAL_MASS; }
+uint16_t getTipInertia() { return TIP_THERMAL_MASS; }
 
 void showBootLogo(void) { BootLogo::handleShowingLogo((uint8_t *)FLASH_LOGOADDR); }

source/Core/BSP/Miniware/BSP.cpp

@@ -396,7 +396,7 @@ bool isTipShorted() { return tipShorted; }
 #else
 bool isTipShorted() { return false; }
 #endif
-uint8_t getTipThermalMass() {
+uint16_t getTipThermalMass() {
 #ifdef TIP_RESISTANCE_SENSE_Pin
   if (lastTipResistance >= 80) {
     return TIP_THERMAL_MASS;
@@ -406,7 +406,7 @@ uint8_t getTipThermalMass() {
   return TIP_THERMAL_MASS;
 #endif
 }
-uint8_t getTipInertia() {
+uint16_t getTipInertia() {
 #ifdef TIP_RESISTANCE_SENSE_Pin
   if (lastTipResistance >= 80) {
     return TIP_THERMAL_MASS;

source/Core/BSP/Pinecil/BSP.cpp

@@ -97,7 +97,7 @@ uint8_t getTipResistanceX10() { return TIP_RESISTANCE; }
 bool    isTipShorted() { return false; }
 uint8_t preStartChecksDone() { return 1; }
 
-uint8_t getTipThermalMass() { return TIP_THERMAL_MASS; }
+uint16_t getTipThermalMass() { return TIP_THERMAL_MASS; }
-uint8_t getTipInertia() { return TIP_THERMAL_MASS; }
+uint16_t getTipInertia() { return TIP_THERMAL_MASS; }
 
 void showBootLogo(void) { BootLogo::handleShowingLogo((uint8_t *)FLASH_LOGOADDR); }

source/Core/BSP/Pinecilv2/BSP.cpp

@@ -160,18 +160,8 @@ uint8_t       getTipResistanceX10() {
   return lastTipResistance;
 }
 
-uint8_t getTipThermalMass() {
+uint16_t getTipThermalMass() { return 120; }
-  if (lastTipResistance >= 80) {
+uint16_t getTipInertia() { return 750; }
-    return 65;
-  }
-  return 45;
-}
-uint8_t getTipInertia() {
-  if (lastTipResistance >= 80) {
-    return 90;
-  }
-  return 10;
-}
 // We want to calculate lastTipResistance
 // If tip is connected, and the tip is cold and the tip is not being heated
 // We can use the GPIO to inject a small current into the tip and measure this
@@ -180,7 +170,7 @@ uint8_t getTipInertia() {
 // Which is around 0.54mA this will induce:
 // 6 ohm tip -> 3.24mV (Real world ~= 3320)
 // 8 ohm tip -> 4.32mV (Real world ~= 4500)
-// Which is definitely measureable
+// Which is definitely measurable
 // Taking shortcuts here as we know we only really have to pick apart 6 and 8 ohm tips
 // These are reported as 60 and 75 respectively
 void performTipResistanceSampleReading() {

source/Core/BSP/Pinecilv2/IRQ.cpp

@@ -19,17 +19,17 @@ extern "C" {
 }
 void start_PWM_output(void);
 
-#define ADC_Filter_Smooth 1
+#define ADC_Filter_Smooth 4 /* This basically smooths over one PWM cycle / set of readings */
 history<uint16_t, ADC_Filter_Smooth> ADC_Vin;
 history<uint16_t, ADC_Filter_Smooth> ADC_Temp;
 history<uint16_t, ADC_Filter_Smooth> ADC_Tip;
-volatile uint8_t                     ADCBurstCounter = 0;
+
+// IRQ is called at the end of the 8 set readings, pop these from the FIFO and send to filters
 void adc_fifo_irq(void) {
   if (ADC_GetIntStatus(ADC_INT_FIFO_READY) == SET) {
     // Read out all entries in the fifo
     while (ADC_Get_FIFO_Count()) {
-      ADCBurstCounter++;
+      uint32_t reading = ADC_Read_FIFO();
-      volatile uint32_t reading = ADC_Read_FIFO();
       // As per manual, 26 bit reading; lowest 16 are the ADC
       uint16_t sample = reading & 0xFFFF;
       uint8_t  source = (reading >> 21) & 0b11111;
@@ -43,16 +43,10 @@ void                                 adc_fifo_irq(void) {
       case VIN_ADC_CHANNEL:
         ADC_Vin.update(sample);
         break;
-
       default:
         break;
       }
     }
-
-    if (ADCBurstCounter >= 8) {
-      ADCBurstCounter = 0;
-      start_PWM_output();
-
     // unblock the PID controller thread
     if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
       BaseType_t xHigherPriorityTaskWoken = pdFALSE;
@@ -62,7 +56,6 @@ void                                 adc_fifo_irq(void) {
       }
     }
   }
-  }
   // Clear IRQ
   ADC_IntClr(ADC_INT_ALL);
 }
@@ -100,16 +93,43 @@ void start_PWM_output(void) {
     PWM_Channel_Disable(PWM_Channel);
     switchToFastPWM();
   }
-  TIMER_Enable(TIMER_CH0);
 }
 
 // Timer 0 is used to co-ordinate the ADC and the output PWM
 void timer0_comp0_callback(void) {
-  TIMER_Disable(TIMER_CH0);
+  if (PWM_Channel_Is_Enabled(PWM_Channel)) {
+    // So there appears to be a bug _somewhere_ where sometimes the comparator doesn't fire
+    // Its not re-occurring with specific values, so suspect its a weird bug
+    // For now, we just skip the cycle and throw away the ADC readings. Its a waste but
+    // It stops stupid glitches in readings, i'd take slight instability from the time jump
+    // Over the readings we get that are borked as the header is left on
+    // <Ralim 2023/10/14>
+    PWM_Channel_Disable(PWM_Channel);
+    // MSG("ALERT PWM Glitch\r\n");
+    // Triger the PID now instead
+    if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
+      BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+      if (pidTaskNotification) {
+        vTaskNotifyGiveFromISR(pidTaskNotification, &xHigherPriorityTaskWoken);
+        portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+      }
+    }
+  } else {
     ADC_Start();
+  }
+  TIMER_ClearIntStatus(TIMER_CH0, TIMER_COMP_ID_0);
+}
+void timer0_comp1_callback(void) {
+  // Trigged at end of output cycle; turn off the tip PWM
+  PWM_Channel_Disable(PWM_Channel);
+  TIMER_ClearIntStatus(TIMER_CH0, TIMER_COMP_ID_1);
 }
-void timer0_comp1_callback(void) { PWM_Channel_Disable(PWM_Channel); } // Trigged at end of output cycle; turn off the tip PWM
 
+void timer0_comp2_callback(void) {
+  // Triggered at end of timer cycle; re-start the tip driver
+  start_PWM_output();
+  TIMER_ClearIntStatus(TIMER_CH0, TIMER_COMP_ID_2);
+}
 void switchToFastPWM(void) {
   inFastPWMMode    = true;
   holdoffTicks     = 10;
@@ -119,8 +139,8 @@ void switchToFastPWM(void) {
 
   // ~10Hz
   TIMER_SetCompValue(TIMER_CH0, TIMER_COMP_ID_0, powerPWM + holdoffTicks);
-  // Set divider to 10 ~= 10.5Hz
 
+  // Set divider to 10 ~= 10.5Hz
   uint32_t tmpVal = BL_RD_REG(TIMER_BASE, TIMER_TCDR);
 
   tmpVal = BL_SET_REG_BITS_VAL(tmpVal, TIMER_TCDR2, 10);
@@ -139,7 +159,7 @@ void switchToSlowPWM(void) {
   // Adjust ADC
   TIMER_SetCompValue(TIMER_CH0, TIMER_COMP_ID_0, powerPWM + holdoffTicks);
 
-  // Set divider to 22
+  // Set divider for ~ 5Hz
 
   uint32_t tmpVal = BL_RD_REG(TIMER_BASE, TIMER_TCDR);
 
@@ -193,5 +213,6 @@ uint16_t getADCHandleTemp(uint8_t sample) { return ADC_Temp.average(); }
 
 uint16_t getADCVin(uint8_t sample) { return ADC_Vin.average(); }
 
-// Returns either average or instant value. When sample is set the samples from the injected ADC are copied to the filter and then the raw reading is returned
+// Returns the current raw tip reading after any cleanup filtering
+// For Pinecil V2 we dont do any rolling filtering other than just averaging all 4 readings in the adc snapshot
 uint16_t getTipRawTemp(uint8_t sample) { return ADC_Tip.average() >> 1; }

source/Core/BSP/Pinecilv2/Setup.cpp

@@ -102,7 +102,7 @@ void setup_adc(void) {
 
   adc_cfg.clkDiv         = ADC_CLK_DIV_4;
   adc_cfg.vref           = ADC_VREF_3P2V;
-  adc_cfg.resWidth       = ADC_DATA_WIDTH_14_WITH_64_AVERAGE;
+  adc_cfg.resWidth       = ADC_DATA_WIDTH_14_WITH_16_AVERAGE;
   adc_cfg.inputMode      = ADC_INPUT_SINGLE_END;
   adc_cfg.v18Sel         = ADC_V18_SEL_1P72V;
   adc_cfg.v11Sel         = ADC_V11_SEL_1P1V;
@@ -111,7 +111,7 @@ void setup_adc(void) {
   adc_cfg.chopMode       = ADC_CHOP_MOD_AZ_ON;
   adc_cfg.biasSel        = ADC_BIAS_SEL_MAIN_BANDGAP;
   adc_cfg.vcm            = ADC_PGA_VCM_1P6V;
-  adc_cfg.offsetCalibEn  = ENABLE;
+  adc_cfg.offsetCalibEn  = DISABLE;
   adc_cfg.offsetCalibVal = 0;
 
   ADC_Disable();
@@ -120,7 +120,7 @@ void setup_adc(void) {
 
   ADC_Init(&adc_cfg);
   adc_fifo_cfg.dmaEn         = DISABLE;
-  adc_fifo_cfg.fifoThreshold = ADC_FIFO_THRESHOLD_8;
+  adc_fifo_cfg.fifoThreshold = ADC_FIFO_THRESHOLD_8; // Triger FIFO when all 8 measurements are done
   ADC_FIFO_Cfg(&adc_fifo_cfg);
   ADC_MIC_Bias_Disable();
   ADC_Tsen_Disable();
@@ -140,24 +140,27 @@ void setup_timer_scheduler() {
   TIMER_CFG_Type cfg = {
       TIMER_CH0,                                              // Channel
       TIMER_CLKSRC_32K,                                       // Clock source
-      TIMER_PRELOAD_TRIG_COMP0,            // Trigger; reset after trigger 0
+      TIMER_PRELOAD_TRIG_COMP2,                               // Trigger; reset after trigger 0
       TIMER_COUNT_PRELOAD,                                    // Counter mode
       22,                                                     // Clock div
       (uint16_t)(powerPWM + holdoffTicks),                    // CH0 compare (adc)
-      0,                                   // CH1 compare (pwm out)
+      (uint16_t)(powerPWM),                                   // CH1 compare (pwm out)
-      0,                                   // CH2 compare not used
+      (uint16_t)(powerPWM + holdoffTicks + tempMeasureTicks), // CH2 compare end of cycle
       0,                                                      // Preload
   };
   TIMER_Init(&cfg);
 
   Timer_Int_Callback_Install(TIMER_CH0, TIMER_INT_COMP_0, timer0_comp0_callback);
   Timer_Int_Callback_Install(TIMER_CH0, TIMER_INT_COMP_1, timer0_comp1_callback);
+  Timer_Int_Callback_Install(TIMER_CH0, TIMER_INT_COMP_2, timer0_comp2_callback);
 
   TIMER_ClearIntStatus(TIMER_CH0, TIMER_COMP_ID_0);
   TIMER_ClearIntStatus(TIMER_CH0, TIMER_COMP_ID_1);
+  TIMER_ClearIntStatus(TIMER_CH0, TIMER_COMP_ID_2);
 
   TIMER_IntMask(TIMER_CH0, TIMER_INT_COMP_0, UNMASK);
   TIMER_IntMask(TIMER_CH0, TIMER_INT_COMP_1, UNMASK);
+  TIMER_IntMask(TIMER_CH0, TIMER_INT_COMP_2, UNMASK);
   CPU_Interrupt_Enable(TIMER_CH0_IRQn);
   TIMER_Enable(TIMER_CH0);
 }

source/Core/BSP/Pinecilv2/bl_mcu_sdk/drivers/bl702_driver/std_drv/inc/bl702_pwm.h

@@ -36,8 +36,9 @@
 #ifndef __BL702_PWM_H__
 #define __BL702_PWM_H__
 
-#include "pwm_reg.h"
 #include "bl702_common.h"
+#include "pwm_reg.h"
+
 
 /** @addtogroup  BL702_Peripheral_Driver
  *  @{
@@ -99,8 +100,7 @@ typedef enum {
 /**
  *  @brief PWM configuration structure type definition
  */
-typedef struct
+typedef struct {
-{
   PWM_CH_ID_Type     ch;          /*!< PWM channel */
   PWM_Clk_Type       clk;         /*!< PWM Clock */
   PWM_Stop_Mode_Type stopMode;    /*!< PWM Stop Mode */
@@ -121,37 +121,27 @@ typedef struct
 /** @defgroup  PWM_CH_ID_TYPE
  *  @{
  */
-#define IS_PWM_CH_ID_TYPE(type) (((type) == PWM_CH0) || \
+#define IS_PWM_CH_ID_TYPE(type) (((type) == PWM_CH0) || ((type) == PWM_CH1) || ((type) == PWM_CH2) || ((type) == PWM_CH3) || ((type) == PWM_CH4) || ((type) == PWM_CH_MAX))
-                                 ((type) == PWM_CH1) || \
-                                 ((type) == PWM_CH2) || \
-                                 ((type) == PWM_CH3) || \
-                                 ((type) == PWM_CH4) || \
-                                 ((type) == PWM_CH_MAX))
 
 /** @defgroup  PWM_CLK_TYPE
  *  @{
  */
-#define IS_PWM_CLK_TYPE(type) (((type) == PWM_CLK_XCLK) || \
+#define IS_PWM_CLK_TYPE(type) (((type) == PWM_CLK_XCLK) || ((type) == PWM_CLK_BCLK) || ((type) == PWM_CLK_32K))
-                               ((type) == PWM_CLK_BCLK) || \
-                               ((type) == PWM_CLK_32K))
 
 /** @defgroup  PWM_STOP_MODE_TYPE
  *  @{
  */
-#define IS_PWM_STOP_MODE_TYPE(type) (((type) == PWM_STOP_ABRUPT) || \
+#define IS_PWM_STOP_MODE_TYPE(type) (((type) == PWM_STOP_ABRUPT) || ((type) == PWM_STOP_GRACEFUL))
-                                     ((type) == PWM_STOP_GRACEFUL))
 
 /** @defgroup  PWM_POLARITY_TYPE
  *  @{
  */
-#define IS_PWM_POLARITY_TYPE(type) (((type) == PWM_POL_NORMAL) || \
+#define IS_PWM_POLARITY_TYPE(type) (((type) == PWM_POL_NORMAL) || ((type) == PWM_POL_INVERT))
-                                    ((type) == PWM_POL_INVERT))
 
 /** @defgroup  PWM_INT_TYPE
  *  @{
  */
-#define IS_PWM_INT_TYPE(type) (((type) == PWM_INT_PULSE_CNT) || \
+#define IS_PWM_INT_TYPE(type) (((type) == PWM_INT_PULSE_CNT) || ((type) == PWM_INT_ALL))
-                               ((type) == PWM_INT_ALL))
 
 /*@} end of group PWM_Public_Constants */
 
@@ -181,13 +171,13 @@ void PWM_Channel_Set_Period(PWM_CH_ID_Type ch, uint16_t period);
 void        PWM_Channel_Get(PWM_CH_ID_Type ch, uint16_t *period, uint16_t *threshold1, uint16_t *threshold2);
 void        PWM_IntMask(PWM_CH_ID_Type ch, PWM_INT_Type intType, BL_Mask_Type intMask);
 void        PWM_Channel_Enable(PWM_CH_ID_Type ch);
+uint8_t     PWM_Channel_Is_Enabled(PWM_CH_ID_Type ch);
 void        PWM_Channel_Disable(PWM_CH_ID_Type ch);
 void        PWM_SW_Mode(PWM_CH_ID_Type ch, BL_Fun_Type enable);
 void        PWM_SW_Force_Value(PWM_CH_ID_Type ch, uint8_t value);
 void        PWM_Int_Callback_Install(PWM_CH_ID_Type ch, uint32_t intType, intCallback_Type *cbFun);
 BL_Err_Type PWM_Smart_Configure(PWM_CH_ID_Type ch, uint32_t frequency, uint8_t dutyCycle);
 
-
 /*@} end of group PWM_Public_Functions */
 
 /*@} end of group PWM */

source/Core/BSP/Pinecilv2/bl_mcu_sdk/drivers/bl702_driver/std_drv/src/bl702_pwm.c

@@ -354,7 +354,18 @@ void PWM_Channel_Enable(PWM_CH_ID_Type ch) {
   tmpVal = BL_RD_REG(PWMx, PWM_CONFIG);
   BL_WR_REG(PWMx, PWM_CONFIG, BL_CLR_REG_BIT(tmpVal, PWM_STOP_EN));
 }
+uint8_t PWM_Channel_Is_Enabled(PWM_CH_ID_Type ch) {
+  uint32_t tmpVal;
+  /* Get channel register */
+  uint32_t PWMx = PWM_Get_Channel_Reg(ch);
 
+  /* Check the parameters */
+  CHECK_PARAM(IS_PWM_CH_ID_TYPE(ch));
+
+  /* Config pwm clock to enable pwm */
+  tmpVal = BL_RD_REG(PWMx, PWM_CONFIG);
+  return BL_GET_REG_BITS_VAL(tmpVal, PWM_STOP_EN) == 0;
+}
 /****************************************************************************
  * @brief  PWM disable
  *

source/Core/BSP/Pinecilv2/configuration.h

@@ -161,9 +161,15 @@
 #define DEBUG_UART_OUTPUT
 #define HAS_POWER_DEBUG_MENU
 #define HARDWARE_MAX_WATTAGE_X10  750
-#define BLE_ENABLED
+#define BLE_ENABLED                   // We have a BLE stack
-#define NEEDS_VBUS_PROBE 0
+#define NEEDS_VBUS_PROBE          0   // No vbus probe, its not connected in pcb
-#define CANT_DIRECT_READ_SETTINGS
+#define CANT_DIRECT_READ_SETTINGS     // We cant memcpy settings due to flash cache
+#define TIP_CONTROL_PID               // We use PID rather than integrator
+#define TIP_PID_KP                45  // Reasonable compromise for most tips so far
+#define TIP_PID_KI                9   // About as high for stability across tips
+#define TIP_PID_KD                200 // Helps dampen smaller tips; ~= nothing for larger tips
+#define FILTER_DISPLAYED_TIP_TEMP 8   // Filtering for GUI display
+
 #endif /* Pinecilv2 */
 
 #define FLASH_PAGE_SIZE (1024) // Read pages

source/Core/BSP/Sequre_S60/BSP.cpp

@@ -234,8 +234,8 @@ uint8_t getTipResistanceX10() { return TIP_RESISTANCE; }
 bool    isTipShorted() { return false; }
 uint8_t preStartChecksDone() { return 1; }
 
-uint8_t getTipThermalMass() { return TIP_THERMAL_MASS; }
+uint16_t getTipThermalMass() { return TIP_THERMAL_MASS; }
-uint8_t getTipInertia() { return TIP_THERMAL_INERTIA; }
+uint16_t getTipInertia() { return TIP_THERMAL_INERTIA; }
 
 void setBuzzer(bool on) {}
 

source/Core/Threads/OperatingModes/OperatingModes.h

@@ -10,6 +10,7 @@ extern "C" {
 #include "Settings.h"
 #include "TipThermoModel.h"
 #include "Translation.h"
+#include "Types.h"
 #include "cmsis_os.h"
 #include "configuration.h"
 #include "history.hpp"
@@ -47,5 +48,5 @@ void renderHomeScreenAssets(void);                                 // Called to
 
 // Common helpers
 int8_t            getPowerSourceNumber(void); // Returns number ID of power source
-uint16_t getTipTemp(void);                                         // Returns temperature of the tip in *C/*F (based on user settings)
+TemperatureType_t getTipTemp(void);           // Returns temperature of the tip in *C/*F (based on user settings)
 #endif

source/Core/Threads/OperatingModes/Sleep.cpp

@@ -25,7 +25,7 @@ int gui_SolderingSleepingMode(bool stayOff, bool autoStarted) {
     }
 
     // draw the lcd
-    uint16_t tipTemp = getTipTemp();
+    TemperatureType_t tipTemp = getTipTemp();
 
     OLED::clearScreen();
     OLED::setCursor(0, 0);

source/Core/Threads/OperatingModes/utils/DrawTipTemperature.cpp

@@ -4,7 +4,7 @@
 
 void gui_drawTipTemp(bool symbol, const FontStyle font) {
   // Draw tip temp handling unit conversion & tolerance near setpoint
-  uint16_t Temp = getTipTemp();
+  TemperatureType_t Temp = getTipTemp();
 
   OLED::printNumber(Temp, 3, font); // Draw the tip temp out
   if (symbol) {

source/Core/Threads/OperatingModes/utils/SolderingCommon.cpp

@@ -4,6 +4,8 @@
 
 #include "SolderingCommon.h"
 #include "OperatingModes.h"
+#include "configuration.h"
+#include "history.hpp"
 
 extern bool heaterThermalRunaway;
 
@@ -166,4 +168,14 @@ int8_t getPowerSourceNumber(void) {
 }
 
 // Returns temperature of the tip in *C/*F (based on user settings)
-uint16_t getTipTemp(void) { return getSettingValue(SettingsOptions::TemperatureInF) ? TipThermoModel::getTipInF() : TipThermoModel::getTipInC(); }
+TemperatureType_t getTipTemp(void) {
+#ifdef FILTER_DISPLAYED_TIP_TEMP
+  static history<TemperatureType_t, FILTER_DISPLAYED_TIP_TEMP> Filter_Temp;
+  TemperatureType_t                                            reading = getSettingValue(SettingsOptions::TemperatureInF) ? TipThermoModel::getTipInF() : TipThermoModel::getTipInC();
+  Filter_Temp.update(reading);
+  return Filter_Temp.average();
+
+#else
+  return getSettingValue(SettingsOptions::TemperatureInF) ? TipThermoModel::getTipInF() : TipThermoModel::getTipInC();
+#endif
+}

source/Core/Threads/PIDThread.cpp

@@ -10,6 +10,7 @@
 #include "Settings.h"
 #include "TipThermoModel.h"
 #include "cmsis_os.h"
+#include "configuration.h"
 #include "history.hpp"
 #include "main.hpp"
 #include "power.hpp"
@@ -22,7 +23,7 @@ volatile TemperatureType_t currentTempTargetDegC   = 0; // Current temperature t
 int32_t                    powerSupplyWattageLimit = 0;
 bool                       heaterThermalRunaway    = false;
 
-static int32_t getPIDResultX10Watts(TemperatureType_t tError);
+static int32_t getPIDResultX10Watts(TemperatureType_t set_point, TemperatureType_t current_value);
 static void    detectThermalRunaway(const TemperatureType_t currentTipTempInC, const TemperatureType_t tError);
 static void    setOutputx10WattsViaFilters(int32_t x10Watts);
 static int32_t getX10WattageLimits();
@@ -71,10 +72,9 @@ void startPIDTask(void const *argument __unused) {
         if (PIDTempTarget > TipThermoModel::getTipMaxInC()) {
           PIDTempTarget = TipThermoModel::getTipMaxInC();
         }
-        TemperatureType_t tError = PIDTempTarget - currentTipTempInC;
 
-        detectThermalRunaway(currentTipTempInC, tError);
+        detectThermalRunaway(currentTipTempInC, PIDTempTarget - currentTipTempInC);
-        x10WattsOut = getPIDResultX10Watts(tError);
+        x10WattsOut = getPIDResultX10Watts(PIDTempTarget, currentTipTempInC);
       } else {
         detectThermalRunaway(currentTipTempInC, 0);
       }
@@ -89,6 +89,53 @@ void startPIDTask(void const *argument __unused) {
   }
 }
 
+#ifdef TIP_CONTROL_PID
+template <class T, T Kp, T Ki, T Kd, T integral_limit_scale> struct PID {
+  T previous_error_term;
+  T integration_running_sum;
+
+  T update(const T set_point, const T new_reading, const TickType_t interval_ms, const T max_output) {
+    const T target_delta = set_point - new_reading;
+
+    // Proportional term
+    const T kp_result = Kp * target_delta;
+
+    // Integral term as we use mixed sampling rates, we cant assume a constant sample interval
+    // Thus we multiply this out by the interval time to ~= dv/dt
+    // Then the shift by 1000 is ms -> Seconds
+
+    integration_running_sum += (target_delta * interval_ms * Ki) / 1000;
+
+    // We constrain integration_running_sum to limit windup
+    // This is not overly required for most use cases but can prevent large overshoot in constrained implementations
+    if (integration_running_sum > integral_limit_scale * max_output) {
+      integration_running_sum = integral_limit_scale * max_output;
+    } else if (integration_running_sum < -integral_limit_scale * max_output) {
+      integration_running_sum = -integral_limit_scale * max_output;
+    }
+    // Calculate the integral term, we use a shift 100 to get precision in integral as we often need small amounts
+    T ki_result = integration_running_sum / 100;
+
+    // Derivative term
+    T derivative = (target_delta - previous_error_term);
+    T kd_result  = ((Kd * derivative) / (T)(interval_ms));
+
+    // Summation of the outputs
+    T output = kp_result + ki_result + kd_result;
+
+    // Restrict to max / 0
+    if (output > max_output)
+      output = max_output;
+    else if (output < 0)
+      output = 0;
+
+    // Save target_delta to previous target_delta
+    previous_error_term = target_delta;
+
+    return output;
+  }
+};
+#else
 template <class T = TemperatureType_t> struct Integrator {
   T sum;
 
@@ -114,11 +161,19 @@ template <class T = TemperatureType_t> struct Integrator {
 
   T get(bool positiveOnly = true) const { return (positiveOnly) ? ((sum > 0) ? sum : 0) : sum; }
 };
-int32_t getPIDResultX10Watts(TemperatureType_t setpointDelta) {
+#endif
+int32_t getPIDResultX10Watts(TemperatureType_t set_point, TemperatureType_t current_reading) {
   static TickType_t lastCall = 0;
-  static Integrator<TemperatureType_t> powerStore = {0};
 
+#ifdef TIP_CONTROL_PID
+  static PID<TemperatureType_t, TIP_PID_KP, TIP_PID_KI, TIP_PID_KD, 5> pid = {0, 0};
+
+  const TickType_t interval = (xTaskGetTickCount() - lastCall);
+
+#else
+  static Integrator<TemperatureType_t> powerStore = {0};
   const TickType_t                     rate       = TICKS_SECOND / (xTaskGetTickCount() - lastCall);
+#endif
   lastCall = xTaskGetTickCount();
   // Sandman note:
   // PID Challenge - we have a small thermal mass that we to want heat up as fast as possible but we don't
@@ -141,11 +196,16 @@ int32_t getPIDResultX10Watts(TemperatureType_t setpointDelta) {
   // tip temperature with (Delta Temperature ) °C in 1 second.
   // Note on powerStore. On update, if the value is provided in X10 (W) units then inertia shall be provided
   // in X10 (J / °C) units as well.
-  return powerStore.update(((TemperatureType_t)getTipThermalMass()) * setpointDelta, // the required power
+
+#ifdef TIP_CONTROL_PID
+  return pid.update(set_point, current_reading, interval, getX10WattageLimits());
+#else
+  return powerStore.update(((TemperatureType_t)getTipThermalMass()) * (set_point - current_reading), // the required power
                            getTipInertia(),                                                          // Inertia, smaller numbers increase dominance of the previous value
                            2,                                                                        // gain
                            rate,                                                                     // PID cycle frequency
                            getX10WattageLimits());
+#endif
 }
 
 void detectThermalRunaway(const TemperatureType_t currentTipTempInC, const TemperatureType_t tError) {