diff --git a/hardware/firmware/box_rp2040/src/main.c b/hardware/firmware/box_rp2040/src/main.c
index 879993e..9d3d1c3 100644
--- a/hardware/firmware/box_rp2040/src/main.c
+++ b/hardware/firmware/box_rp2040/src/main.c
@@ -20,6 +20,7 @@ void init(void) {
 void update(void) {
     usb_task();
     io_task();
+    pwr_brd_fan_task();
 }
 
 int main(void) {
diff --git a/hardware/firmware/box_rp2040/src/pwr_brd_ctl/fan_ctl.c b/hardware/firmware/box_rp2040/src/pwr_brd_ctl/fan_ctl.c
index 37a45cc..036500d 100644
--- a/hardware/firmware/box_rp2040/src/pwr_brd_ctl/fan_ctl.c
+++ b/hardware/firmware/box_rp2040/src/pwr_brd_ctl/fan_ctl.c
@@ -10,6 +10,16 @@
 
 #define FAN_CTL_I2C_TIMEOUT 200000
 
+uint64_t time_last_cmd[NUMFAN];
+uint16_t desired_fan_speed[NUMFAN];
+
+void pwr_brd_fan_init() {
+    for (int i = 0; i < NUMFAN; i++) {
+        time_last_cmd[i] = 0;
+        desired_fan_speed[i] = DESIRED_RPM;
+    }
+}
+
 bool fan_ctl_i2c_read(uint8_t reg_id, uint8_t* dest) {
     return pwr_brd_i2c_read_reg(PWR_BRD_FAN_CTL_ADDR, reg_id, dest, 1);
 }
@@ -26,12 +36,15 @@ bool fan_ctl_i2c_write_and_check(uint8_t reg_id, uint8_t val) {
     if (!pwr_brd_i2c_read_reg(PWR_BRD_FAN_CTL_ADDR, reg_id, &rval, 1)) {
         return false;
     }
+
     if (rval != val) {
         return false;
     }
     return true;
 }
 
+#define FAN_TACH_CONSTANT (7864320)
+
 bool fan_ctl_get_fan_speed(uint8_t fan_id, uint16_t* dest) {
     uint8_t lsb, msb;
     if (!fan_ctl_i2c_read(EMC2301_REG_TACHREADMSB + 0x10*fan_id, &msb)) {
@@ -50,7 +63,7 @@ bool fan_ctl_get_fan_speed(uint8_t fan_id, uint16_t* dest) {
     // Based on the FOSDEM fans, documentation and guesswork
     // rpm = 7864320 / tacho
 
-    uint16_t rpm = 7864320 / tacho;
+    uint16_t rpm = FAN_TACH_CONSTANT / tacho;
 
     *dest = rpm;
     return true;
@@ -74,23 +87,10 @@ bool fan_ctl_set_pwm(uint8_t fan_id, uint8_t duty) {
 }
 
 bool fan_ctl_set_fan_speed(uint8_t fan_id, uint16_t speed) {
-    uint8_t fanconfig1;
-    if (!fan_ctl_i2c_read(EMC2301_REG_FANCONFIG1 + 0x10*fan_id, &fanconfig1)) {
-        return false;
-    }
-    fanconfig1 |= (1 << 7);    // enable PID controller
-    if (!fan_ctl_i2c_write_and_check(EMC2301_REG_FANCONFIG1 + 0x10*fan_id, fanconfig1)) {
+    if (speed < 100 || speed > 12000)
         return false;
-    }
 
-    uint8_t lsb = (speed << 3) && 0xf8;
-    uint8_t msb = (speed >> 5) && 0xff;
-    if (!fan_ctl_i2c_write(EMC2301_REG_TACHTARGETLSB + 0x10 * fan_id, lsb)) {
-        return false;
-    }
-    if (!fan_ctl_i2c_write(EMC2301_REG_TACHTARGETMSB + 0x10 * fan_id, msb)) {
-        return false;
-    }
+    desired_fan_speed[fan_id] = speed;
     return true;
 }
 
@@ -98,3 +98,58 @@ bool fan_ctl_get_fan_status(uint8_t* dest) {
     return fan_ctl_i2c_read(EMC2301_REG_FANSTATUS, dest);
 }
 
+
+#define likely(x)       __builtin_expect(!!(x), 1)
+#define unlikely(x)     __builtin_expect(!!(x), 0)
+
+/*
+
+Standard fan control:
+
+- if fan speed is 0, go to max
+- if fan speed is above needed by more than 2x, lower pwm = pwm / 2
+- if fan speed is above needed by less than 2x, lower pwm = pwm - 1
+- if fan speed is below needed, pwm = pwm + 1
+- any change is given 1s to take effect
+- the fan speed is within a threshold, so if we are at less than desired + threshold and above desired - threshold, we do nothing
+
+*/
+
+void pwr_brd_fan_task() {
+    static uint16_t cnt=0;
+    int i;
+    uint64_t now;
+
+    cnt++;
+
+    if (likely(cnt % FAN_WAIT_LOOPS != 0))
+        return;
+
+    now = time_us_64();
+    for (i = 0; i < NUMFAN; i++) {
+        if (time_last_cmd[i] + CMD_WAIT_TIME_US < now) continue;
+
+        {
+            uint16_t fanspeed;
+            uint8_t pwm;
+
+            fan_ctl_get_fan_speed(i, &fanspeed);
+            if (fanspeed > desired_fan_speed[i] - DESIRED_RPM_THRESH && fanspeed < desired_fan_speed[i] + DESIRED_RPM_THRESH) {
+                continue;
+            } else if (fanspeed == 0) {
+                fan_ctl_set_pwm(i, 255);
+            } else if (fanspeed > desired_fan_speed[i] * 2) {
+                fan_ctl_get_pwm(i, &pwm);
+                fan_ctl_set_pwm(i, pwm / 2 );
+            } else if (fanspeed > desired_fan_speed[i] + DESIRED_RPM_THRESH) {
+                fan_ctl_get_pwm(i, &pwm);
+                fan_ctl_set_pwm(i, pwm - 1 );
+            } else if (fanspeed < desired_fan_speed[i] - DESIRED_RPM_THRESH) {
+                fan_ctl_get_pwm(i, &pwm);
+                fan_ctl_set_pwm(i, pwm + 1 );
+            }
+            time_last_cmd[i] = now;
+        }
+    }
+
+}
diff --git a/hardware/firmware/box_rp2040/src/pwr_brd_ctl/pwr_brd_ctl.c b/hardware/firmware/box_rp2040/src/pwr_brd_ctl/pwr_brd_ctl.c
index 0e75420..98ed74c 100644
--- a/hardware/firmware/box_rp2040/src/pwr_brd_ctl/pwr_brd_ctl.c
+++ b/hardware/firmware/box_rp2040/src/pwr_brd_ctl/pwr_brd_ctl.c
@@ -14,6 +14,7 @@ void pwr_brd_ctl_init() {
     gpio_set_function(PWR_BRD_I2C_SDA, GPIO_FUNC_I2C);
     gpio_set_function(PWR_BRD_I2C_SCL, GPIO_FUNC_I2C);
     expander_init(&expander, PWR_BRD_I2C_INST, PWR_BRD_EXPANDER_ADDR, PWR_BRD_I2C_TIMEOUT_US);
+    pwr_brd_fan_init();
 }
 
 bool pwr_brd_raw_gpio_read(uint8_t* val) {
diff --git a/hardware/firmware/box_rp2040/src/pwr_brd_ctl/pwr_brd_ctl.h b/hardware/firmware/box_rp2040/src/pwr_brd_ctl/pwr_brd_ctl.h
index f3f600d..abff993 100644
--- a/hardware/firmware/box_rp2040/src/pwr_brd_ctl/pwr_brd_ctl.h
+++ b/hardware/firmware/box_rp2040/src/pwr_brd_ctl/pwr_brd_ctl.h
@@ -3,7 +3,16 @@
 #include <stdbool.h>
 #include <stdint.h>
 
+#define FAN_WAIT_LOOPS (1000)
+#define CMD_WAIT_TIME_US (1000*1000)
+#define DESIRED_RPM (3000)
+#define DESIRED_RPM_THRESH (1000)
+#define NUMFAN (5)
+
+
 void pwr_brd_ctl_init();
+void pwr_brd_fan_task();
+void pwr_brd_fan_init();
 bool pwr_brd_raw_gpio_read(uint8_t* val);
 void pwr_brd_i2c_bus_scan();
 void pwr_brd_i2c_dump_all_regs(uint8_t addr);