Simple integrating fan controller (#1022)

* fast rpm measurement

* fix indentation

* this seems stable

* clip fan integral

* fix misra

* add fan power to health

* board-specific max rpm

* refactor fan enable

* cleanup

* stall detection and reset

Co-authored-by: Comma Device <device@comma.ai>

board/boards/black.h

@@ -210,6 +210,7 @@ const board board_black = {
   .has_obd = true,
   .has_lin = false,
   .has_rtc_battery = false,
+  .fan_max_rpm = 0U,
   .init = black_init,
   .enable_can_transceiver = black_enable_can_transceiver,
   .enable_can_transceivers = black_enable_can_transceivers,
@@ -220,7 +221,7 @@ const board board_black = {
   .usb_power_mode_tick = unused_usb_power_mode_tick,
   .check_ignition = black_check_ignition,
   .read_current = unused_read_current,
-  .set_fan_power = unused_set_fan_power,
+  .set_fan_enabled = unused_set_fan_enabled,
   .set_ir_power = unused_set_ir_power,
   .set_phone_power = unused_set_phone_power,
   .set_clock_source_mode = unused_set_clock_source_mode,

board/boards/board_declarations.h

@@ -10,7 +10,7 @@ typedef void (*board_usb_power_mode_tick)(uint32_t uptime);
 typedef bool (*board_check_ignition)(void);
 typedef uint32_t (*board_read_current)(void);
 typedef void (*board_set_ir_power)(uint8_t percentage);
-typedef void (*board_set_fan_power)(uint8_t percentage);
+typedef void (*board_set_fan_enabled)(bool enabled);
 typedef void (*board_set_phone_power)(bool enabled);
 typedef void (*board_set_clock_source_mode)(uint8_t mode);
 typedef void (*board_set_siren)(bool enabled);
@@ -23,6 +23,7 @@ struct board {
   const bool has_obd;
   const bool has_lin;
   const bool has_rtc_battery;
+  const uint16_t fan_max_rpm;
   board_init init;
   board_enable_can_transceiver enable_can_transceiver;
   board_enable_can_transceivers enable_can_transceivers;
@@ -34,7 +35,7 @@ struct board {
   board_check_ignition check_ignition;
   board_read_current read_current;
   board_set_ir_power set_ir_power;
-  board_set_fan_power set_fan_power;
+  board_set_fan_enabled set_fan_enabled;
   board_set_phone_power set_phone_power;
   board_set_clock_source_mode set_clock_source_mode;
   board_set_siren set_siren;

board/boards/dos.h

@@ -114,10 +114,8 @@ void dos_set_ir_power(uint8_t percentage){
   pwm_set(TIM4, 2, percentage);
 }
 
-void dos_set_fan_power(uint8_t percentage){
-  // Enable fan power only if percentage is non-zero.
-  set_gpio_output(GPIOA, 1, (percentage != 0U));
-  fan_set_power(percentage);
+void dos_set_fan_enabled(bool enabled){
+  set_gpio_output(GPIOA, 1, enabled);
 }
 
 void dos_set_clock_source_mode(uint8_t mode){
@@ -159,7 +157,7 @@ void dos_init(void) {
 
   // Initialize fan and set to 0%
   fan_init();
-  dos_set_fan_power(0U);
+  dos_set_fan_enabled(false);
 
   // Initialize harness
   harness_init();
@@ -209,6 +207,7 @@ const board board_dos = {
   .has_obd = true,
   .has_lin = false,
   .has_rtc_battery = true,
+  .fan_max_rpm = 6500U,
   .init = dos_init,
   .enable_can_transceiver = dos_enable_can_transceiver,
   .enable_can_transceivers = dos_enable_can_transceivers,
@@ -219,7 +218,7 @@ const board board_dos = {
   .usb_power_mode_tick = unused_usb_power_mode_tick,
   .check_ignition = dos_check_ignition,
   .read_current = unused_read_current,
-  .set_fan_power = dos_set_fan_power,
+  .set_fan_enabled = dos_set_fan_enabled,
   .set_ir_power = dos_set_ir_power,
   .set_phone_power = unused_set_phone_power,
   .set_clock_source_mode = dos_set_clock_source_mode,

board/boards/grey.h

@@ -41,6 +41,7 @@ const board board_grey = {
   .has_obd = false,
   .has_lin = true,
   .has_rtc_battery = false,
+  .fan_max_rpm = 0U,
   .init = grey_init,
   .enable_can_transceiver = white_enable_can_transceiver,
   .enable_can_transceivers = white_enable_can_transceivers,
@@ -51,7 +52,7 @@ const board board_grey = {
   .usb_power_mode_tick = unused_usb_power_mode_tick,
   .check_ignition = white_check_ignition,
   .read_current = white_read_current,
-  .set_fan_power = unused_set_fan_power,
+  .set_fan_enabled = unused_set_fan_enabled,
   .set_ir_power = unused_set_ir_power,
   .set_phone_power = unused_set_phone_power,
   .set_clock_source_mode = unused_set_clock_source_mode,

board/boards/pedal.h

@@ -84,6 +84,7 @@ const board board_pedal = {
   .has_obd = false,
   .has_lin = false,
   .has_rtc_battery = false,
+  .fan_max_rpm = 0U,
   .init = pedal_init,
   .enable_can_transceiver = pedal_enable_can_transceiver,
   .enable_can_transceivers = pedal_enable_can_transceivers,
@@ -94,7 +95,7 @@ const board board_pedal = {
   .usb_power_mode_tick = unused_usb_power_mode_tick,
   .check_ignition = pedal_check_ignition,
   .read_current = unused_read_current,
-  .set_fan_power = unused_set_fan_power,
+  .set_fan_enabled = unused_set_fan_enabled,
   .set_ir_power = unused_set_ir_power,
   .set_phone_power = unused_set_phone_power,
   .set_clock_source_mode = unused_set_clock_source_mode,

board/boards/red.h

@@ -187,6 +187,7 @@ const board board_red = {
   .has_obd = true,
   .has_lin = false,
   .has_rtc_battery = false,
+  .fan_max_rpm = 0U,
   .init = red_init,
   .enable_can_transceiver = red_enable_can_transceiver,
   .enable_can_transceivers = red_enable_can_transceivers,
@@ -197,7 +198,7 @@ const board board_red = {
   .usb_power_mode_tick = unused_usb_power_mode_tick,
   .check_ignition = red_check_ignition,
   .read_current = unused_read_current,
-  .set_fan_power = unused_set_fan_power,
+  .set_fan_enabled = unused_set_fan_enabled,
   .set_ir_power = unused_set_ir_power,
   .set_phone_power = unused_set_phone_power,
   .set_clock_source_mode = unused_set_clock_source_mode,

board/boards/uno.h

@@ -167,10 +167,8 @@ void uno_set_ir_power(uint8_t percentage){
   pwm_set(TIM4, 2, percentage);
 }
 
-void uno_set_fan_power(uint8_t percentage){
-  // Enable fan power only if percentage is non-zero.
-  set_gpio_output(GPIOA, 1, (percentage != 0U));
-  fan_set_power(percentage);
+void uno_set_fan_enabled(bool enabled){
+  set_gpio_output(GPIOA, 1, enabled);
 }
 
 void uno_init(void) {
@@ -213,7 +211,7 @@ void uno_init(void) {
 
   // Initialize fan and set to 0%
   fan_init();
-  uno_set_fan_power(0U);
+  uno_set_fan_enabled(false);
 
   // Initialize harness
   harness_init();
@@ -270,6 +268,7 @@ const board board_uno = {
   .has_obd = true,
   .has_lin = false,
   .has_rtc_battery = true,
+  .fan_max_rpm = 5100U,
   .init = uno_init,
   .enable_can_transceiver = uno_enable_can_transceiver,
   .enable_can_transceivers = uno_enable_can_transceivers,
@@ -280,7 +279,7 @@ const board board_uno = {
   .usb_power_mode_tick = uno_usb_power_mode_tick,
   .check_ignition = uno_check_ignition,
   .read_current = unused_read_current,
-  .set_fan_power = uno_set_fan_power,
+  .set_fan_enabled = uno_set_fan_enabled,
   .set_ir_power = uno_set_ir_power,
   .set_phone_power = uno_set_phone_power,
   .set_clock_source_mode = unused_set_clock_source_mode,

board/boards/unused_funcs.h

@@ -10,8 +10,8 @@ void unused_set_ir_power(uint8_t percentage) {
   UNUSED(percentage);
 }
 
-void unused_set_fan_power(uint8_t percentage) {
-  UNUSED(percentage);
+void unused_set_fan_enabled(bool enabled) {
+  UNUSED(enabled);
 }
 
 void unused_set_phone_power(bool enabled) {

board/boards/white.h

@@ -249,6 +249,7 @@ const board board_white = {
   .has_obd = false,
   .has_lin = true,
   .has_rtc_battery = false,
+  .fan_max_rpm = 0U,
   .init = white_init,
   .enable_can_transceiver = white_enable_can_transceiver,
   .enable_can_transceivers = white_enable_can_transceivers,
@@ -259,7 +260,7 @@ const board board_white = {
   .usb_power_mode_tick = unused_usb_power_mode_tick,
   .check_ignition = white_check_ignition,
   .read_current = white_read_current,
-  .set_fan_power = unused_set_fan_power,
+  .set_fan_enabled = unused_set_fan_enabled,
   .set_ir_power = unused_set_ir_power,
   .set_phone_power = unused_set_phone_power,
   .set_clock_source_mode = unused_set_clock_source_mode,

board/drivers/fan.h

@@ -1,14 +1,56 @@
-uint16_t fan_tach_counter = 0U;
-uint16_t fan_rpm = 0U;
+struct fan_state_t {
+  uint16_t tach_counter;
+  uint16_t rpm;
+  uint16_t target_rpm;
+  uint8_t power;
+  float error_integral;
+  uint8_t stall_counter;
+} fan_state_t;
+struct fan_state_t fan_state;
+
+#define FAN_I 0.001f
+#define FAN_STALL_THRESHOLD 25U
 
 void fan_set_power(uint8_t percentage){
-  pwm_set(TIM3, 3, percentage);
+  fan_state.target_rpm = ((current_board->fan_max_rpm * MIN(100U, MAX(0U, percentage))) / 100U);
 }
 
-// Can be way more acurate than this, but this is probably good enough for our purposes.
-// Call this every second
+// Call this at 8Hz
 void fan_tick(void){
+  if (current_board->fan_max_rpm > 0U) {
     // 4 interrupts per rotation
-    fan_rpm = fan_tach_counter * 15U;
-    fan_tach_counter = 0U;
+    uint16_t fan_rpm_fast = fan_state.tach_counter * (60U * 8U / 4U);
+    fan_state.tach_counter = 0U;
+    fan_state.rpm = (fan_rpm_fast + (3U * fan_state.rpm)) / 4U;
+
+    // Enable fan if we want it to spin
+    current_board->set_fan_enabled(fan_state.target_rpm > 0U);
+
+    // Stall detection
+    if(fan_state.power > 0U) {
+      if (fan_rpm_fast == 0U) {
+        fan_state.stall_counter = MIN(fan_state.stall_counter + 1U, 255U);
+      } else {
+        fan_state.stall_counter = 0U;
+      }
+
+      if (fan_state.stall_counter > FAN_STALL_THRESHOLD) {
+        // Stall detected, power cycling fan controller
+        current_board->set_fan_enabled(false);
+        fan_state.error_integral = 0U;
+      }
+    } else {
+      fan_state.stall_counter = 0U;
+    }
+
+    // Update controller
+    float feedforward = (fan_state.target_rpm * 100.0f) / current_board->fan_max_rpm;
+    float error = fan_state.target_rpm - fan_rpm_fast;
+
+    fan_state.error_integral += FAN_I * error;
+    fan_state.error_integral = MIN(70.0f, MAX(-70.0f, fan_state.error_integral));
+
+    fan_state.power = MIN(100U, MAX(0U, feedforward + fan_state.error_integral));
+    pwm_set(TIM3, 3, fan_state.power);
+  }
 }

board/health.h

@@ -1,5 +1,5 @@
 // When changing this struct, python/__init__.py needs to be kept up to date!
-#define HEALTH_PACKET_VERSION 7
+#define HEALTH_PACKET_VERSION 8
 struct __attribute__((packed)) health_t {
   uint32_t uptime_pkt;
   uint32_t voltage_pkt;
@@ -23,4 +23,5 @@ struct __attribute__((packed)) health_t {
   uint16_t alternative_experience_pkt;
   uint32_t blocked_msg_cnt_pkt;
   float interrupt_load;
+  uint8_t fan_power;
 };

board/main.c

@@ -157,6 +157,9 @@ void tick_handler(void) {
     // siren
     current_board->set_siren((loop_counter & 1U) && (siren_enabled || (siren_countdown > 0U)));
 
+    // tick drivers
+    fan_tick();
+
     // decimated to 1Hz
     if (loop_counter == 0U) {
       can_live = pending_can_live;
@@ -177,9 +180,6 @@ void tick_handler(void) {
         puts("tx3:"); puth4(can_tx3_q.r_ptr); puts("-"); puth4(can_tx3_q.w_ptr); puts("\n");
       #endif
 
-      // Tick drivers
-      fan_tick();
-
       // set green LED to be controls allowed
       current_board->set_led(LED_GREEN, controls_allowed | green_led_enabled);
 
@@ -243,9 +243,9 @@ void tick_handler(void) {
 
           // If enumerated but no heartbeat (phone up, boardd not running), turn the fan on to cool the device
           if(usb_enumerated){
-            current_board->set_fan_power(50U);
+            fan_set_power(50U);
           } else {
-            current_board->set_fan_power(0U);
+            fan_set_power(0U);
           }
         }
 
@@ -423,7 +423,7 @@ int main(void) {
     } else {
       if (deepsleep_allowed && !usb_enumerated && !check_started() && ignition_seen && (heartbeat_counter > 20U)) {
         usb_soft_disconnect(true);
-        current_board->set_fan_power(0U);
+        fan_set_power(0U);
         current_board->set_usb_power_mode(USB_POWER_CLIENT);
         NVIC_DisableIRQ(TICK_TIMER_IRQ);
         delay(512000U);

board/main_comms.h

@@ -38,6 +38,8 @@ int get_health_pkt(void *dat) {
 
   health->interrupt_load = interrupt_load;
 
+  health->fan_power = fan_state.power;
+
   return sizeof(*health);
 }
 
@@ -229,12 +231,12 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) {
       break;
     // **** 0xb1: set fan power
     case 0xb1:
-      current_board->set_fan_power(req->param1);
+      fan_set_power(req->param1);
       break;
     // **** 0xb2: get fan rpm
     case 0xb2:
-      resp[0] = (fan_rpm & 0x00FFU);
-      resp[1] = ((fan_rpm & 0xFF00U) >> 8U);
+      resp[0] = (fan_state.rpm & 0x00FFU);
+      resp[1] = ((fan_state.rpm & 0xFF00U) >> 8U);
       resp_len = 2;
       break;
     // **** 0xb3: set phone power

board/stm32fx/llfan.h

@@ -2,7 +2,7 @@
 void EXTI2_IRQ_Handler(void) {
     volatile unsigned int pr = EXTI->PR & (1U << 2);
     if ((pr & (1U << 2)) != 0U) {
-        fan_tach_counter++;
+        fan_state.tach_counter++;
     }
     EXTI->PR = (1U << 2);
 }

python/__init__.py

@@ -173,8 +173,8 @@ class Panda:
   HW_TYPE_RED_PANDA = b'\x07'
 
   CAN_PACKET_VERSION = 2
-  HEALTH_PACKET_VERSION = 7
-  HEALTH_STRUCT = struct.Struct("<IIIIIIIIBBBBBBBHBBBHIf")
+  HEALTH_PACKET_VERSION = 8
+  HEALTH_STRUCT = struct.Struct("<IIIIIIIIBBBBBBBHBBBHIfB")
 
   F2_DEVICES = (HW_TYPE_PEDAL, )
   F4_DEVICES = (HW_TYPE_WHITE_PANDA, HW_TYPE_GREY_PANDA, HW_TYPE_BLACK_PANDA, HW_TYPE_UNO, HW_TYPE_DOS)
@@ -438,6 +438,7 @@ class Panda:
       "alternative_experience": a[19],
       "blocked_msg_cnt": a[20],
       "interrupt_load": a[21],
+      "fan_power": a[22],
     }
 
   # ******************* control *******************

tests/fan_test.py

@@ -9,9 +9,10 @@ from panda import Panda  # noqa: E402
 power = 0
 if __name__ == "__main__":
   p = Panda()
+  p.set_heartbeat_disabled()
   while True:
     p.set_fan_power(power)
     time.sleep(5)
-    print("Power: ", power, "RPM: ", str(p.get_fan_rpm()))
+    print("Power: ", power, "RPM:", str(p.get_fan_rpm()), "Expected:", int(6500 * power / 100))
     power += 10
-    power %= 100
+    power %= 110