diff --git a/board/boards/black.h b/board/boards/black.h index 3cf58aef6..a1297345b 100644 --- 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, diff --git a/board/boards/board_declarations.h b/board/boards/board_declarations.h index eb9a33f19..50a7ee451 100644 --- 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; diff --git a/board/boards/dos.h b/board/boards/dos.h index e1252ecaa..38d647037 100644 --- 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, diff --git a/board/boards/grey.h b/board/boards/grey.h index 6f0a55b2c..4ff6f8987 100644 --- 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, diff --git a/board/boards/pedal.h b/board/boards/pedal.h index 1327c6454..72286e646 100644 --- 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, diff --git a/board/boards/red.h b/board/boards/red.h index 4499bea72..ac22f78f6 100644 --- 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, diff --git a/board/boards/uno.h b/board/boards/uno.h index 164b9a31e..f5d097fd0 100644 --- 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, diff --git a/board/boards/unused_funcs.h b/board/boards/unused_funcs.h index e9d59c69a..540106575 100644 --- 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) { diff --git a/board/boards/white.h b/board/boards/white.h index 6c88d621c..58c71a46a 100644 --- 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, diff --git a/board/drivers/fan.h b/board/drivers/fan.h index c273972a1..9d33df70a 100644 --- 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); + } } diff --git a/board/health.h b/board/health.h index 82c476e96..3c7d14422 100644 --- 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; }; diff --git a/board/main.c b/board/main.c index f2c93340c..84e5da911 100644 --- 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); diff --git a/board/main_comms.h b/board/main_comms.h index f2263f933..0472f3e48 100644 --- 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 diff --git a/board/stm32fx/llfan.h b/board/stm32fx/llfan.h index a9d21f19b..403314153 100644 --- 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); } diff --git a/python/__init__.py b/python/__init__.py index 4e6ed2417..e12426d51 100644 --- 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("