Add comma body firmware

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