Merge branch 'master' of https://github.com/sunnypilot/panda

2026-02-18 17:23:52 +08:00 · 2025-12-13 12:13:49 +01:00
parent 5bd386f4ff 5f3c09c910
commit 3214b53a75
27 changed files with 1084 additions and 166 deletions
--- a/3
+++ b/3
@@ -161,6 +161,9 @@ if os.getenv("FINAL_PROVISIONING"):
  flags += ["-DFINAL_PROVISIONING"]
 build_project("panda_jungle_h7", base_project_h7, "./board/jungle/main.c", flags)

+# body fw
+build_project("body_h7", base_project_h7, "./board/body/main.c", ["-DPANDA_BODY"])
+
 # test files
 if GetOption('extras'):
  SConscript('tests/libpanda/SConscript')
--- a/init.py
+++ b/init.py
@@ -8,3 +8,6 @@ from .python import (Panda, PandaDFU, # noqa: F401

 # panda jungle
 from .board.jungle import PandaJungle, PandaJungleDFU # noqa: F401
+
+# panda body
+from .board.body import PandaBody  # noqa: F401
--- a/board/boards/cuatro.h
+++ b/board/boards/cuatro.h
@@ -128,7 +128,7 @@ board board_cuatro = {
  .read_current_mA = cuatro_read_current_mA,
  .set_fan_enabled = cuatro_set_fan_enabled,
  .set_ir_power = unused_set_ir_power,
-  .set_siren = unused_set_siren,
+  .set_siren = fake_siren_set,
  .set_bootkick = cuatro_set_bootkick,
  .read_som_gpio = tres_read_som_gpio,
  .set_amp_enabled = cuatro_set_amp_enabled
--- a/board/boards/tres.h
+++ b/board/boards/tres.h
@@ -168,7 +168,7 @@ board board_tres = {
  .read_current_mA = unused_read_current,
  .set_fan_enabled = tres_set_fan_enabled,
  .set_ir_power = tres_set_ir_power,
-  .set_siren = fake_siren_set,
+  .set_siren = fake_i2c_siren_set,
  .set_bootkick = tres_set_bootkick,
  .read_som_gpio = tres_read_som_gpio,
  .set_amp_enabled = unused_set_amp_enabled
--- a/board/body/init.py
+++ b/board/body/init.py
@@ -0,0 +1,42 @@
+# python helpers for the body panda
+import struct
+
+from panda import Panda
+
+class PandaBody(Panda):
+
+  MOTOR_LEFT: int = 1
+  MOTOR_RIGHT: int = 2
+
+  # ****************** Motor Control *****************
+  @staticmethod
+  def _ensure_valid_motor(motor: int) -> None:
+    if motor not in (PandaBody.MOTOR_LEFT, PandaBody.MOTOR_RIGHT):
+      raise ValueError("motor must be MOTOR_LEFT or MOTOR_RIGHT")
+
+  def motor_set_speed(self, motor: int, speed: int) -> None:
+    self._ensure_valid_motor(motor)
+    assert -100 <= speed <= 100, "speed must be between -100 and 100"
+    speed_param = speed if speed >= 0 else (256 + speed)
+    self._handle.controlWrite(Panda.REQUEST_OUT, 0xe0, motor, speed_param, b'')
+
+  def motor_set_target_rpm(self, motor: int, rpm: float) -> None:
+    self._ensure_valid_motor(motor)
+    target_deci_rpm = int(round(rpm * 10.0))
+    assert -32768 <= target_deci_rpm <= 32767, "target rpm out of range (-3276.8 to 3276.7)"
+    target_param = target_deci_rpm if target_deci_rpm >= 0 else (target_deci_rpm + (1 << 16))
+    self._handle.controlWrite(Panda.REQUEST_OUT, 0xe4, motor, target_param, b'')
+
+  def motor_stop(self, motor: int) -> None:
+    self._ensure_valid_motor(motor)
+    self._handle.controlWrite(Panda.REQUEST_OUT, 0xe1, motor, 0, b'')
+
+  def motor_get_encoder_state(self, motor: int) -> tuple[int, float]:
+    self._ensure_valid_motor(motor)
+    dat = self._handle.controlRead(Panda.REQUEST_IN, 0xe2, motor, 0, 8)
+    position, rpm_milli = struct.unpack("<ii", dat)
+    return position, rpm_milli / 1000.0
+
+  def motor_reset_encoder(self, motor: int) -> None:
+    self._ensure_valid_motor(motor)
+    self._handle.controlWrite(Panda.REQUEST_OUT, 0xe3, motor, 0, b'')
--- a/board/body/boards/board_body.h
+++ b/board/body/boards/board_body.h
@@ -0,0 +1,21 @@
+#include "board/body/motor_control.h"
+
+void board_body_init(void) {
+  motor_init();
+  motor_encoder_init();
+  motor_speed_controller_init();
+  motor_encoder_reset(1);
+  motor_encoder_reset(2);
+
+  // Initialize CAN pins
+  set_gpio_pullup(GPIOD, 0, PULL_NONE);
+  set_gpio_alternate(GPIOD, 0, GPIO_AF9_FDCAN1);
+  set_gpio_pullup(GPIOD, 1, PULL_NONE);
+  set_gpio_alternate(GPIOD, 1, GPIO_AF9_FDCAN1);
+}
+
+board board_body = {
+  .led_GPIO = {GPIOC, GPIOC, GPIOC},
+  .led_pin = {7, 7, 7},
+  .init = board_body_init,
+};
--- a/board/body/boards/board_declarations.h
+++ b/board/body/boards/board_declarations.h
@@ -0,0 +1,21 @@
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+
+// ******************** Prototypes ********************
+typedef void (*board_init)(void);
+typedef void (*board_init_bootloader)(void);
+typedef void (*board_enable_can_transceiver)(uint8_t transceiver, bool enabled);
+
+struct board {
+  GPIO_TypeDef * const led_GPIO[3];
+  const uint8_t led_pin[3];
+  const uint8_t led_pwm_channels[3]; // leave at 0 to disable PWM
+  board_init init;
+  board_init_bootloader init_bootloader;
+  const bool has_spi;
+};
+
+// ******************* Definitions ********************
+#define HW_TYPE_BODY 0xB1U
--- a/board/body/can.h
+++ b/board/body/can.h
@@ -0,0 +1,76 @@
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "board/can.h"
+#include "board/health.h"
+#include "board/body/motor_control.h"
+#include "board/drivers/can_common_declarations.h"
+#include "opendbc/safety/declarations.h"
+
+#define BODY_CAN_ADDR_MOTOR_SPEED      0x201U
+#define BODY_CAN_MOTOR_SPEED_PERIOD_US 10000U
+#define BODY_BUS_NUMBER                 0U
+
+static struct {
+  bool pending;
+  int32_t left_target_deci_rpm;
+  int32_t right_target_deci_rpm;
+} body_can_command;
+
+void body_can_send_motor_speeds(uint8_t bus, float left_speed_rpm, float right_speed_rpm) {
+  CANPacket_t pkt;
+  pkt.bus = bus;
+  pkt.addr = BODY_CAN_ADDR_MOTOR_SPEED;
+  pkt.returned = 0;
+  pkt.rejected = 0;
+  pkt.extended = 0;
+  pkt.fd = 0;
+  pkt.data_len_code = 8;
+  int16_t left_speed_deci = left_speed_rpm * 10;
+  int16_t right_speed_deci = right_speed_rpm * 10;
+  pkt.data[0] = (uint8_t)((left_speed_deci >> 8) & 0xFFU);
+  pkt.data[1] = (uint8_t)(left_speed_deci & 0xFFU);
+  pkt.data[2] = (uint8_t)((right_speed_deci >> 8) & 0xFFU);
+  pkt.data[3] = (uint8_t)(right_speed_deci & 0xFFU);
+  pkt.data[4] = 0U;
+  pkt.data[5] = 0U;
+  pkt.data[6] = 0U;
+  pkt.data[7] = 0U;
+  can_set_checksum(&pkt);
+  can_send(&pkt, bus, true);
+}
+
+void body_can_process_target(int16_t left_target_deci_rpm, int16_t right_target_deci_rpm) {
+  body_can_command.left_target_deci_rpm = (int32_t)left_target_deci_rpm;
+  body_can_command.right_target_deci_rpm = (int32_t)right_target_deci_rpm;
+  body_can_command.pending = true;
+}
+
+void body_can_init(void) {
+  body_can_command.pending = false;
+  can_silent = false;
+  can_loopback = false;
+  (void)set_safety_hooks(SAFETY_BODY, 0U);
+  set_gpio_output(GPIOD, 2U, 0); // Enable CAN transceiver
+  can_init_all();
+}
+
+void body_can_periodic(uint32_t now) {
+  if (body_can_command.pending) {
+    body_can_command.pending = false;
+    float left_target_rpm = ((float)body_can_command.left_target_deci_rpm) * 0.1f;
+    float right_target_rpm = ((float)body_can_command.right_target_deci_rpm) * 0.1f;
+    motor_speed_controller_set_target_rpm(BODY_MOTOR_LEFT, left_target_rpm);
+    motor_speed_controller_set_target_rpm(BODY_MOTOR_RIGHT, right_target_rpm);
+  }
+
+  static uint32_t last_motor_speed_tx_us = 0;
+  if ((now - last_motor_speed_tx_us) >= BODY_CAN_MOTOR_SPEED_PERIOD_US) {
+    float left_speed_rpm = motor_encoder_get_speed_rpm(BODY_MOTOR_LEFT);
+    float right_speed_rpm = motor_encoder_get_speed_rpm(BODY_MOTOR_RIGHT);
+    body_can_send_motor_speeds(BODY_BUS_NUMBER, left_speed_rpm, right_speed_rpm);
+    last_motor_speed_tx_us = now;
+  }
+}
--- a/board/body/flash.py
+++ b/board/body/flash.py
@@ -0,0 +1,49 @@
+#!/usr/bin/env python3
+import argparse
+import os
+import subprocess
+
+from panda import Panda
+
+BODY_DIR = os.path.dirname(os.path.realpath(__file__))
+BOARD_DIR = os.path.abspath(os.path.join(BODY_DIR, ".."))
+REPO_ROOT = os.path.abspath(os.path.join(BOARD_DIR, ".."))
+DEFAULT_FIRMWARE = os.path.join(BOARD_DIR, "obj", "body_h7.bin.signed")
+
+
+def build_body() -> None:
+  subprocess.check_call(
+    f"scons -C {REPO_ROOT} -j$(nproc) board/obj/body_h7.bin.signed",
+    shell=True,
+  )
+
+
+if __name__ == "__main__":
+  parser = argparse.ArgumentParser()
+  parser.add_argument("firmware", nargs="?", help="Optional path to firmware binary to flash")
+  parser.add_argument("--all", action="store_true", help="Flash all Panda devices")
+  parser.add_argument(
+    "--wait-usb",
+    action="store_true",
+    help="Wait for the panda to reconnect over USB after flashing (defaults to skipping reconnect).",
+  )
+  args = parser.parse_args()
+
+  firmware_path = os.path.abspath(args.firmware) if args.firmware is not None else DEFAULT_FIRMWARE
+
+  build_body()
+
+  if not os.path.isfile(firmware_path):
+    parser.error(f"firmware file not found: {firmware_path}")
+
+  if args.all:
+    serials = Panda.list()
+    print(f"found {len(serials)} panda(s) - {serials}")
+  else:
+    serials = [None]
+
+  for s in serials:
+    with Panda(serial=s) as p:
+      print("flashing", p.get_usb_serial())
+      p.flash(firmware_path, reconnect=args.wait_usb)
+  exit(1 if len(serials) == 0 else 0)
--- a/board/body/main.c
+++ b/board/body/main.c
@@ -0,0 +1,81 @@
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "board/config.h"
+#include "board/drivers/led.h"
+#include "board/drivers/pwm.h"
+#include "board/drivers/usb.h"
+#include "board/early_init.h"
+#include "board/obj/gitversion.h"
+#include "board/body/motor_control.h"
+#include "board/body/can.h"
+#include "opendbc/safety/safety.h"
+#include "board/drivers/can_common.h"
+#include "board/drivers/fdcan.h"
+#include "board/can_comms.h"
+
+extern int _app_start[0xc000];
+
+#include "board/body/main_comms.h"
+
+void debug_ring_callback(uart_ring *ring) {
+  char rcv;
+  while (get_char(ring, &rcv)) {
+    (void)injectc(ring, rcv);
+  }
+}
+
+void __attribute__ ((noinline)) enable_fpu(void) {
+  SCB->CPACR |= ((3UL << (10U * 2U)) | (3UL << (11U * 2U)));
+}
+
+void __initialize_hardware_early(void) {
+  enable_fpu();
+  early_initialization();
+}
+
+volatile uint32_t tick_count = 0;
+
+void tick_handler(void) {
+  if (TICK_TIMER->SR != 0) {
+    if (can_health[0].transmit_error_cnt >= 128) {
+      (void)llcan_init(CANIF_FROM_CAN_NUM(0));
+    }
+    static bool led_on = false;
+    led_set(LED_RED, led_on);
+    led_on = !led_on;
+    tick_count++;
+  }
+  TICK_TIMER->SR = 0;
+}
+
+int main(void) {
+  disable_interrupts();
+  init_interrupts(true);
+
+  clock_init();
+  peripherals_init();
+
+  current_board = &board_body;
+  hw_type = HW_TYPE_BODY;
+
+  current_board->init();
+  
+  REGISTER_INTERRUPT(TICK_TIMER_IRQ, tick_handler, 10U, FAULT_INTERRUPT_RATE_TICK);
+
+  led_init();
+  microsecond_timer_init();
+  tick_timer_init();
+  usb_init();
+  body_can_init();
+
+  enable_interrupts();
+
+  while (true) {
+    uint32_t now = microsecond_timer_get();
+    motor_speed_controller_update(now);
+    body_can_periodic(now);
+  }
+
+  return 0;
+}
--- a/board/body/main_comms.h
+++ b/board/body/main_comms.h
@@ -0,0 +1,105 @@
+void comms_endpoint2_write(const uint8_t *data, uint32_t len) {
+  UNUSED(data);
+  UNUSED(len);
+}
+
+int comms_control_handler(ControlPacket_t *req, uint8_t *resp) {
+  unsigned int resp_len = 0;
+
+  switch (req->request) {
+    // **** 0xc1: get hardware type
+    case 0xc1:
+      resp[0] = hw_type;
+      resp_len = 1;
+      break;
+
+    // **** 0xd1: enter bootloader mode
+    case 0xd1:
+      switch (req->param1) {
+        case 0:
+          print("-> entering bootloader\n");
+          enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC;
+          NVIC_SystemReset();
+          break;
+        case 1:
+          print("-> entering softloader\n");
+          enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC;
+          NVIC_SystemReset();
+          break;
+        default:
+          print("Bootloader mode invalid\n");
+          break;
+      }
+      break;
+
+    // **** 0xd3/0xd4: signature bytes
+    case 0xd3:
+    case 0xd4: {
+      uint8_t offset = (req->request == 0xd3) ? 0U : 64U;
+      resp_len = 64U;
+      char *code = (char *)_app_start;
+      int code_len = _app_start[0];
+      (void)memcpy(resp, &code[code_len + offset], resp_len);
+      break;
+    }
+
+    // **** 0xd6: get version
+    case 0xd6:
+      (void)memcpy(resp, gitversion, sizeof(gitversion));
+      resp_len = sizeof(gitversion) - 1U;
+      break;
+
+    // **** 0xd8: reset ST
+    case 0xd8:
+      NVIC_SystemReset();
+      break;
+
+    // **** 0xe0: set motor speed
+    case 0xe0:
+      motor_set_speed((uint8_t)req->param1, (int8_t)req->param2);
+      break;
+
+    // **** 0xe1: stop motor
+    case 0xe1:
+      motor_set_speed((uint8_t)req->param1, 0);
+      break;
+
+    // **** 0xe2: get motor encoder state
+    case 0xe2: {
+      uint8_t motor = (uint8_t)req->param1;
+      int32_t position = motor_encoder_get_position(motor);
+      float rpm = motor_encoder_get_speed_rpm(motor);
+      int32_t rpm_milli = (int32_t)(rpm * 1000.0f);
+      (void)memcpy(resp, &position, sizeof(position));
+      (void)memcpy(resp + sizeof(position), &rpm_milli, sizeof(rpm_milli));
+      resp_len = (uint8_t)(sizeof(position) + sizeof(rpm_milli));
+      break;
+    }
+
+    // **** 0xe3: reset encoder position
+    case 0xe3:
+      motor_encoder_reset((uint8_t)req->param1);
+      break;
+
+    // **** 0xe4: set motor target speed (rpm * 0.1)
+    case 0xe4: {
+      uint8_t motor = (uint8_t)req->param1;
+      float target_rpm = ((int16_t)req->param2) * 0.1f;
+      motor_speed_controller_set_target_rpm(motor, target_rpm);
+      break;
+    }
+
+    // **** 0xdd: get healthpacket and CANPacket versions
+    case 0xdd:
+      resp[0] = HEALTH_PACKET_VERSION;
+      resp[1] = CAN_PACKET_VERSION;
+      resp[2] = CAN_HEALTH_PACKET_VERSION;
+      resp_len = 3U;
+      break;
+
+    default:
+      // Ignore unhandled requests
+      break;
+  }
+  return resp_len;
+}
--- a/board/body/motor_common.h
+++ b/board/body/motor_common.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#define BODY_MOTOR_COUNT 2U
+
+typedef enum {
+  BODY_MOTOR_LEFT = 1U,
+  BODY_MOTOR_RIGHT = 2U,
+} body_motor_id_e;
+
+static inline bool body_motor_is_valid(uint8_t motor) {
+  return (motor > 0U) && (motor <= BODY_MOTOR_COUNT);
+}
--- a/board/body/motor_control.h
+++ b/board/body/motor_control.h
@@ -0,0 +1,251 @@
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "board/body/motor_common.h"
+#include "board/body/motor_encoder.h"
+
+// Motor pin map:
+// M1 drive: PB8 -> TIM16_CH1, PB9 -> TIM17_CH1, PE2/PE3 enables
+// M2 drive: PA2 -> TIM15_CH1, PA3 -> TIM15_CH2, PE8/PE9 enables
+
+#define PWM_TIMER_CLOCK_HZ 120000000U
+#define PWM_FREQUENCY_HZ 5000U
+#define PWM_PERCENT_MAX 100
+#define PWM_RELOAD_TICKS ((PWM_TIMER_CLOCK_HZ + (PWM_FREQUENCY_HZ / 2U)) / PWM_FREQUENCY_HZ)
+
+#define KP         0.25f
+#define KI         0.5f
+#define KD         0.008f
+#define KFF        0.9f
+#define MAX_RPM    100.0f
+#define OUTPUT_MAX 100.0f
+#define DEADBAND_RPM 1.0f
+#define UPDATE_PERIOD_US 1000U
+
+typedef struct {
+  TIM_TypeDef *forward_timer;
+  uint8_t forward_channel;
+  TIM_TypeDef *reverse_timer;
+  uint8_t reverse_channel;
+  GPIO_TypeDef *pwm_port[2];
+  uint8_t pwm_pin[2];
+  uint8_t pwm_af[2];
+  GPIO_TypeDef *enable_port[2];
+  uint8_t enable_pin[2];
+} motor_pwm_config_t;
+
+static const motor_pwm_config_t motor_pwm_config[BODY_MOTOR_COUNT] = {
+  [BODY_MOTOR_LEFT - 1U] = {
+    TIM16, 1U, TIM17, 1U,
+    {GPIOB, GPIOB}, {8U, 9U}, {GPIO_AF1_TIM16, GPIO_AF1_TIM17},
+    {GPIOE, GPIOE}, {2U, 3U},
+  },
+  [BODY_MOTOR_RIGHT - 1U] = {
+    TIM15, 2U, TIM15, 1U,
+    {GPIOA, GPIOA}, {2U, 3U}, {GPIO_AF4_TIM15, GPIO_AF4_TIM15},
+    {GPIOE, GPIOE}, {8U, 9U},
+  },
+};
+
+typedef struct {
+  bool active;
+  float target_rpm;
+  float integral;
+  float previous_error;
+  float last_output;
+  uint32_t last_update_us;
+} motor_speed_state_t;
+
+static inline float motor_absf(float value) {
+  return (value < 0.0f) ? -value : value;
+}
+
+static inline float motor_clampf(float value, float min_value, float max_value) {
+  if (value < min_value) {
+    return min_value;
+  }
+  if (value > max_value) {
+    return max_value;
+  }
+  return value;
+}
+
+static motor_speed_state_t motor_speed_states[BODY_MOTOR_COUNT];
+
+static inline void motor_pwm_write(TIM_TypeDef *timer, uint8_t channel, uint8_t percentage) {
+  uint32_t period = (timer->ARR != 0U) ? timer->ARR : PWM_RELOAD_TICKS;
+  uint16_t comp = (uint16_t)((period * (uint32_t)percentage) / 100U);
+  if (channel == 1U) {
+    register_set(&(timer->CCR1), comp, 0xFFFFU);
+  } else if (channel == 2U) {
+    register_set(&(timer->CCR2), comp, 0xFFFFU);
+  }
+}
+
+static inline motor_speed_state_t *motor_speed_state_get(uint8_t motor) {
+  return body_motor_is_valid(motor) ? &motor_speed_states[motor - 1U] : NULL;
+}
+
+static inline void motor_speed_state_reset(motor_speed_state_t *state) {
+  state->active = false;
+  state->target_rpm = 0.0f;
+  state->integral = 0.0f;
+  state->previous_error = 0.0f;
+  state->last_output = 0.0f;
+  state->last_update_us = 0U;
+}
+
+static void motor_speed_controller_init(void) {
+  for (uint8_t i = 0U; i < BODY_MOTOR_COUNT; i++) {
+    motor_speed_state_reset(&motor_speed_states[i]);
+  }
+}
+
+static void motor_speed_controller_disable(uint8_t motor) {
+  motor_speed_state_t *state = motor_speed_state_get(motor);
+  if (state == NULL) {
+    return;
+  }
+  motor_speed_state_reset(state);
+}
+
+static inline void motor_write_enable(uint8_t motor_index, bool enable) {
+  const motor_pwm_config_t *cfg = &motor_pwm_config[motor_index];
+  uint8_t level = enable ? 1U : 0U;
+  set_gpio_output(cfg->enable_port[0], cfg->enable_pin[0], level);
+  set_gpio_output(cfg->enable_port[1], cfg->enable_pin[1], level);
+}
+
+static void motor_init(void) {
+  register_set_bits(&(RCC->AHB4ENR), RCC_AHB4ENR_GPIOAEN | RCC_AHB4ENR_GPIOBEN | RCC_AHB4ENR_GPIOEEN);
+  register_set_bits(&(RCC->APB2ENR), RCC_APB2ENR_TIM16EN | RCC_APB2ENR_TIM17EN | RCC_APB2ENR_TIM15EN);
+
+  for (uint8_t i = 0U; i < BODY_MOTOR_COUNT; i++) {
+    const motor_pwm_config_t *cfg = &motor_pwm_config[i];
+    motor_write_enable(i, false);
+    set_gpio_alternate(cfg->pwm_port[0], cfg->pwm_pin[0], cfg->pwm_af[0]);
+    set_gpio_alternate(cfg->pwm_port[1], cfg->pwm_pin[1], cfg->pwm_af[1]);
+
+    pwm_init(cfg->forward_timer, cfg->forward_channel);
+    pwm_init(cfg->reverse_timer, cfg->reverse_channel);
+
+    TIM_TypeDef *forward_timer = cfg->forward_timer;
+    register_set(&(forward_timer->PSC), 0U, 0xFFFFU);
+    register_set(&(forward_timer->ARR), PWM_RELOAD_TICKS, 0xFFFFU);
+    forward_timer->EGR |= TIM_EGR_UG;
+    register_set(&(forward_timer->BDTR), TIM_BDTR_MOE, 0xFFFFU);
+
+    if (cfg->reverse_timer != cfg->forward_timer) {
+      TIM_TypeDef *reverse_timer = cfg->reverse_timer;
+      register_set(&(reverse_timer->PSC), 0U, 0xFFFFU);
+      register_set(&(reverse_timer->ARR), PWM_RELOAD_TICKS, 0xFFFFU);
+      reverse_timer->EGR |= TIM_EGR_UG;
+      register_set(&(reverse_timer->BDTR), TIM_BDTR_MOE, 0xFFFFU);
+    }
+  }
+}
+
+static void motor_apply_pwm(uint8_t motor, int32_t speed_command) {
+  if (!body_motor_is_valid(motor)) {
+    return;
+  }
+
+  int8_t speed = (int8_t)motor_clampf((float)speed_command, -(float)PWM_PERCENT_MAX, (float)PWM_PERCENT_MAX);
+  uint8_t pwm_value = (uint8_t)((speed < 0) ? -speed : speed);
+  uint8_t motor_index = motor - 1U;
+  motor_write_enable(motor_index, speed != 0);
+  const motor_pwm_config_t *cfg = &motor_pwm_config[motor_index];
+
+  if (speed > 0) {
+    motor_pwm_write(cfg->forward_timer, cfg->forward_channel, pwm_value);
+    motor_pwm_write(cfg->reverse_timer, cfg->reverse_channel, 0U);
+  } else if (speed < 0) {
+    motor_pwm_write(cfg->forward_timer, cfg->forward_channel, 0U);
+    motor_pwm_write(cfg->reverse_timer, cfg->reverse_channel, pwm_value);
+  } else {
+    motor_pwm_write(cfg->forward_timer, cfg->forward_channel, 0U);
+    motor_pwm_write(cfg->reverse_timer, cfg->reverse_channel, 0U);
+  }
+}
+
+static inline void motor_set_speed(uint8_t motor, int8_t speed) {
+  motor_speed_controller_disable(motor);
+  motor_apply_pwm(motor, (int32_t)speed);
+}
+
+static inline void motor_speed_controller_set_target_rpm(uint8_t motor, float target_rpm) {
+  motor_speed_state_t *state = motor_speed_state_get(motor);
+  if (state == NULL) {
+    return;
+  }
+
+  target_rpm = motor_clampf(target_rpm, -MAX_RPM, MAX_RPM);
+  if (motor_absf(target_rpm) <= DEADBAND_RPM) {
+    motor_speed_controller_disable(motor);
+    motor_apply_pwm(motor, 0);
+    return;
+  }
+
+  state->active = true;
+  state->target_rpm = target_rpm;
+  state->integral = 0.0f;
+  state->previous_error = target_rpm - motor_encoder_get_speed_rpm(motor);
+  state->last_output = 0.0f;
+  state->last_update_us = 0U;
+}
+
+static inline void motor_speed_controller_update(uint32_t now_us) {
+  for (uint8_t motor = BODY_MOTOR_LEFT; motor <= BODY_MOTOR_RIGHT; motor++) {
+    motor_speed_state_t *state = motor_speed_state_get(motor);
+    if (!state->active) {
+      continue;
+    }
+
+    bool first_update = (state->last_update_us == 0U);
+    uint32_t dt_us = first_update ? UPDATE_PERIOD_US : (now_us - state->last_update_us);
+    if (!first_update && (dt_us < UPDATE_PERIOD_US)) {
+      continue;
+    }
+
+    float measured_rpm = motor_encoder_get_speed_rpm(motor);
+    float error = state->target_rpm - measured_rpm;
+
+    if ((motor_absf(state->target_rpm) <= DEADBAND_RPM) && (motor_absf(error) <= DEADBAND_RPM) && (motor_absf(measured_rpm) <= DEADBAND_RPM)) {
+      motor_speed_controller_disable(motor);
+      motor_apply_pwm(motor, 0);
+      continue;
+    }
+
+    float dt_s = (float)dt_us * 1.0e-6f;
+    float control = KFF * state->target_rpm;
+
+    if (dt_s > 0.0f) {
+      state->integral += error * dt_s;
+      float derivative = first_update ? 0.0f : (error - state->previous_error) / dt_s;
+
+      control += (KP * error) + (KI * state->integral) + (KD * derivative);
+    } else {
+      state->integral = 0.0f;
+      control += KP * error;
+    }
+
+    if ((state->target_rpm > 0.0f) && (control < 0.0f)) {
+      control = 0.0f;
+      state->integral = 0.0f;
+    } else if ((state->target_rpm < 0.0f) && (control > 0.0f)) {
+      control = 0.0f;
+      state->integral = 0.0f;
+    }
+
+    control = motor_clampf(control, -OUTPUT_MAX, OUTPUT_MAX);
+
+    int32_t command = (control >= 0.0f) ? (int32_t)(control + 0.5f) : (int32_t)(control - 0.5f);
+    motor_apply_pwm(motor, command);
+
+    state->previous_error = error;
+    state->last_output = control;
+    state->last_update_us = now_us;
+  }
+}
--- a/board/body/motor_encoder.h
+++ b/board/body/motor_encoder.h
@@ -0,0 +1,170 @@
+#pragma once
+
+#include <stdint.h>
+
+#include "board/body/motor_common.h"
+
+// Encoder pin map:
+// Left motor:  PB6 -> TIM4_CH1, PB7 -> TIM4_CH2
+// Right motor: PA6 -> TIM3_CH1, PA7 -> TIM3_CH2
+
+typedef struct {
+  TIM_TypeDef *timer;
+  GPIO_TypeDef *pin_a_port;
+  uint8_t pin_a;
+  uint8_t pin_a_af;
+  GPIO_TypeDef *pin_b_port;
+  uint8_t pin_b;
+  uint8_t pin_b_af;
+  int8_t direction;
+  uint32_t counts_per_output_rev;
+  uint32_t min_dt_us;
+  float speed_alpha;
+  uint32_t filter;
+} motor_encoder_config_t;
+
+typedef struct {
+  const motor_encoder_config_t *config;
+  uint16_t last_timer_count;
+  int32_t accumulated_count;
+  int32_t last_speed_count;
+  uint32_t last_speed_timestamp_us;
+  float cached_speed_rps;
+} motor_encoder_state_t;
+
+static const motor_encoder_config_t motor_encoder_config[BODY_MOTOR_COUNT] = {
+  [BODY_MOTOR_LEFT - 1U] = {
+    .timer = TIM4,
+    .pin_a_port = GPIOB, .pin_a = 6U, .pin_a_af = GPIO_AF2_TIM4,
+    .pin_b_port = GPIOB, .pin_b = 7U, .pin_b_af = GPIO_AF2_TIM4,
+    .direction = -1,
+    .counts_per_output_rev = 44U * 90U,
+    .min_dt_us = 250U,
+    .speed_alpha = 0.2f,
+    .filter = 3U,
+  },
+  [BODY_MOTOR_RIGHT - 1U] = {
+    .timer = TIM3,
+    .pin_a_port = GPIOA, .pin_a = 6U, .pin_a_af = GPIO_AF2_TIM3,
+    .pin_b_port = GPIOA, .pin_b = 7U, .pin_b_af = GPIO_AF2_TIM3,
+    .direction = 1,
+    .counts_per_output_rev = 44U * 90U,
+    .min_dt_us = 250U,
+    .speed_alpha = 0.2f,
+    .filter = 3U,
+  },
+};
+
+static motor_encoder_state_t motor_encoders[BODY_MOTOR_COUNT] = {
+  { .config = &motor_encoder_config[0] },
+  { .config = &motor_encoder_config[1] },
+};
+
+static void motor_encoder_configure_gpio(const motor_encoder_config_t *cfg) {
+  set_gpio_pullup(cfg->pin_a_port, cfg->pin_a, PULL_UP);
+  set_gpio_output_type(cfg->pin_a_port, cfg->pin_a, OUTPUT_TYPE_PUSH_PULL);
+  set_gpio_alternate(cfg->pin_a_port, cfg->pin_a, cfg->pin_a_af);
+
+  set_gpio_pullup(cfg->pin_b_port, cfg->pin_b, PULL_UP);
+  set_gpio_output_type(cfg->pin_b_port, cfg->pin_b, OUTPUT_TYPE_PUSH_PULL);
+  set_gpio_alternate(cfg->pin_b_port, cfg->pin_b, cfg->pin_b_af);
+}
+
+static void motor_encoder_configure_timer(motor_encoder_state_t *state) {
+  const motor_encoder_config_t *cfg = state->config;
+  TIM_TypeDef *timer = cfg->timer;
+  timer->CR1 = 0U;
+  timer->CR2 = 0U;
+  timer->SMCR = 0U;
+  timer->DIER = 0U;
+  timer->SR = 0U;
+  timer->CCMR1 = (TIM_CCMR1_CC1S_0) | (TIM_CCMR1_CC2S_0) | (cfg->filter << TIM_CCMR1_IC1F_Pos) | (cfg->filter << TIM_CCMR1_IC2F_Pos);
+  timer->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E;
+  timer->PSC = 0U;
+  timer->ARR = 0xFFFFU;
+  timer->CNT = 0U;
+  state->last_timer_count = 0U;
+  state->accumulated_count = 0;
+  state->last_speed_count = 0;
+  state->cached_speed_rps = 0.0f;
+  timer->SMCR = (TIM_SMCR_SMS_0 | TIM_SMCR_SMS_1);
+  timer->CR1 = TIM_CR1_CEN;
+}
+
+static void motor_encoder_init(void) {
+  register_set_bits(&(RCC->APB1LENR), RCC_APB1LENR_TIM4EN | RCC_APB1LENR_TIM3EN);
+  register_set_bits(&(RCC->APB1LRSTR), RCC_APB1LRSTR_TIM4RST | RCC_APB1LRSTR_TIM3RST);
+  register_clear_bits(&(RCC->APB1LRSTR), RCC_APB1LRSTR_TIM4RST | RCC_APB1LRSTR_TIM3RST);
+
+  for (uint8_t i = 0U; i < BODY_MOTOR_COUNT; i++) {
+    motor_encoder_state_t *state = &motor_encoders[i];
+    const motor_encoder_config_t *cfg = state->config;
+    motor_encoder_configure_gpio(cfg);
+    motor_encoder_configure_timer(state);
+    state->last_speed_timestamp_us = 0U;
+  }
+}
+
+static inline int32_t motor_encoder_refresh(motor_encoder_state_t *state) {
+  const motor_encoder_config_t *cfg = state->config;
+  TIM_TypeDef *timer = cfg->timer;
+  uint16_t raw = (uint16_t)timer->CNT;
+  int32_t delta = (int16_t)(raw - state->last_timer_count);
+  delta *= cfg->direction;
+  state->last_timer_count = raw;
+  state->accumulated_count += delta;
+  return state->accumulated_count;
+}
+
+static inline int32_t motor_encoder_get_position(uint8_t motor) {
+  if (!body_motor_is_valid(motor)) {
+    return 0;
+  }
+  motor_encoder_state_t *state = &motor_encoders[motor - 1U];
+  return motor_encoder_refresh(state);
+}
+
+static void motor_encoder_reset(uint8_t motor) {
+  if (!body_motor_is_valid(motor)) {
+    return;
+  }
+  motor_encoder_state_t *state = &motor_encoders[motor - 1U];
+  state->config->timer->CNT = 0U;
+  state->last_timer_count = 0U;
+  state->accumulated_count = 0;
+  state->last_speed_count = 0;
+  state->last_speed_timestamp_us = 0U;
+  state->cached_speed_rps = 0.0f;
+}
+
+static float motor_encoder_get_speed_rpm(uint8_t motor) {
+  if (!body_motor_is_valid(motor)) {
+    return 0.0f;
+  }
+  motor_encoder_state_t *state = &motor_encoders[motor - 1U];
+
+  const motor_encoder_config_t *cfg = state->config;
+  motor_encoder_refresh(state);
+
+  uint32_t now = microsecond_timer_get();
+  if (state->last_speed_timestamp_us == 0U) {
+    state->last_speed_timestamp_us = now;
+    state->last_speed_count = state->accumulated_count;
+    state->cached_speed_rps = 0.0f;
+    return 0.0f;
+  }
+
+  uint32_t dt = now - state->last_speed_timestamp_us;
+  int32_t delta = state->accumulated_count - state->last_speed_count;
+  if ((dt < cfg->min_dt_us) || (delta == 0)) {
+    return state->cached_speed_rps * 60.0f;
+  }
+
+  state->last_speed_count = state->accumulated_count;
+  state->last_speed_timestamp_us = now;
+
+  float counts_per_second = ((float)delta * 1000000.0f) / (float)dt;
+  float new_speed_rps = (cfg->counts_per_output_rev != 0U) ? (counts_per_second / (float)cfg->counts_per_output_rev) : 0.0f;
+  state->cached_speed_rps += cfg->speed_alpha * (new_speed_rps - state->cached_speed_rps);
+  return state->cached_speed_rps * 60.0f;
+}
--- a/board/body/stm32h7/board.h
+++ b/board/body/stm32h7/board.h
@@ -0,0 +1,9 @@
+#include "board/body/boards/board_declarations.h"
+#include "board/body/boards/board_body.h"
+
+extern board *current_board;
+extern uint8_t hw_type;
+
+void detect_board_type(void) {
+  // Board type set explicitly in main()
+}
--- a/board/drivers/fake_siren.h
+++ b/board/drivers/fake_siren.h
@@ -2,7 +2,36 @@

 #define CODEC_I2C_ADDR 0x10

-void fake_siren_init(void);
+void siren_tim7_init(void) {
+  // Init trigger timer (around 2.5kHz)
+  register_set(&TIM7->PSC, 0U, 0xFFFFU);
+  register_set(&TIM7->ARR, 133U, 0xFFFFU);
+  register_set(&TIM7->CR2, (0b10U << TIM_CR2_MMS_Pos), TIM_CR2_MMS_Msk);
+  register_set(&TIM7->CR1, TIM_CR1_ARPE | TIM_CR1_URS, 0x088EU);
+  TIM7->SR = 0U;
+  TIM7->CR1 |= TIM_CR1_CEN;
+}
+
+void siren_dac_init(void) {
+  DAC1->DHR8R1 = (1U << 7);
+  DAC1->DHR8R2 = (1U << 7);
+  register_set(&DAC1->MCR, 0U, 0xFFFFFFFFU);
+  register_set(&DAC1->CR, DAC_CR_TEN1 | (6U << DAC_CR_TSEL1_Pos) | DAC_CR_DMAEN1, 0xFFFFFFFFU);
+  register_set_bits(&DAC1->CR, DAC_CR_EN1 | DAC_CR_EN2);
+}
+
+void siren_dma_init(void) {
+  // 1Vpp sine wave with 1V offset
+  static const uint8_t fake_siren_lut[360] = { 134U, 135U, 137U, 138U, 139U, 140U, 141U, 143U, 144U, 145U, 146U, 148U, 149U, 150U, 151U, 152U, 154U, 155U, 156U, 157U, 158U, 159U, 160U, 162U, 163U, 164U, 165U, 166U, 167U, 168U, 169U, 170U, 171U, 172U, 174U, 175U, 176U, 177U, 177U, 178U, 179U, 180U, 181U, 182U, 183U, 184U, 185U, 186U, 186U, 187U, 188U, 189U, 190U, 190U, 191U, 192U, 193U, 193U, 194U, 195U, 195U, 196U, 196U, 197U, 197U, 198U, 199U, 199U, 199U, 200U, 200U, 201U, 201U, 202U, 202U, 202U, 203U, 203U, 203U, 203U, 204U, 204U, 204U, 204U, 204U, 204U, 204U, 205U, 205U, 205U, 205U, 205U, 205U, 205U, 204U, 204U, 204U, 204U, 204U, 204U, 204U, 203U, 203U, 203U, 203U, 202U, 202U, 202U, 201U, 201U, 200U, 200U, 199U, 199U, 199U, 198U, 197U, 197U, 196U, 196U, 195U, 195U, 194U, 193U, 193U, 192U, 191U, 190U, 190U, 189U, 188U, 187U, 186U, 186U, 185U, 184U, 183U, 182U, 181U, 180U, 179U, 178U, 177U, 177U, 176U, 175U, 174U, 172U, 171U, 170U, 169U, 168U, 167U, 166U, 165U, 164U, 163U, 162U, 160U, 159U, 158U, 157U, 156U, 155U, 154U, 152U, 151U, 150U, 149U, 148U, 146U, 145U, 144U, 143U, 141U, 140U, 139U, 138U, 137U, 135U, 134U, 133U, 132U, 130U, 129U, 128U, 127U, 125U, 124U, 123U, 122U, 121U, 119U, 118U, 117U, 116U, 115U, 113U, 112U, 111U, 110U, 109U, 108U, 106U, 105U, 104U, 103U, 102U, 101U, 100U, 99U, 98U, 97U, 96U, 95U, 94U, 93U, 92U, 91U, 90U, 89U, 88U, 87U, 86U, 85U, 84U, 83U, 82U, 82U, 81U, 80U, 79U, 78U, 78U, 77U, 76U, 76U, 75U, 74U, 74U, 73U, 72U, 72U, 71U, 71U, 70U, 70U, 69U, 69U, 68U, 68U, 67U, 67U, 67U, 66U, 66U, 66U, 65U, 65U, 65U, 65U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 63U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 65U, 65U, 65U, 65U, 66U, 66U, 66U, 67U, 67U, 67U, 68U, 68U, 69U, 69U, 70U, 70U, 71U, 71U, 72U, 72U, 73U, 74U, 74U, 75U, 76U, 76U, 77U, 78U, 78U, 79U, 80U, 81U, 82U, 82U, 83U, 84U, 85U, 86U, 87U, 88U, 89U, 90U, 91U, 92U, 93U, 94U, 95U, 96U, 97U, 98U, 99U, 100U, 101U, 102U, 103U, 104U, 105U, 106U, 108U, 109U, 110U, 111U, 112U, 113U, 115U, 116U, 117U, 118U, 119U, 121U, 122U, 123U, 124U, 125U, 127U, 128U, 129U, 130U, 132U, 133U };
+  // Setup DMAMUX (DAC_CH1_DMA as input)
+  register_set(&DMAMUX1_Channel1->CCR, 67U, DMAMUX_CxCR_DMAREQ_ID_Msk);
+  register_set(&DMA1_Stream1->PAR, (uint32_t)&(DAC1->DHR8R1), 0xFFFFFFFFU);
+  // Setup DMA
+  register_set(&DMA1_Stream1->M0AR, (uint32_t)fake_siren_lut, 0xFFFFFFFFU);
+  register_set(&DMA1_Stream1->FCR, 0U, 0x00000083U);
+  DMA1_Stream1->NDTR = sizeof(fake_siren_lut);
+  DMA1_Stream1->CR = (0b11UL << DMA_SxCR_PL_Pos) | DMA_SxCR_MINC | DMA_SxCR_CIRC | (1U << DMA_SxCR_DIR_Pos);
+}

 void fake_siren_codec_enable(bool enabled) {
  if (enabled) {
@@ -28,13 +57,21 @@ void fake_siren_codec_enable(bool enabled) {
  }
 }

+static void fake_i2c_siren_init(void) {
+  siren_dac_init();
+  siren_dma_init();
+  siren_tim7_init();
+  // Enable the I2C to the codec
+  i2c_init(I2C5);
+  fake_siren_codec_enable(false);
+}

-void fake_siren_set(bool enabled) {
+void fake_i2c_siren_set(bool enabled) {
  static bool initialized = false;
  static bool fake_siren_enabled = false;

  if (!initialized) {
-    fake_siren_init();
+    fake_i2c_siren_init();
    initialized = true;
  }

@@ -50,35 +87,29 @@ void fake_siren_set(bool enabled) {
  fake_siren_enabled = enabled;
 }

-void fake_siren_init(void) {
-  // 1Vpp sine wave with 1V offset
-  static const uint8_t fake_siren_lut[360] = { 134U, 135U, 137U, 138U, 139U, 140U, 141U, 143U, 144U, 145U, 146U, 148U, 149U, 150U, 151U, 152U, 154U, 155U, 156U, 157U, 158U, 159U, 160U, 162U, 163U, 164U, 165U, 166U, 167U, 168U, 169U, 170U, 171U, 172U, 174U, 175U, 176U, 177U, 177U, 178U, 179U, 180U, 181U, 182U, 183U, 184U, 185U, 186U, 186U, 187U, 188U, 189U, 190U, 190U, 191U, 192U, 193U, 193U, 194U, 195U, 195U, 196U, 196U, 197U, 197U, 198U, 199U, 199U, 199U, 200U, 200U, 201U, 201U, 202U, 202U, 202U, 203U, 203U, 203U, 203U, 204U, 204U, 204U, 204U, 204U, 204U, 204U, 205U, 205U, 205U, 205U, 205U, 205U, 205U, 204U, 204U, 204U, 204U, 204U, 204U, 204U, 203U, 203U, 203U, 203U, 202U, 202U, 202U, 201U, 201U, 200U, 200U, 199U, 199U, 199U, 198U, 197U, 197U, 196U, 196U, 195U, 195U, 194U, 193U, 193U, 192U, 191U, 190U, 190U, 189U, 188U, 187U, 186U, 186U, 185U, 184U, 183U, 182U, 181U, 180U, 179U, 178U, 177U, 177U, 176U, 175U, 174U, 172U, 171U, 170U, 169U, 168U, 167U, 166U, 165U, 164U, 163U, 162U, 160U, 159U, 158U, 157U, 156U, 155U, 154U, 152U, 151U, 150U, 149U, 148U, 146U, 145U, 144U, 143U, 141U, 140U, 139U, 138U, 137U, 135U, 134U, 133U, 132U, 130U, 129U, 128U, 127U, 125U, 124U, 123U, 122U, 121U, 119U, 118U, 117U, 116U, 115U, 113U, 112U, 111U, 110U, 109U, 108U, 106U, 105U, 104U, 103U, 102U, 101U, 100U, 99U, 98U, 97U, 96U, 95U, 94U, 93U, 92U, 91U, 90U, 89U, 88U, 87U, 86U, 85U, 84U, 83U, 82U, 82U, 81U, 80U, 79U, 78U, 78U, 77U, 76U, 76U, 75U, 74U, 74U, 73U, 72U, 72U, 71U, 71U, 70U, 70U, 69U, 69U, 68U, 68U, 67U, 67U, 67U, 66U, 66U, 66U, 65U, 65U, 65U, 65U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 63U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 65U, 65U, 65U, 65U, 66U, 66U, 66U, 67U, 67U, 67U, 68U, 68U, 69U, 69U, 70U, 70U, 71U, 71U, 72U, 72U, 73U, 74U, 74U, 75U, 76U, 76U, 77U, 78U, 78U, 79U, 80U, 81U, 82U, 82U, 83U, 84U, 85U, 86U, 87U, 88U, 89U, 90U, 91U, 92U, 93U, 94U, 95U, 96U, 97U, 98U, 99U, 100U, 101U, 102U, 103U, 104U, 105U, 106U, 108U, 109U, 110U, 111U, 112U, 113U, 115U, 116U, 117U, 118U, 119U, 121U, 122U, 123U, 124U, 125U, 127U, 128U, 129U, 130U, 132U, 133U };
+void fake_siren_set(bool enabled) {
+  static bool initialized = false;
+  static bool fake_siren_enabled = false;

-  // Init DAC
-  register_set(&DAC1->MCR, 0U, 0xFFFFFFFFU);
-  register_set(&DAC1->CR, DAC_CR_TEN1 | (6U << DAC_CR_TSEL1_Pos) | DAC_CR_DMAEN1, 0xFFFFFFFFU);
-  register_set_bits(&DAC1->CR, DAC_CR_EN1);
+  if (!initialized) {
+    siren_tim7_init();
+    initialized = true;
+  }

-  // Setup DMAMUX (DAC_CH1_DMA as input)
-  register_set(&DMAMUX1_Channel1->CCR, 67U, DMAMUX_CxCR_DMAREQ_ID_Msk);
-
-  // Setup DMA
-  register_set(&DMA1_Stream1->M0AR, (uint32_t) fake_siren_lut, 0xFFFFFFFFU);
-  register_set(&DMA1_Stream1->PAR, (uint32_t) &(DAC1->DHR8R1), 0xFFFFFFFFU);
-  DMA1_Stream1->NDTR = sizeof(fake_siren_lut);
-  register_set(&DMA1_Stream1->FCR, 0U, 0x00000083U);
-  DMA1_Stream1->CR = (0b11UL << DMA_SxCR_PL_Pos);
-  DMA1_Stream1->CR |= DMA_SxCR_MINC | DMA_SxCR_CIRC | (1U << DMA_SxCR_DIR_Pos);
-
-  // Init trigger timer (around 2.5kHz)
-  register_set(&TIM7->PSC, 0U, 0xFFFFU);
-  register_set(&TIM7->ARR, 133U, 0xFFFFU);
-  register_set(&TIM7->CR2, (0b10U << TIM_CR2_MMS_Pos), TIM_CR2_MMS_Msk);
-  register_set(&TIM7->CR1, TIM_CR1_ARPE | TIM_CR1_URS, 0x088EU);
-  TIM7->SR = 0U;
-  TIM7->CR1 |= TIM_CR1_CEN;
-
-  // Enable the I2C to the codec
-  i2c_init(I2C5);
-  fake_siren_codec_enable(false);
+  if (enabled != fake_siren_enabled) {
+    if (enabled) {
+      sound_stop_dac();
+      siren_dac_init();
+      siren_dma_init();
+      current_board->set_amp_enabled(true);
+      register_set_bits(&DMA1_Stream1->CR, DMA_SxCR_EN);
+    } else {
+      current_board->set_amp_enabled(false);
+      // Stop modified 8-bit DAC and start normal 12-bit DAC
+      sound_stop_dac();
+      sound_init_dac();
+      register_set_bits(&BDMA_Channel0->CCR, BDMA_CCR_EN);
+    }
+    fake_siren_enabled = enabled;
+  }
 }
--- a/board/stm32h7/board.h
+++ b/board/stm32h7/board.h
@@ -9,8 +9,8 @@
 #include "board/drivers/harness.h"
 #include "board/drivers/fan.h"
 #include "board/stm32h7/llfan.h"
-#include "board/drivers/fake_siren.h"
 #include "board/stm32h7/sound.h"
+#include "board/drivers/fake_siren.h"
 #include "board/drivers/clock_source.h"
 #include "board/boards/red.h"
 #include "board/boards/tres.h"
--- a/board/stm32h7/sound.h
+++ b/board/stm32h7/sound.h
@@ -5,10 +5,13 @@
 __attribute__((section(".sram4"))) static uint16_t sound_rx_buf[2][SOUND_RX_BUF_SIZE];
 __attribute__((section(".sram4"))) static uint16_t sound_tx_buf[2][SOUND_TX_BUF_SIZE];
 __attribute__((section(".sram4"))) static uint32_t mic_rx_buf[2][MIC_RX_BUF_SIZE];
+__attribute__((section(".sram4"))) static uint16_t mic_tx_buf[2][MIC_TX_BUF_SIZE];

 #define SOUND_IDLE_TIMEOUT 4U
+#define MIC_SKIP_BUFFERS 2U // Skip first 2 buffers (1024 samples = ~21ms at 48kHz)
 static uint8_t sound_idle_count;
 static uint8_t mic_idle_count;
+static uint8_t mic_buffer_count;

 void sound_tick(void) {
  if (sound_idle_count > 0U) {
@@ -23,28 +26,31 @@ void sound_tick(void) {
    mic_idle_count--;
    if (mic_idle_count == 0U) {
      register_clear_bits(&DFSDM1_Channel0->CHCFGR1, DFSDM_CHCFGR1_DFSDMEN);
+      mic_buffer_count = 0U;
    }
  }
 }

 // Recording processing
 static void DMA1_Stream0_IRQ_Handler(void) {
-  __attribute__((section(".sram4"))) static uint16_t tx_buf[MIC_TX_BUF_SIZE];
+  DMA1->LIFCR |= 0x7DU; // clear flags

-  DMA1->LIFCR |= 0x7D; // clear flags
+  uint8_t tx_buf_idx = (((BDMA_Channel1->CCR & BDMA_CCR_CT) >> BDMA_CCR_CT_Pos) == 1U) ? 0U : 1U;

-  // process samples
-  uint8_t buf_idx = (((DMA1_Stream0->CR & DMA_SxCR_CT) >> DMA_SxCR_CT_Pos) == 1U) ? 0U : 1U;
-  for (uint16_t i=0U; i < MIC_RX_BUF_SIZE; i++) {
-    tx_buf[2U*i] = ((mic_rx_buf[buf_idx][i] >> 16U) & 0xFFFFU);
-    tx_buf[(2U*i)+1U] = tx_buf[2U*i];
+  if (mic_buffer_count < MIC_SKIP_BUFFERS) {
+    // Send silence during settling
+    mic_buffer_count++;
+    for (uint16_t i = 0U; i < MIC_TX_BUF_SIZE; i++) {
+      mic_tx_buf[tx_buf_idx][i] = 0U;
+    }
+  } else {
+    // process samples
+    uint8_t buf_idx = (((DMA1_Stream0->CR & DMA_SxCR_CT) >> DMA_SxCR_CT_Pos) == 1U) ? 0U : 1U;
+    for (uint16_t i=0U; i < MIC_RX_BUF_SIZE; i++) {
+      mic_tx_buf[tx_buf_idx][2U*i] = ((mic_rx_buf[buf_idx][i] >> 16U) & 0xFFFFU);
+      mic_tx_buf[tx_buf_idx][(2U*i)+1U] = mic_tx_buf[tx_buf_idx][2U*i];
+    }
  }
-
-  BDMA->IFCR |= BDMA_IFCR_CGIF1;
-  BDMA_Channel1->CCR &= ~BDMA_CCR_EN;
-  register_set(&BDMA_Channel1->CM0AR, (uint32_t) tx_buf, 0xFFFFFFFFU);
-  BDMA_Channel1->CNDTR = MIC_TX_BUF_SIZE;
-  BDMA_Channel1->CCR |= BDMA_CCR_EN;
 }

 // Playback processing
@@ -59,7 +65,7 @@ static void BDMA_Channel0_IRQ_Handler(void) {
  // wait until we're done playing the current buf
  for (uint32_t timeout_counter = 100000U; timeout_counter > 0U; timeout_counter--){
    uint8_t playing_buf = (DMA1_Stream1->CR & DMA_SxCR_CT) >> DMA_SxCR_CT_Pos;
-    if ((playing_buf != playback_buf) || (!(DMA1_Stream1->CR & DMA_SxCR_EN))) {
+    if ((playing_buf != playback_buf) || ((DMA1_Stream1->CR & DMA_SxCR_EN) == 0U)) {
      break;
    }
  }
@@ -100,10 +106,7 @@ static void BDMA_Channel0_IRQ_Handler(void) {
  sound_tick();
 }

-void sound_init(void) {
-  REGISTER_INTERRUPT(BDMA_Channel0_IRQn, BDMA_Channel0_IRQ_Handler, 128U, FAULT_INTERRUPT_RATE_SOUND_DMA)
-  REGISTER_INTERRUPT(DMA1_Stream0_IRQn, DMA1_Stream0_IRQ_Handler, 128U, FAULT_INTERRUPT_RATE_SOUND_DMA)
-
+void sound_init_dac(void) {
  // Init DAC
  DAC1->DHR12R1 = (1UL << 11);
  DAC1->DHR12R2 = (1UL << 11);
@@ -121,10 +124,30 @@ void sound_init(void) {
  register_set(&DMA1_Stream1->FCR, 0U, 0x00000083U);
  DMA1_Stream1->NDTR = SOUND_TX_BUF_SIZE;
  DMA1_Stream1->CR = DMA_SxCR_DBM | (0b11UL << DMA_SxCR_PL_Pos) | (0b01UL << DMA_SxCR_MSIZE_Pos) | (0b01UL << DMA_SxCR_PSIZE_Pos) | DMA_SxCR_MINC | (1U << DMA_SxCR_DIR_Pos);
+}
+
+static void sound_stop_dac(void) {
+  register_clear_bits(&BDMA_Channel0->CCR, BDMA_CCR_EN);
+  BDMA->IFCR = 0xFFFFFFFFU;
+
+  register_clear_bits(&DMA1_Stream1->CR, DMA_SxCR_EN);
+  while ((DMA1_Stream1->CR & DMA_SxCR_EN) != 0U) {}
+  DMA1->LIFCR = (0x3FU << 6);
+
+  register_clear_bits(&DAC1->CR, DAC_CR_EN1 | DAC_CR_EN2);
+  while ((DAC1->CR & (DAC_CR_EN1 | DAC_CR_EN2)) != 0U) {}
+}
+
+void sound_init(void) {
+  REGISTER_INTERRUPT(BDMA_Channel0_IRQn, BDMA_Channel0_IRQ_Handler, 128U, FAULT_INTERRUPT_RATE_SOUND_DMA)
+  REGISTER_INTERRUPT(DMA1_Stream0_IRQn, DMA1_Stream0_IRQ_Handler, 128U, FAULT_INTERRUPT_RATE_SOUND_DMA)
+
+  // Init DAC and its DMA
+  sound_init_dac();

  // Init trigger timer (little slower than 48kHz, pulled in sync by SAI4_FS_B)
  register_set(&TIM5->PSC, 2600U, 0xFFFFU);
-  register_set(&TIM5->ARR, 100U, 0xFFFFFFFFU); // not important
+  register_set(&TIM5->ARR, 100U, 0xFFFFFFFFU);
  register_set(&TIM5->AF1, (0b0010UL << TIM5_AF1_ETRSEL_Pos), TIM5_AF1_ETRSEL_Msk);
  register_set(&TIM5->CR2, (0b010U << TIM_CR2_MMS_Pos), TIM_CR2_MMS_Msk);
  register_set(&TIM5->SMCR, TIM_SMCR_ECE | (0b00111UL << TIM_SMCR_TS_Pos)| (0b0100UL << TIM_SMCR_SMS_Pos), 0x31FFF7U);
@@ -169,12 +192,16 @@ void sound_init(void) {
  register_set(&DMA1_Stream0->CR, DMA_SxCR_DBM | (0b10UL << DMA_SxCR_MSIZE_Pos) | (0b10UL << DMA_SxCR_PSIZE_Pos) | DMA_SxCR_MINC | DMA_SxCR_CIRC | DMA_SxCR_TCIE, 0x01F7FFFFU);
  register_set(&DMAMUX1_Channel0->CCR, 101U, DMAMUX_CxCR_DMAREQ_ID_Msk); // DFSDM1_DMA0
  register_set_bits(&DMA1_Stream0->CR, DMA_SxCR_EN);
-  DMA1->LIFCR |= 0x7D; // clear flags
+  DMA1->LIFCR |= 0x7DU; // clear flags

  // DMA (memory -> SAI4)
  register_set(&BDMA_Channel1->CPAR, (uint32_t) &(SAI4_Block_A->DR), 0xFFFFFFFFU);
-  register_set(&BDMA_Channel1->CCR, (0b01UL << BDMA_CCR_MSIZE_Pos) | (0b01UL << BDMA_CCR_PSIZE_Pos) | BDMA_CCR_MINC | (0b1U << BDMA_CCR_DIR_Pos), 0xFFFEU);
+  register_set(&BDMA_Channel1->CM0AR, (uint32_t) mic_tx_buf[0], 0xFFFFFFFFU);
+  register_set(&BDMA_Channel1->CM1AR, (uint32_t) mic_tx_buf[1], 0xFFFFFFFFU);
+  BDMA_Channel1->CNDTR = MIC_TX_BUF_SIZE;
+  register_set(&BDMA_Channel1->CCR, BDMA_CCR_DBM | (0b01UL << BDMA_CCR_MSIZE_Pos) |(0b01UL << BDMA_CCR_PSIZE_Pos) | BDMA_CCR_MINC | BDMA_CCR_CIRC | (0b1U << BDMA_CCR_DIR_Pos), 0xFFFFU);
  register_set(&DMAMUX2_Channel1->CCR, 15U, DMAMUX_CxCR_DMAREQ_ID_Msk); // SAI4_A_DMA
+  register_set_bits(&BDMA_Channel1->CCR, BDMA_CCR_EN);

  // enable all initted blocks
  register_set_bits(&SAI4_Block_A->CR1, SAI_xCR1_SAIEN);
--- a/board/stm32h7/stm32h7_config.h
+++ b/board/stm32h7/stm32h7_config.h
@@ -71,6 +71,8 @@ separate IRQs for RX and TX.

 #ifdef PANDA_JUNGLE
 #include "board/jungle/stm32h7/board.h"
+#elif defined(PANDA_BODY)
+#include "board/body/stm32h7/board.h"
 #else
 #include "board/stm32h7/board.h"
 #endif
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -30,7 +30,6 @@ dev = [
  "mypy",
  "setuptools",
  "spidev; platform_system == 'Linux'",
-  "spidev2; platform_system == 'Linux'",
 ]

 [build-system]
@@ -73,4 +72,4 @@ addopts = "-Werror --strict-config --strict-markers --durations=10 --ignore-glob
 python_files = "test_*.py"
 testpaths = [
  "tests/"
-]
+]
--- a/python/init.py
+++ b/python/init.py
@@ -116,6 +116,7 @@ class Panda:
  HW_TYPE_RED_PANDA = b'\x07'
  HW_TYPE_TRES = b'\x09'
  HW_TYPE_CUATRO = b'\x0a'
+  HW_TYPE_BODY = b'\xb1'

  CAN_PACKET_VERSION = 4
  HEALTH_PACKET_VERSION = 17
@@ -124,7 +125,7 @@ class Panda:
  CAN_HEALTH_STRUCT = struct.Struct("<BIBBBBBBBBIIIIIIIHHBBBIIII")

  F4_DEVICES = [HW_TYPE_WHITE, HW_TYPE_BLACK]
-  H7_DEVICES = [HW_TYPE_RED_PANDA, HW_TYPE_TRES, HW_TYPE_CUATRO]
+  H7_DEVICES = [HW_TYPE_RED_PANDA, HW_TYPE_TRES, HW_TYPE_CUATRO, HW_TYPE_BODY]
  SUPPORTED_DEVICES = H7_DEVICES

  INTERNAL_DEVICES = (HW_TYPE_TRES, HW_TYPE_CUATRO)
@@ -743,14 +744,14 @@ class Panda:

  # ******************* serial *******************

-  def serial_read(self, port_number):
-    ret = []
+  def serial_read(self, port_number, maxlen=1024):
+    ret = b''
    while 1:
-      lret = bytes(self._handle.controlRead(Panda.REQUEST_IN, 0xe0, port_number, 0, 0x40))
-      if len(lret) == 0:
+      r = bytes(self._handle.controlRead(Panda.REQUEST_IN, 0xe0, port_number, 0, 0x40))
+      if len(r) == 0 or len(ret) >= maxlen:
        break
-      ret.append(lret)
-    return b''.join(ret)
+      ret += r
+    return ret

  def serial_write(self, port_number, ln):
    ret = 0
--- a/python/socketpanda.py
+++ b/python/socketpanda.py
@@ -1,5 +1,6 @@
 import socket
 import struct
+import time

 # /**
 #  * struct canfd_frame - CAN flexible data rate frame structure
@@ -23,6 +24,9 @@ CAN_HEADER_LEN = struct.calcsize(CAN_HEADER_FMT)
 CAN_MAX_DLEN = 8
 CANFD_MAX_DLEN = 64

+CAN_CONFIRM_FLAG = 0x800
+CAN_EFF_FLAG = 0x80000000
+
 CANFD_BRS = 0x01 # bit rate switch (second bitrate for payload data)
 CANFD_FDF = 0x04 # mark CAN FD for dual use of struct canfd_frame

@@ -32,11 +36,12 @@ SO_RXQ_OVFL = 40

 import typing
@typing.no_type_check # mypy struggles with macOS here...
-def create_socketcan(interface:str, recv_buffer_size:int, fd:bool) -> socket.socket:
+def create_socketcan(interface:str, recv_buffer_size:int) -> socket.socket:
  # settings mostly from https://github.com/linux-can/can-utils/blob/master/candump.c
  socketcan = socket.socket(socket.AF_CAN, socket.SOCK_RAW, socket.CAN_RAW)
-  if fd:
-    socketcan.setsockopt(socket.SOL_CAN_RAW, socket.CAN_RAW_FD_FRAMES, 1)
+  socketcan.setblocking(False)
+  socketcan.setsockopt(socket.SOL_CAN_RAW, socket.CAN_RAW_FD_FRAMES, 1)
+  socketcan.setsockopt(socket.SOL_CAN_RAW, socket.CAN_RAW_RECV_OWN_MSGS, 1)
  socketcan.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, recv_buffer_size)
  # TODO: why is it always 2x the requested size?
  assert socketcan.getsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF) == recv_buffer_size * 2
@@ -47,50 +52,72 @@ def create_socketcan(interface:str, recv_buffer_size:int, fd:bool) -> socket.soc

 # Panda class substitute for socketcan device (to support using the uds/iso-tp/xcp/ccp library)
 class SocketPanda():
-  def __init__(self, interface:str="can0", bus:int=0, fd:bool=False, recv_buffer_size:int=212992) -> None:
+  def __init__(self, interface:str="can0", recv_buffer_size:int=212992) -> None:
    self.interface = interface
-    self.bus = bus
-    self.fd = fd
-    self.flags = CANFD_BRS | CANFD_FDF if fd else 0
-    self.data_len = CANFD_MAX_DLEN if fd else CAN_MAX_DLEN
    self.recv_buffer_size = recv_buffer_size
-    self.socket = create_socketcan(interface, recv_buffer_size, fd)
+    self.socket = create_socketcan(interface, recv_buffer_size)

  def __del__(self):
    self.socket.close()

  def get_serial(self) -> tuple[int, int]:
-    return (0, 0) # TODO: implemented in panda socketcan driver
+    return (0, 0)

  def get_version(self) -> int:
-    return 0 # TODO: implemented in panda socketcan driver
+    return 0

  def can_clear(self, bus:int) -> None:
-    # TODO: implemented in panda socketcan driver
    self.socket.close()
-    self.socket = create_socketcan(self.interface, self.recv_buffer_size, self.fd)
+    self.socket = create_socketcan(self.interface, self.recv_buffer_size)

  def set_safety_mode(self, mode:int, param=0) -> None:
-    pass # TODO: implemented in panda socketcan driver
+    pass

-  def has_obd(self) -> bool:
-    return False # TODO: implemented in panda socketcan driver
+  def can_send_many(self, arr, *, fd=False, timeout=0) -> None:
+    for msg in arr:
+      self.can_send(*msg, fd=fd, timeout=timeout)

-  def can_send(self, addr, dat, bus=0, timeout=0) -> None:
+  def can_send(self, addr, dat, bus, *, fd=False, timeout=0) -> None:
+    # Even if the CANFD_FDF flag is not set, the data still must be 8 bytes for classic CAN frames.
+    data_len = CANFD_MAX_DLEN if fd else CAN_MAX_DLEN
    msg_len = len(dat)
-    msg_dat = dat.ljust(self.data_len, b'\x00')
-    can_frame = struct.pack(CAN_HEADER_FMT, addr, msg_len, self.flags) + msg_dat
-    self.socket.sendto(can_frame, (self.interface,))
+    msg_dat = dat.ljust(data_len, b'\x00')
+
+    # Set extended ID flag
+    if addr > 0x7ff:
+      addr |= CAN_EFF_FLAG
+
+    # Set FD flags
+    flags = CANFD_BRS | CANFD_FDF if fd else 0
+
+    can_frame = struct.pack(CAN_HEADER_FMT, addr, msg_len, flags) + msg_dat
+
+    # Try to send until timeout. sendto might block if the TX buffer is full.
+    # TX buffer size can also be adjusted through `ip link set can0 txqueuelen <size>` if needed
+    start_t = time.monotonic()
+    while (time.monotonic() - start_t < (timeout / 1000)) or (timeout == 0):
+      try:
+        self.socket.sendto(can_frame, (self.interface,))
+        break
+      except (BlockingIOError, OSError):
+        continue
+    else:
+      raise TimeoutError
+

  def can_recv(self) -> list[tuple[int, bytes, int]]:
    msgs = list()
    while True:
      try:
-        dat, _ = self.socket.recvfrom(self.recv_buffer_size, socket.MSG_DONTWAIT)
-        assert len(dat) == CAN_HEADER_LEN + self.data_len, f"ERROR: received {len(dat)} bytes"
+        dat, _, msg_flags, _ = self.socket.recvmsg(self.recv_buffer_size)
+        assert len(dat) >= CAN_HEADER_LEN, f"ERROR: received {len(dat)} bytes"
+
        can_id, msg_len, _ = struct.unpack(CAN_HEADER_FMT, dat[:CAN_HEADER_LEN])
+        assert len(dat) >= CAN_HEADER_LEN + msg_len, f"ERROR: received {len(dat)} bytes, expected at least {CAN_HEADER_LEN + msg_len} bytes"
+
        msg_dat = dat[CAN_HEADER_LEN:CAN_HEADER_LEN+msg_len]
-        msgs.append((can_id, msg_dat, self.bus))
+        bus = 128 if (msg_flags & CAN_CONFIRM_FLAG) else 0
+        msgs.append((can_id, msg_dat, bus))
      except BlockingIOError:
        break # buffered data exhausted
    return msgs
--- a/python/spi.py
+++ b/python/spi.py
@@ -16,10 +16,6 @@ try:
  import spidev
 except ImportError:
  spidev = None
-try:
-  import spidev2
-except ImportError:
-  spidev2 = None

 # Constants
 SYNC = 0x5A
@@ -121,8 +117,7 @@ class PandaSpiHandle(BaseHandle):

  def __init__(self) -> None:
    self.dev = SpiDevice()
-    if spidev2 is not None and "SPI2" in os.environ:
-      self._spi2 = spidev2.SPIBus("/dev/spidev0.0", "w+b", bits_per_word=8, speed_hz=50_000_000)
+    self.no_retry = "NO_RETRY" in os.environ

  # helpers
  def _calc_checksum(self, data: bytes) -> int:
@@ -183,52 +178,6 @@ class PandaSpiHandle(BaseHandle):

      return dat[3:-1]

-  def _transfer_spidev2(self, spi, endpoint: int, data, timeout: int, max_rx_len: int = USBPACKET_MAX_SIZE, expect_disconnect: bool = False) -> bytes:
-    max_rx_len = max(USBPACKET_MAX_SIZE, max_rx_len)
-
-    header = self.HEADER.pack(SYNC, endpoint, len(data), max_rx_len)
-
-    header_ack = bytearray(1)
-
-    # ACK + <2 bytes for response length> + data + checksum
-    data_rx = bytearray(3+max_rx_len+1)
-
-    self._spi2.submitTransferList(spidev2.SPITransferList((
-      # header
-      {'tx_buf': header + bytes([self._calc_checksum(header), ]), 'delay_usecs': 0, 'cs_change': True},
-      {'rx_buf': header_ack, 'delay_usecs': 0, 'cs_change': True},
-
-      # send data
-      {'tx_buf': bytes([*data, self._calc_checksum(data)]), 'delay_usecs': 0, 'cs_change': True},
-      {'rx_buf': data_rx, 'delay_usecs': 0, 'cs_change': True},
-    )))
-
-    if header_ack[0] != HACK:
-      raise PandaSpiMissingAck
-
-    if expect_disconnect:
-      logger.debug("- expecting disconnect, returning")
-      return b""
-    else:
-      dat = bytes(data_rx)
-      if dat[0] != DACK:
-        if dat[0] == NACK:
-          raise PandaSpiNackResponse
-
-        print("trying again")
-        dat = self._wait_for_ack(spi, DACK, timeout, 0x13, length=3 + max_rx_len)
-
-      # get response length, then response
-      response_len = struct.unpack("<H", dat[1:3])[0]
-      if response_len > max_rx_len:
-        raise PandaSpiException(f"response length greater than max ({max_rx_len} {response_len})")
-
-      dat = dat[:3 + response_len + 1]
-      if self._calc_checksum(dat) != 0:
-        raise PandaSpiBadChecksum
-
-      return dat[3:-1]
-
  def _transfer(self, endpoint: int, data, timeout: int, max_rx_len: int = 1000, expect_disconnect: bool = False) -> bytes:
    logger.debug("starting transfer: endpoint=%d, max_rx_len=%d", endpoint, max_rx_len)
    logger.debug("==============================================")
@@ -241,12 +190,12 @@ class PandaSpiHandle(BaseHandle):
      logger.debug("\ntry #%d", n)
      with self.dev.acquire() as spi:
        try:
-          fn = self._transfer_spidev
-          #fn = self._transfer_spidev2
-          return fn(spi, endpoint, data, timeout, max_rx_len, expect_disconnect)
+          return self._transfer_spidev(spi, endpoint, data, timeout, max_rx_len, expect_disconnect)
        except PandaSpiException as e:
          exc = e
          logger.debug("SPI transfer failed, retrying", exc_info=True)
+          if self.no_retry:
+            break

          # ensure slave is in a consistent state and ready for the next transfer
          # (e.g. slave TX buffer isn't stuck full)
--- a/scripts/check_fw_size.py
+++ b/scripts/check_fw_size.py
@@ -78,9 +78,12 @@ def check_space(file, mcu):


 if __name__ == "__main__":
-  # red panda
-  check_space("../board/obj/bootstub.panda_h7.elf", "H7")
-  check_space("../board/obj/panda_h7.elf", "H7")
+  # panda
+  check_space("../board/obj/panda_h7/bootstub.elf", "H7")
+  check_space("../board/obj/panda_h7/main.elf", "H7")
  # jungle v2
-  check_space("../board/jungle/obj/bootstub.panda_jungle_h7.elf", "H7")
-  check_space("../board/jungle/obj/panda_jungle_h7.elf", "H7")
+  check_space("../board/obj/panda_jungle_h7/bootstub.elf", "H7")
+  check_space("../board/obj/panda_jungle_h7/main.elf", "H7")
+  # body
+  check_space("../board/obj/body_h7/bootstub.elf", "H7")
+  check_space("../board/obj/body_h7/main.elf", "H7")
--- a/scripts/health_test.py
+++ b/scripts/health_test.py
@@ -1,18 +0,0 @@
-#!/usr/bin/env python3
-import time
-from panda import Panda
-
-if __name__ == "__main__":
-  i = 0
-  pi = 0
-
-  panda = Panda()
-  while True:
-    st = time.monotonic()
-    while time.monotonic() - st < 1:
-      panda.health()
-      i += 1
-    print(i, panda.health(), "\n")
-    print(f"Speed: {i - pi}Hz")
-    pi = i
-
--- a/scripts/spi_test.py
+++ b/scripts/spi_test.py
@@ -0,0 +1,50 @@
+#!/usr/bin/env python3
+import os
+import sys
+import time
+from datetime import datetime
+from collections import defaultdict, deque
+
+os.environ['NO_RETRY'] = '1'  # no spi retries
+from panda import Panda, PandaSpiException
+
+
+if __name__ == "__main__":
+  s = defaultdict(lambda: deque(maxlen=30))
+  def avg(k):
+    return sum(s[k])/len(s[k])
+
+  #core = 6
+  #from openpilot.common.realtime import config_realtime_process
+  #config_realtime_process(core, 10)
+  #os.system(f"echo {core} | sudo tee /proc/irq/10/smp_affinity_list")
+  #os.system("sudo chrt -f -p 1 $(pgrep -f spi0)")
+  #print(f"sudo taskset -pc {core} $(pgrep -f spi0)")
+  #os.system(f"sudo taskset -pc {core} $(pgrep -f spi0)")
+
+  p = Panda()
+  p.reset()
+  start = datetime.now()
+  le = p.health()['spi_error_count']
+  while True:
+    cnt = 0
+    st = time.monotonic()
+    while time.monotonic() - st < 1:
+      try:
+        p.get_type()   # get_type has no processing on panda side
+      except PandaSpiException:
+        print(f"ERROR after {datetime.now() - start}\n\n")
+        sys.stdout.write("\033[1;32;40m" + p.serial_read(0).decode('utf8') + "\033[00m" + "\n")
+        sys.stdout.flush()
+        sys.exit()
+      cnt += 1
+    s['hz'].append(cnt)
+
+    err = p.health()['spi_error_count']
+    s['err'].append((err - le) & 0xFFFFFFFF)
+    le = err
+
+    print(
+      f"{avg('hz'):04.0f}Hz   {avg('err'):04.0f} errors   [{cnt:04d}Hz   {s['err'][-1]:04d} errors]  {datetime.now() - start}"
+    )
+
--- a/tests/misra/test_mutation.py
+++ b/tests/misra/test_mutation.py
@@ -13,6 +13,7 @@ ROOT = os.path.join(HERE, "../../")
 IGNORED_PATHS = (
  'board/obj',
  'board/jungle',
+  'board/body',
  'board/stm32h7/inc',
  'board/fake_stm.h',