seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
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>
This commit is contained in:
sl-firmware 2026-03-16 23:30:15 -04:00
parent 0fcad75cb4
commit d4e7d5b233
4 changed files with 275 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
include/vesc_comm.h Normal file
View File

@ -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 */

16
src/balance.c
View File

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

42
src/vesc_can.c Normal file
View File

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

186
src/vesc_uart.c Normal file
View File

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