Add comma body firmware (#2291)

* motors

* can

* cleanup unused stuff

* initial clean

* more clean

* remove integral and derivative clamps, revert pwm driver to original

* remove integral and derivative clamps, revert pwm driver to original

* remove integral and derivative clamps, revert pwm driver to original

* dont need this for now

* clean

* fix can rx and can version error

* ignore body for misra mutation test

* fix bus recovery, remove body rx hook

SConscript

@@ -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')

__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

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'')

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,
+};

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

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;
+  }
+}

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)

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;
+}

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;
+}

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);
+}

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;
+  }
+}

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;
+}

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()
+}

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

python/__init__.py

@@ -117,6 +117,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:
   HEALTH_STRUCT = struct.Struct("<IIIIIIIIBBBBBHBBBHfBBHHHB")
   CAN_HEALTH_STRUCT = struct.Struct("<BIBBBBBBBBIIIIIIIHHBBBIIII")
 
-  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)

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',