diff --git a/fw/display.c b/fw/display.c
index e9b8631..04e5849 100644
--- a/fw/display.c
+++ b/fw/display.c
@@ -6,15 +6,17 @@
 #include "lib_i2c.h"
 
 
-/*
- * GPIO and delay callback for u8g2/u8x8 display driver
- */
-
-
 static u8g2_t u8g2;
+static const char digits_lut[] =
+    "0001020304050607080910111213141516171819"
+    "2021222324252627282930313233343536373839"
+    "4041424344454647484950515253545556575859"
+    "6061626364656667686970717273747576777879"
+    "8081828384858687888990919293949596979899";
 
 
-uint8_t u8x8_gpio_and_delay(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int, void *arg_ptr)
+// GPIO and delay callback for u8g2/u8x8 display driver
+static uint8_t u8x8_gpio_and_delay(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int, void *arg_ptr)
 {
 	switch(msg) {
 	case U8X8_MSG_GPIO_AND_DELAY_INIT:
@@ -129,7 +131,7 @@ uint8_t u8x8_gpio_and_delay(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int, void *ar
 }
 
 
-uint8_t u8x8_byte_i2c(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int, void *arg_ptr)
+static uint8_t u8x8_byte_i2c(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int, void *arg_ptr)
 {
 	static uint8_t buffer[32];
 	static uint8_t buffer_idx;
@@ -173,5 +175,77 @@ u8g2_t* display_init(void)
 	u8g2_InitDisplay(&u8g2);
 	u8g2_SetPowerSave(&u8g2, 0);
 	u8g2_SetContrast(&u8g2, 255);
+
+	u8g2_SetBitmapMode(&u8g2, 1);
+	u8g2_SetFontMode(&u8g2, 1);
+
 	return &u8g2;
 }
+
+
+const char* u16toa(uint16_t value)
+{
+	// Max uint16_t is 65535 (5 digits) + null terminator
+	static char buf[6];
+	char *p = &buf[5];
+	*p = '\0';
+
+	// Process two digits at a time using the LUT
+	while (value >= 100) {
+		const unsigned int idx = (value % 100) * 2;
+		value /= 100;
+		*--p = digits_lut[idx + 1];
+		*--p = digits_lut[idx];
+	}
+
+	// Handle the remaining value (< 100)
+	if (value < 10) {
+		*--p = (char)('0' + value);
+	} else {
+		const unsigned int idx = value * 2;
+		*--p = digits_lut[idx + 1];
+		*--p = digits_lut[idx];
+	}
+
+	return p;
+}
+
+
+const char* i16toa(int16_t value)
+{
+	static char buf[7];
+	uint16_t uval;
+	bool negative = false;
+
+	if (value < 0) {
+		negative = true;
+		uval = (uint16_t)-value;
+	} else {
+		uval = (uint16_t)value;
+	}
+
+	char *p = &buf[6];
+	*p = '\0';
+
+	// Same LUT logic as unsigned
+	while (uval >= 100) {
+		const unsigned int idx = (uval % 100) * 2;
+		uval /= 100;
+		*--p = digits_lut[idx + 1];
+		*--p = digits_lut[idx];
+	}
+
+	if (uval < 10) {
+		*--p = (char)('0' + uval);
+	} else {
+		const unsigned int idx = uval * 2;
+		*--p = digits_lut[idx + 1];
+		*--p = digits_lut[idx];
+	}
+
+	if (negative) {
+		*--p = '-';
+	}
+
+	return p;
+}
diff --git a/fw/display.h b/fw/display.h
index 635f4da..217806b 100644
--- a/fw/display.h
+++ b/fw/display.h
@@ -8,5 +8,8 @@
 #define PIN_DISP_RST PA7  // display reset
 
 u8g2_t* display_init(void);
+const char* i16toa(int16_t value);
+const char* u16toa(uint16_t value);
+
 
 #endif // _DISPLAY_H
diff --git a/fw/filter.h b/fw/filter.h
index 6d4deb9..7084f42 100644
--- a/fw/filter.h
+++ b/fw/filter.h
@@ -1,20 +1,79 @@
 #ifndef _FILTER_H
 #define _FILTER_H
 
+#include <stdint.h>
+
+
+#define MIN(a, b) ((a) < (b) ? (a) : (b))
+#define MAX(a, b) ((a) > (b) ? (a) : (b))
+
+static_assert(-4 >> 1 == -2, ">> doesn't do sign extension");
 
 // Fixed-Point Exponential Moving Average
 // alpha = 1/2^k
 // x: output value
 // s: current sample
-#define U16_FP_EMA_K2(x, s)  (u16)((((u32)(x)<<2) - (x) + (s)) >> 2)
-#define U16_FP_EMA_K4(x, s)  (u16)((((u32)(x)<<4) - (x) + (s)) >> 4)
-#define U16_FP_EMA_K8(x, s)  (u16)((((u32)(x)<<8) - (x) + (s)) >> 8)
-#define U16_FP_EMA_K16(x, s) (u16)((((u32)(x)<<16) - (x) + (s)) >> 16)
+#define U16_FP_EMA_K2(x, s)  (uint16_t)((((uint32_t)(x)<<2) - (x) + (s)) >> 2)
+#define U16_FP_EMA_K4(x, s)  (uint16_t)((((uint32_t)(x)<<4) - (x) + (s)) >> 4)
+#define U16_FP_EMA_K8(x, s)  (uint16_t)((((uint32_t)(x)<<8) - (x) + (s)) >> 8)
+#define U16_FP_EMA_K16(x, s) (uint16_t)((((uint32_t)(x)<<16) - (x) + (s)) >> 16)
 
-#define I16_FP_EMA_K2(x, s)  (s16)((((s32)(x)<<2) - (x) + (s)) >> 2)
-#define I16_FP_EMA_K4(x, s)  (s16)((((s32)(x)<<4) - (x) + (s)) >> 4)
-#define I16_FP_EMA_K8(x, s)  (s16)((((s32)(x)<<8) - (x) + (s)) >> 8)
-#define I16_FP_EMA_K16(x, s) (s16)((((s32)(x)<<16) - (x) + (s)) >> 16)
+#define I16_FP_EMA_K2(x, s)  (int16_t)((((int32_t)(x)<<2) - (x) + (s)) >> 2)
+#define I16_FP_EMA_K4(x, s)  (int16_t)((((int32_t)(x)<<4) - (x) + (s)) >> 4)
+#define I16_FP_EMA_K8(x, s)  (int16_t)((((int32_t)(x)<<8) - (x) + (s)) >> 8)
+#define I16_FP_EMA_K16(x, s) (int16_t)((((int32_t)(x)<<16) - (x) + (s)) >> 16)
+
+/* ------------------------- FIXED POINT OPERATIONS ------------------------- */
+
+// Fixed-point number in 8.8 format
+typedef int16_t fp16_t;
+#define F32_TO_FP16(f) ((fp16_t)((f) * 256.0f))
+#define I16_TO_FP16(i) ((fp16_t)((i) << 8))
+#define U16_TO_FP16(u) ((fp16_t)((u) << 8))
+
+// Intger part of fp16_t
+#define I(f) ((f) >> 8)
+// Decimal part of fp16_t
+#define D(f) ((f) & 0xFF)
+
+static inline int16_t i16_mul_fp16(int16_t v, fp16_t f)
+{
+	return (((int32_t)(v)<<8) * f) >> 8;
+}
+
+static inline uint16_t u16_mul_fp16(uint16_t v, fp16_t f)
+{
+	return (((uint32_t)(v)<<8) * f) >> 8;
+}
+
+// v * a + b for signed 16-bit
+#define I16_LINCAL(v, a, b) ((int16_t)((((int32_t)(v)<<8) * (a) + (b)) >> 8))
+// v * a + b for unsigned 16-bit
+#define U16_LINCAL(v, a, b) ((uint16_t)((((uint32_t)(v)<<8) * (a) + (b)) >> 8))
+
+
+// Fixed-point number in 24.8 format
+typedef uint32_t fp24_8_t;
+
+static inline fp24_8_t f32_to_fp24_8(float f)
+{
+	return ((fp24_8_t)((f) * 256.0f));
+}
+
+static inline fp24_8_t i16_to_fp24_8(int16_t i)
+{
+	return ((fp24_8_t)(i)) << 8;
+}
+
+static inline fp24_8_t u16_to_fp24_8(uint16_t u)
+{
+	return ((fp24_8_t)(u)) << 8;
+}
+
+static inline fp24_8_t fp24_8_mul(fp24_8_t a, fp24_8_t b)
+{
+	return ((fp24_8_t)((uint32_t)a * b) >> 8);
+}
 
 
 #endif // _FILTER_H
diff --git a/fw/lincal.h b/fw/lincal.h
deleted file mode 100644
index cc5becd..0000000
--- a/fw/lincal.h
+++ /dev/null
@@ -1,22 +0,0 @@
-#ifndef _LINCAL_H
-#define _LINCAL_H
-
-#include <ch32fun.h>
-
-
-// Fixed-point number in 8.8 format
-typedef int16_t fp16_t;
-#define F32_TO_FP16(f) ((fp16_t)((f) * 256.0f))
-
-// Intger part of fp16_t
-#define I(f) ((f) >> 8)
-// Decimal part of fp16_t
-#define D(f) ((f) & 0xFF)
-
-// v * a + b for signed 16-bit
-#define I16_LINCAL(v, a, b) ((int16_t)((((int32_t)(v) * (a)) >> 8) + (b)))
-// v * a + b for unsigned 16-bit
-#define U16_LINCAL(v, a, b) ((uint16_t)((((uint32_t)(v) * (a)) >> 8) + (b)))
-
-
-#endif
diff --git a/fw/main.c b/fw/main.c
index f5b4015..2653cf3 100644
--- a/fw/main.c
+++ b/fw/main.c
@@ -94,6 +94,7 @@ void update_encoder(void)
 
 	// FIXME: even with interrupts disabled and debounce, a single detent triggers
 	// multiple interrupts, leading to multiple encoder updates in a short time.
+	if (funDigitalRead(PIN_BTN) == 0) return;
 	bool a = funDigitalRead(PIN_ENC_A);
 	bool b = funDigitalRead(PIN_ENC_B);
 	Delay_Us(100);
@@ -354,9 +355,6 @@ static inline uint16_t isqrt(uint32_t x)
 }
 
 
-#define MIN(a, b) ((a) < (b) ? (a) : (b))
-
-
 __attribute__((noreturn)) int main(void)
 {
 	SystemInit();
@@ -419,8 +417,6 @@ __attribute__((noreturn)) int main(void)
 	sc7a20_init();
 
 	u8g2_ClearBuffer(u8g2);
-	u8g2_SetBitmapMode(u8g2, 1);
-	u8g2_SetFontMode(u8g2, 1);
 	u8g2_SetFont(u8g2, u8g2_font_5x8_tr);
 	u8g2_DrawStr(u8g2, x_off+0, y_off+7, "Negotiating...");
 	u8g2_SendBuffer(u8g2);
@@ -429,8 +425,6 @@ __attribute__((noreturn)) int main(void)
 	bool has_pd = pd_negotiate(eUSBPD_VCC_3V3);
 	if (has_pd == false) {
 		u8g2_ClearBuffer(u8g2);
-		u8g2_SetBitmapMode(u8g2, 1);
-		u8g2_SetFontMode(u8g2, 1);
 		u8g2_SetFont(u8g2, u8g2_font_5x8_tr);
 		u8g2_DrawStr(u8g2, x_off+0, y_off+7, "Negotiation FAILED");
 		u8g2_DrawStr(u8g2, x_off+0, y_off+14, USBPD_ResultToStr(pd_get_result()));
@@ -449,8 +443,6 @@ __attribute__((noreturn)) int main(void)
 			, 100);
 
 		u8g2_ClearBuffer(u8g2);
-		u8g2_SetBitmapMode(u8g2, 1);
-		u8g2_SetFontMode(u8g2, 1);
 		u8g2_SetFont(u8g2, u8g2_font_5x8_tr);
 		// Display tip temperature
 		u8g2_DrawStr(u8g2, x_off+0, y_off+7, "A:");
@@ -478,8 +470,6 @@ __attribute__((noreturn)) int main(void)
 		poll_input(); // usb
 
 		u8g2_ClearBuffer(u8g2);
-		u8g2_SetBitmapMode(u8g2, 1);
-		u8g2_SetFontMode(u8g2, 1);
 		u8g2_SetFont(u8g2, u8g2_font_5x8_tr);
 
 		static bool pwm = false; // PWM status
@@ -493,6 +483,7 @@ __attribute__((noreturn)) int main(void)
 			static uint16_t vbus_mv, current_ma;
 			static int16_t temp_c, tip_temp_c;
 			static uint16_t power;
+			static fp24_8_t e;
 			vbus_mv = U16_FP_EMA_K4(vbus_mv, ((u32)adc_buffer[0]*VCC_MV*11)/4096);
 			current_ma = U16_FP_EMA_K4(current_ma, get_current_ma(adc_buffer[1]));
 			temp_c = I16_FP_EMA_K4(temp_c, get_temp_c(adc_buffer[2]));
@@ -528,16 +519,17 @@ __attribute__((noreturn)) int main(void)
 			case STATE_HEATING:
 				// Display tip temperature
 				u8g2_DrawStr(u8g2, x_off+0, y_off+7, "TIP:");
-				u8g2_DrawStr(u8g2, x_off+20, y_off+7, u8g2_u16toa(tip_temp_c, 4));
+				u8g2_DrawStr(u8g2, x_off+20, y_off+7, u16toa(tip_temp_c));
 				// Display bus voltage
 				u8g2_DrawStr(u8g2, x_off+45, y_off+7, "V:");
-				u8g2_DrawStr(u8g2, x_off+55, y_off+7, u8g2_u16toa(vbus_mv/1000, 2));
+				u8g2_DrawStr(u8g2, x_off+55, y_off+7, u16toa(vbus_mv/1000));
 				// Display power
-				u8g2_DrawStr(u8g2, x_off+0, y_off+15, "W:");
-				u8g2_DrawStr(u8g2, x_off+10, y_off+15, u8g2_u16toa(power, 3));
+				//u8g2_DrawStr(u8g2, x_off+0, y_off+15, "W:");
+				//u8g2_DrawStr(u8g2, x_off+10, y_off+15, u8g2_u16toa(power, 3));
+				u8g2_DrawStr(u8g2, x_off+0, y_off+15, i16toa(pd_profile.set_temp - tip_temp_c));
 				// Display current
 				u8g2_DrawStr(u8g2, x_off+45, y_off+15, "A:");
-				u8g2_DrawStr(u8g2, x_off+55, y_off+15, u8g2_u16toa(current_ma, 5));
+				u8g2_DrawStr(u8g2, x_off+55, y_off+15, u16toa(current_ma));
 
 
 				if (enabled) {
@@ -566,8 +558,23 @@ __attribute__((noreturn)) int main(void)
 
 					if (pwm) {
 						const uint16_t tim_max = FUNCONF_SYSTEM_CORE_CLOCK / PWM_FREQ_HZ - 1;
+						static int16_t err_p, err_i, err_d, prev_delta;
+
 						int16_t delta = pd_profile.set_temp - tip_temp_c;
-						uint16_t duty = MIN((25*pd_profile.max_duty*delta)/(pd_profile.set_temp*10), pd_profile.max_duty);
+						err_p = delta;
+						err_i += delta;
+						err_i = MAX(-1000, MIN(1000, err_i));
+						err_d = delta - prev_delta;
+						prev_delta = delta;
+
+						const fp24_8_t kp = f32_to_fp24_8(0.8f);
+						const fp24_8_t ki = f32_to_fp24_8(0.15f);
+						const fp24_8_t kd = f32_to_fp24_8(0.0f);
+						e = fp24_8_mul(i16_to_fp24_8(err_p), kp) +
+							fp24_8_mul(i16_to_fp24_8(err_i), ki) +
+							fp24_8_mul(i16_to_fp24_8(err_d), kd);
+						uint16_t duty = MAX(0, MIN(I(e), pd_profile.max_duty));
+
 						pwm_set(((u32)duty*tim_max)/100);
 						u8g2_DrawBox(u8g2, x_off+92, y_off+12, 4, 4);
 					} else {