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>
2022-08-17 20:43:49 -07:00 · 2022-08-17 20:43:49 -07:00 · ba8772123f
parent c7c33219ed
commit ba8772123f
16 changed files with 95 additions and 44 deletions
--- a/board/boards/black.h
+++ b/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,
--- a/board/boards/board_declarations.h
+++ b/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;
--- a/board/boards/dos.h
+++ b/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,
--- a/board/boards/grey.h
+++ b/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,
--- a/board/boards/pedal.h
+++ b/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,
--- a/board/boards/red.h
+++ b/board/boards/red.h
@ -83,7 +83,7 @@ void red_set_can_mode(uint8_t mode) {

        set_gpio_pullup(GPIOB, 13, PULL_NONE);
        set_gpio_mode(GPIOB, 13, MODE_ANALOG);
-        
+
        // B5,B6: FDCAN2 mode
        set_gpio_pullup(GPIOB, 5, PULL_NONE);
        set_gpio_alternate(GPIOB, 5, GPIO_AF9_FDCAN2);
@ -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,
--- a/board/boards/uno.h
+++ b/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,
--- a/board/boards/unused_funcs.h
+++ b/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) {
--- a/board/boards/white.h
+++ b/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,
--- a/board/drivers/fan.h
+++ b/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);
+  }
 }
--- a/board/health.h
+++ b/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;
 };
--- a/board/main.c
+++ b/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);
--- a/board/main_comms.h
+++ b/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
--- a/board/stm32fx/llfan.h
+++ b/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);
 }
--- a/python/init.py
+++ b/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 *******************
--- a/tests/fan_test.py
+++ b/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