feat: VESC UART transport for dual FSESC 6.7 Pro (balance bot)

Adds VESC motor control over USART6 for the SaltyBot self-balancing robot.
Motor1 driven directly via COMM_SET_CURRENT; Motor2 bridged via
COMM_FORWARD_CAN (cmd 34) to CAN ID 68. balance_update() now sends current
commands to both VESCs on every PID tick — 0 A on disarm/fault, scaled ±10 A
when armed (configurable via VESC_CURRENT_MAX_A).

- include/vesc_comm.h: common interface (init, send_current, send_duty)
- src/vesc_uart.c: USART6 @ 115200, CRC16-CCITT, VESC FW 6.6 framing
- src/vesc_can.c: phase-2 stub (TODO: bxCAN + SN65HVD230)
- src/balance.c: wire vesc_comm_send_current() into PID output path

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

include/vesc_comm.h

@@ -0,0 +1,31 @@
+#ifndef VESC_COMM_H
+#define VESC_COMM_H
+
+/*
+ * VESC Communication Interface — Dual FSESC 6.7 Pro (FW 6.6)
+ *
+ * Motor1: connected directly to USART6 (PC6=TX, PC7=RX) @ 115200
+ * Motor2: bridged via COMM_FORWARD_CAN (cmd 34) to CAN ID 68 on motor1
+ *
+ * Transport implementations:
+ *   src/vesc_uart.c — USART6 UART (active)
+ *   src/vesc_can.c  — STM32 CAN + SN65HVD230 (phase 2, stub)
+ */
+
+/* Initialise the VESC transport (USART6 GPIO + peripheral) */
+void vesc_comm_init(void);
+
+/*
+ * Send a current command to both VESCs.
+ * current_motor1/2 in Amperes (positive = forward, negative = reverse).
+ * Range limited by VESC motor configuration; typical max ±30 A for FSESC 6.7 Pro.
+ */
+void vesc_comm_send_current(float current_motor1, float current_motor2);
+
+/*
+ * Send a duty-cycle command to both VESCs.
+ * duty_motor1/2 in range -1.0 .. +1.0 (full reverse / full forward).
+ */
+void vesc_comm_send_duty(float duty_motor1, float duty_motor2);
+
+#endif /* VESC_COMM_H */

src/balance.c

@@ -1,8 +1,14 @@
 #include "balance.h"
 #include "slope_estimator.h"
+#include "vesc_comm.h"
 #include "config.h"
 #include <math.h>
 
+/* Scale motor_cmd (±MAX_SPEED_LIMIT) to VESC current in Amperes.
+ * 10 A peak is conservative for FSESC 6.7 Pro on a balance bot;
+ * adjust once motor winding and robot mass are characterised. */
+#define VESC_CURRENT_MAX_A  10.0f
+
 void balance_init(balance_t *b) {
     b->state = BALANCE_DISARMED;
     b->pitch_deg = 0.0f;
@@ -42,6 +48,7 @@ void balance_update(balance_t *b, const IMUData *imu, float dt) {
             b->motor_cmd = 0;
             b->integral = 0.0f;
             slope_estimator_reset(&b->slope);
+            vesc_comm_send_current(0.0f, 0.0f);
             return;
         }
     }
@@ -50,6 +57,7 @@ void balance_update(balance_t *b, const IMUData *imu, float dt) {
         b->motor_cmd = 0;
         b->integral = 0.0f;
         b->prev_error = 0.0f;
+        vesc_comm_send_current(0.0f, 0.0f);
         return;
     }
 
@@ -76,6 +84,14 @@ void balance_update(balance_t *b, const IMUData *imu, float dt) {
 
     b->motor_cmd = (int16_t)output;
     b->prev_error = error;
+
+    /* Drive both VESCs with the same current command.
+     * motor_cmd is clamped to ±MAX_SPEED_LIMIT (100); scale to Amperes.
+     * Both motors run in the same direction — forward/back balance only.
+     * Steering differential is handled separately by motor_driver_update(). */
+    float current_a = (float)b->motor_cmd
+                      * (VESC_CURRENT_MAX_A / (float)MAX_SPEED_LIMIT);
+    vesc_comm_send_current(current_a, current_a);
 }
 
 void balance_arm(balance_t *b) {

src/vesc_can.c

@@ -0,0 +1,42 @@
+/*
+ * VESC CAN Transport — Phase 2 stub
+ *
+ * Hardware: STM32 bxCAN peripheral + SN65HVD230 CAN transceiver
+ *
+ * When implemented this will drive both VESCs directly over CAN
+ * using the native VESC CAN protocol (extended frame, 29-bit IDs).
+ * For now all functions are no-ops; UART transport (vesc_uart.c) is active.
+ *
+ * VESC CAN frame format (FW 6.6, extended IDs):
+ *   arbitration ID = (command << 8) | vesc_can_id   (29-bit extended)
+ *   Example: COMM_SET_CURRENT (6) to CAN ID 1 → 0x00000601
+ *
+ * TODO (phase 2):
+ *   - Initialise bxCAN peripheral (500 kbps, extended frames)
+ *   - Implement vesc_comm_send_current() using HAL_CAN_AddTxMessage()
+ *   - Implement vesc_comm_send_duty() using HAL_CAN_AddTxMessage()
+ *   - Add RX handler to read VESC status frames (current, temp, fault)
+ *   - Gate on build flag (VESC_TRANSPORT=CAN) so UART and CAN builds
+ *     are mutually exclusive at link time
+ */
+
+#include "vesc_comm.h"
+
+void vesc_comm_init(void)
+{
+    /* TODO (phase 2): init bxCAN + SN65HVD230 transceiver GPIO */
+}
+
+void vesc_comm_send_current(float current_motor1, float current_motor2)
+{
+    /* TODO (phase 2): encode COMM_SET_CURRENT and transmit extended CAN frame */
+    (void)current_motor1;
+    (void)current_motor2;
+}
+
+void vesc_comm_send_duty(float duty_motor1, float duty_motor2)
+{
+    /* TODO (phase 2): encode COMM_SET_DUTY and transmit extended CAN frame */
+    (void)duty_motor1;
+    (void)duty_motor2;
+}

src/vesc_uart.c

@@ -0,0 +1,186 @@
+/*
+ * VESC UART Transport — Dual FSESC 6.7 Pro (FW 6.6)
+ *
+ * Hardware: MAMBA F722S USART6 (PC6=TX AF8, PC7=RX AF8) @ 115200 baud
+ *
+ * Motor1: directly attached to USART6 RX line
+ * Motor2: bridged via COMM_FORWARD_CAN (cmd 34, 0x22) to CAN ID 68 (0x44)
+ *         Motor1 VESC relays the packet to Motor2 over its CAN bus
+ *
+ * VESC packet framing (FW 6.6 short form, payload ≤ 255 bytes):
+ *   [0x02] [len:u8] [payload...] [crc_hi:u8] [crc_lo:u8] [0x03]
+ *
+ * Command IDs (VESC FW 6.6):
+ *   COMM_SET_DUTY    = 5
+ *   COMM_SET_CURRENT = 6
+ *   COMM_FORWARD_CAN = 34 (0x22)
+ *
+ * CRC: CRC16-CCITT, poly 0x1021, init 0x0000, no reflection, no final XOR.
+ *      Computed over payload bytes only.
+ */
+
+#include "vesc_comm.h"
+#include "config.h"
+#include "stm32f7xx_hal.h"
+#include <stdint.h>
+#include <string.h>
+
+/* -----------------------------------------------------------------------
+ * VESC command IDs (FW 6.6)
+ * --------------------------------------------------------------------- */
+#define VESC_COMM_SET_DUTY      5u
+#define VESC_COMM_SET_CURRENT   6u
+#define VESC_COMM_FORWARD_CAN  34u   /* 0x22 */
+
+/* CAN ID of the second VESC on the shared CAN bus */
+#define VESC_CAN_ID_MOTOR2     68u   /* 0x44 */
+
+/* Maximum bytes in one VESC packet (start + len + 255 payload + crc*2 + stop) */
+#define VESC_PKT_MAX           261u
+
+/* -----------------------------------------------------------------------
+ * USART6 peripheral handle
+ * --------------------------------------------------------------------- */
+static UART_HandleTypeDef s_uart;
+
+/* -----------------------------------------------------------------------
+ * CRC16-CCITT (poly 0x1021, init 0x0000)
+ * --------------------------------------------------------------------- */
+static uint16_t crc16_ccitt(const uint8_t *data, uint8_t len)
+{
+    uint16_t crc = 0x0000u;
+    for (uint8_t i = 0; i < len; i++) {
+        crc ^= (uint16_t)data[i] << 8u;
+        for (uint8_t bit = 0; bit < 8u; bit++) {
+            if (crc & 0x8000u) {
+                crc = (uint16_t)((crc << 1u) ^ 0x1021u);
+            } else {
+                crc = (uint16_t)(crc << 1u);
+            }
+        }
+    }
+    return crc;
+}
+
+/* -----------------------------------------------------------------------
+ * Build and transmit a framed VESC packet
+ * payload: pointer to payload bytes (command ID + data)
+ * plen:    payload length in bytes
+ * --------------------------------------------------------------------- */
+static void vesc_send_packet(const uint8_t *payload, uint8_t plen)
+{
+    /* Short packet framing: 0x02 + 1-byte length (≤255) */
+    uint8_t  buf[VESC_PKT_MAX];
+    uint8_t  idx = 0;
+
+    uint16_t crc = crc16_ccitt(payload, plen);
+
+    buf[idx++] = 0x02u;                     /* start byte */
+    buf[idx++] = plen;                      /* payload length */
+    memcpy(&buf[idx], payload, plen);
+    idx = (uint8_t)(idx + plen);
+    buf[idx++] = (uint8_t)(crc >> 8u);     /* CRC high */
+    buf[idx++] = (uint8_t)(crc & 0xFFu);   /* CRC low */
+    buf[idx++] = 0x03u;                     /* stop byte */
+
+    /* Blocking TX — balance loop runs at 1 kHz; 6-byte packet at 115200
+     * takes ~0.52 ms.  Keep plen small (≤9) to stay well under 1 ms. */
+    HAL_UART_Transmit(&s_uart, buf, idx, 5u /* ms timeout */);
+}
+
+/* -----------------------------------------------------------------------
+ * Encode a signed 32-bit value big-endian into dst[0..3]
+ * --------------------------------------------------------------------- */
+static void encode_int32_be(uint8_t *dst, int32_t val)
+{
+    dst[0] = (uint8_t)((uint32_t)val >> 24u);
+    dst[1] = (uint8_t)((uint32_t)val >> 16u);
+    dst[2] = (uint8_t)((uint32_t)val >>  8u);
+    dst[3] = (uint8_t)((uint32_t)val        );
+}
+
+/* -----------------------------------------------------------------------
+ * Public API
+ * --------------------------------------------------------------------- */
+
+void vesc_comm_init(void)
+{
+    /* Enable clocks */
+    __HAL_RCC_USART6_CLK_ENABLE();
+    __HAL_RCC_GPIOC_CLK_ENABLE();
+
+    /* PC6 = USART6_TX (AF8), PC7 = USART6_RX (AF8) */
+    GPIO_InitTypeDef gpio = {0};
+    gpio.Pin       = UART6_TX_PIN | UART6_RX_PIN;   /* PC6 | PC7 */
+    gpio.Mode      = GPIO_MODE_AF_PP;
+    gpio.Pull      = GPIO_NOPULL;
+    gpio.Speed     = GPIO_SPEED_FREQ_HIGH;
+    gpio.Alternate = GPIO_AF8_USART6;
+    HAL_GPIO_Init(UART6_TX_PORT, &gpio);             /* both pins on GPIOC */
+
+    /* 115200 8N1, no flow control */
+    s_uart.Instance          = USART6;
+    s_uart.Init.BaudRate     = 115200u;
+    s_uart.Init.WordLength   = UART_WORDLENGTH_8B;
+    s_uart.Init.StopBits     = UART_STOPBITS_1;
+    s_uart.Init.Parity       = UART_PARITY_NONE;
+    s_uart.Init.Mode         = UART_MODE_TX_RX;
+    s_uart.Init.HwFlowCtl    = UART_HWCONTROL_NONE;
+    s_uart.Init.OverSampling = UART_OVERSAMPLING_16;
+    HAL_UART_Init(&s_uart);
+}
+
+void vesc_comm_send_current(float current_motor1, float current_motor2)
+{
+    /* Convert amps to milliamps (VESC FW 6.6 expects mA as int32) */
+    int32_t ma1 = (int32_t)(current_motor1 * 1000.0f);
+    int32_t ma2 = (int32_t)(current_motor2 * 1000.0f);
+
+    /* --- Motor 1: direct COMM_SET_CURRENT ---
+     * payload: [0x06, mA[3], mA[2], mA[1], mA[0]]  (5 bytes) */
+    {
+        uint8_t payload[5];
+        payload[0] = VESC_COMM_SET_CURRENT;
+        encode_int32_be(&payload[1], ma1);
+        vesc_send_packet(payload, sizeof(payload));
+    }
+
+    /* --- Motor 2: COMM_FORWARD_CAN to CAN ID 68, then COMM_SET_CURRENT ---
+     * payload: [0x22, 0x44, 0x06, mA[3], mA[2], mA[1], mA[0]]  (7 bytes) */
+    {
+        uint8_t payload[7];
+        payload[0] = VESC_COMM_FORWARD_CAN;   /* 0x22 = 34 */
+        payload[1] = VESC_CAN_ID_MOTOR2;      /* 0x44 = 68 */
+        payload[2] = VESC_COMM_SET_CURRENT;   /* 0x06 */
+        encode_int32_be(&payload[3], ma2);
+        vesc_send_packet(payload, sizeof(payload));
+    }
+}
+
+void vesc_comm_send_duty(float duty_motor1, float duty_motor2)
+{
+    /* VESC encodes duty as int32 scaled by 100000:
+     *   1.0 = 100000, -1.0 = -100000 */
+    int32_t d1 = (int32_t)(duty_motor1 * 100000.0f);
+    int32_t d2 = (int32_t)(duty_motor2 * 100000.0f);
+
+    /* --- Motor 1: direct COMM_SET_DUTY ---
+     * payload: [0x05, d[3], d[2], d[1], d[0]]  (5 bytes) */
+    {
+        uint8_t payload[5];
+        payload[0] = VESC_COMM_SET_DUTY;
+        encode_int32_be(&payload[1], d1);
+        vesc_send_packet(payload, sizeof(payload));
+    }
+
+    /* --- Motor 2: COMM_FORWARD_CAN to CAN ID 68, then COMM_SET_DUTY ---
+     * payload: [0x22, 0x44, 0x05, d[3], d[2], d[1], d[0]]  (7 bytes) */
+    {
+        uint8_t payload[7];
+        payload[0] = VESC_COMM_FORWARD_CAN;
+        payload[1] = VESC_CAN_ID_MOTOR2;
+        payload[2] = VESC_COMM_SET_DUTY;
+        encode_int32_be(&payload[3], d2);
+        vesc_send_packet(payload, sizeof(payload));
+    }
+}