feat: CAN bus driver for BLDC motor controllers (Issue #597)
- Add can_driver.h / can_driver.c: CAN2 on PB12/PB13 (AF9) at 500 kbps APB1=54 MHz, PSC=6, BS1=13tq, BS2=4tq, SJW=1tq → 18tq/bit, SP 77.8% Filter bank 14 (SlaveStartFilterBank=14); 32-bit mask; FIFO0 Accept std IDs 0x200–0x21F (left/right feedback frames) TX: velocity+torque cmd (0x100+nid, DLC=4) at 100 Hz via main loop RX: velocity/current/position/temp/fault feedback (0x200+nid, DLC=8) AutoBusOff=ENABLE for HW recovery; can_driver_process() drains FIFO0 - Add JLINK_TLM_CAN_STATS (0x89, 16 bytes) + JLINK_CMD_CAN_STATS_GET (0x10) Also add JLINK_TLM_SLOPE (0x88) + jlink_tlm_slope_t missing from Issue #600 - Wire into main.c: init after jlink_init; 100Hz TX loop (differential drive speed_rpm ± steer_rpm/2); CAN enable follows arm state; 1Hz stats telemetry - Add CAN_RPM_SCALE=10 and CAN_TLM_HZ=1 to config.h Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
parent
bb5eff1382
commit
2996d18ace
81
include/can_driver.h
Normal file
81
include/can_driver.h
Normal file
@ -0,0 +1,81 @@
|
||||
#ifndef CAN_DRIVER_H
|
||||
#define CAN_DRIVER_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/* CAN bus driver for BLDC motor controllers (Issue #597)
|
||||
* CAN2 on PB12 (RX, AF9) / PB13 (TX, AF9) at 500 kbps
|
||||
* APB1 = 54 MHz: PSC=6, BS1=13tq, BS2=4tq, SJW=1tq → 18 tq/bit = 500 kbps
|
||||
*/
|
||||
|
||||
/* Node IDs */
|
||||
#define CAN_NUM_MOTORS 2u
|
||||
#define CAN_NODE_LEFT 0u
|
||||
#define CAN_NODE_RIGHT 1u
|
||||
|
||||
/* CAN frame IDs */
|
||||
#define CAN_ID_VEL_CMD_BASE 0x100u /* TX: 0x100 + node_id — velocity/torque command */
|
||||
#define CAN_ID_ENABLE_CMD_BASE 0x110u /* TX: 0x110 + node_id — enable/disable */
|
||||
#define CAN_ID_FEEDBACK_BASE 0x200u /* RX: 0x200 + node_id — position/velocity/current */
|
||||
|
||||
/* Filter: accept standard IDs 0x200–0x21F */
|
||||
#define CAN_FILTER_STDID 0x200u
|
||||
#define CAN_FILTER_MASK 0x7E0u
|
||||
|
||||
/* Bit timing (500 kbps @ 54 MHz APB1) */
|
||||
#define CAN_PRESCALER 6u
|
||||
|
||||
/* TX rate */
|
||||
#define CAN_TX_RATE_HZ 100u
|
||||
|
||||
/* Node alive timeout */
|
||||
#define CAN_NODE_TIMEOUT_MS 100u
|
||||
|
||||
/* TX command frame (8 bytes payload, DLC=4 for vel cmd) */
|
||||
typedef struct {
|
||||
int16_t velocity_rpm; /* target RPM (+/- = fwd/rev) */
|
||||
int16_t torque_x100; /* torque limit × 100 (0 = unlimited) */
|
||||
} can_cmd_t;
|
||||
|
||||
/* RX feedback frame (DLC=8) */
|
||||
typedef struct {
|
||||
int16_t velocity_rpm; /* actual RPM */
|
||||
int16_t current_ma; /* phase current in mA */
|
||||
int16_t position_x100; /* position × 100 (degrees or encoder counts) */
|
||||
int8_t temperature_c; /* controller temperature °C */
|
||||
uint8_t fault; /* fault flags (0 = healthy) */
|
||||
uint32_t last_rx_ms; /* HAL_GetTick() at last valid frame */
|
||||
} can_feedback_t;
|
||||
|
||||
/* Bus statistics */
|
||||
typedef struct {
|
||||
uint32_t tx_count; /* frames transmitted */
|
||||
uint32_t rx_count; /* frames received */
|
||||
uint16_t err_count; /* HAL-level errors */
|
||||
uint8_t bus_off; /* 1 = bus-off state */
|
||||
uint8_t _pad;
|
||||
} can_stats_t;
|
||||
|
||||
/* Initialise CAN2 peripheral, GPIO, and filter bank 14 */
|
||||
void can_driver_init(void);
|
||||
|
||||
/* Send velocity+torque command to one node */
|
||||
void can_driver_send_cmd(uint8_t node_id, const can_cmd_t *cmd);
|
||||
|
||||
/* Send enable/disable command to one node */
|
||||
void can_driver_send_enable(uint8_t node_id, bool enable);
|
||||
|
||||
/* Copy latest feedback snapshot (returns false if node never heard from) */
|
||||
bool can_driver_get_feedback(uint8_t node_id, can_feedback_t *out);
|
||||
|
||||
/* Returns true if node has been heard within CAN_NODE_TIMEOUT_MS */
|
||||
bool can_driver_is_alive(uint8_t node_id, uint32_t now_ms);
|
||||
|
||||
/* Copy bus statistics snapshot */
|
||||
void can_driver_get_stats(can_stats_t *out);
|
||||
|
||||
/* Drain RX FIFO0; call every main-loop tick */
|
||||
void can_driver_process(void);
|
||||
|
||||
#endif /* CAN_DRIVER_H */
|
||||
@ -257,4 +257,9 @@
|
||||
#define GIMBAL_PAN_LIMIT_DEG 180.0f // pan soft limit (deg each side)
|
||||
#define GIMBAL_TILT_LIMIT_DEG 90.0f // tilt soft limit (deg each side)
|
||||
|
||||
// --- CAN Bus Driver (Issue #597) ---
|
||||
// CAN2 on PB12 (RX, AF9) / PB13 (TX, AF9) — repurposes SPI2/OSD pins (unused on balance bot)
|
||||
#define CAN_RPM_SCALE 10 // motor_cmd to RPM: 1 cmd count = 10 RPM
|
||||
#define CAN_TLM_HZ 1u // JLINK_TLM_CAN_STATS transmit rate (Hz)
|
||||
|
||||
#endif // CONFIG_H
|
||||
|
||||
@ -37,6 +37,7 @@
|
||||
* 0x0D SCHED_SET - uint8 num_bands + N*16-byte pid_sched_entry_t (Issue #550)
|
||||
* 0x0E SCHED_SAVE - float kp, ki, kd (12 bytes); save sched+single to flash (Issue #550)
|
||||
* 0x0F FAULT_LOG_GET - no payload; reply with TLM_FAULT_LOG (Issue #565)
|
||||
* 0x10 CAN_STATS_GET - no payload; reply with TLM_CAN_STATS (Issue #597)
|
||||
*
|
||||
* STM32 to Jetson telemetry:
|
||||
* 0x80 STATUS - jlink_tlm_status_t (20 bytes), sent at JLINK_TLM_HZ
|
||||
@ -47,6 +48,8 @@
|
||||
* 0x85 SCHED - jlink_tlm_sched_t (1+N*16 bytes), sent on SCHED_GET (Issue #550)
|
||||
* 0x86 MOTOR_CURRENT - jlink_tlm_motor_current_t (8 bytes, Issue #584)
|
||||
* 0x87 FAULT_LOG - jlink_tlm_fault_log_t (20 bytes), sent on boot + FAULT_LOG_GET (Issue #565)
|
||||
* 0x88 SLOPE - jlink_tlm_slope_t (4 bytes), sent at SLOPE_TLM_HZ (Issue #600)
|
||||
* 0x89 CAN_STATS - jlink_tlm_can_stats_t (16 bytes), sent at CAN_TLM_HZ + CAN_STATS_GET (Issue #597)
|
||||
*
|
||||
* Priority: CRSF RC always takes precedence. Jetson steer/speed only applied
|
||||
* when mode_manager_active() == MODE_AUTONOMOUS (CH6 high). In RC_MANUAL and
|
||||
@ -77,6 +80,7 @@
|
||||
#define JLINK_CMD_SCHED_SET 0x0Du /* uint8 num_bands + N*16-byte entries (Issue #550) */
|
||||
#define JLINK_CMD_SCHED_SAVE 0x0Eu /* float kp,ki,kd; save sched+single to flash (Issue #550) */
|
||||
#define JLINK_CMD_FAULT_LOG_GET 0x0Fu /* no payload; reply TLM_FAULT_LOG (Issue #565) */
|
||||
#define JLINK_CMD_CAN_STATS_GET 0x10u /* no payload; reply TLM_CAN_STATS (Issue #597) */
|
||||
|
||||
/* ---- Telemetry IDs (STM32 to Jetson) ---- */
|
||||
#define JLINK_TLM_STATUS 0x80u
|
||||
@ -88,6 +92,7 @@
|
||||
#define JLINK_TLM_MOTOR_CURRENT 0x86u /* jlink_tlm_motor_current_t (8 bytes, Issue #584) */
|
||||
#define JLINK_TLM_FAULT_LOG 0x87u /* jlink_tlm_fault_log_t (20 bytes, Issue #565) */
|
||||
#define JLINK_TLM_SLOPE 0x88u /* jlink_tlm_slope_t (4 bytes, Issue #600) */
|
||||
#define JLINK_TLM_CAN_STATS 0x89u /* jlink_tlm_can_stats_t (16 bytes, Issue #597) */
|
||||
|
||||
/* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
|
||||
typedef struct __attribute__((packed)) {
|
||||
@ -166,11 +171,11 @@ typedef struct __attribute__((packed)) {
|
||||
} jlink_tlm_motor_current_t; /* 8 bytes */
|
||||
|
||||
/* ---- Telemetry SLOPE payload (4 bytes, packed) Issue #600 ---- */
|
||||
/* Published at SLOPE_TLM_HZ (1 Hz); reports slow-adapting terrain slope estimate. */
|
||||
/* Sent at SLOPE_TLM_HZ (1 Hz) by slope_estimator_send_tlm(). */
|
||||
typedef struct __attribute__((packed)) {
|
||||
int16_t slope_x100; /* slope estimate degrees x100 (0.01° resolution) */
|
||||
uint8_t active; /* 1 = slope compensation enabled */
|
||||
uint8_t _pad; /* reserved */
|
||||
int16_t slope_x100; /* terrain slope estimate (degrees x100; + = nose-up) */
|
||||
uint8_t active; /* 1 = slope estimation enabled */
|
||||
uint8_t _pad;
|
||||
} jlink_tlm_slope_t; /* 4 bytes */
|
||||
|
||||
/* ---- Telemetry FAULT_LOG payload (20 bytes, packed) Issue #565 ---- */
|
||||
@ -187,6 +192,18 @@ typedef struct __attribute__((packed)) {
|
||||
uint32_t hfsr; /* SCB->HFSR */
|
||||
} jlink_tlm_fault_log_t; /* 20 bytes */
|
||||
|
||||
/* ---- Telemetry CAN_STATS payload (16 bytes, packed) Issue #597 ---- */
|
||||
/* Sent at CAN_TLM_HZ (1 Hz) and in response to CAN_STATS_GET. */
|
||||
typedef struct __attribute__((packed)) {
|
||||
uint32_t tx_count; /* total VELOCITY_CMD frames sent */
|
||||
uint32_t rx_count; /* total valid FEEDBACK frames received */
|
||||
uint16_t err_count; /* CAN error frame count */
|
||||
uint8_t bus_off; /* 1 = bus-off state */
|
||||
uint8_t node_faults; /* bit0 = node 0 fault active, bit1 = node 1 */
|
||||
int16_t vel0_rpm; /* node 0 current velocity (RPM) */
|
||||
int16_t vel1_rpm; /* node 1 current velocity (RPM) */
|
||||
} jlink_tlm_can_stats_t; /* 16 bytes */
|
||||
|
||||
/* ---- Volatile state (read from main loop) ---- */
|
||||
typedef struct {
|
||||
/* Drive command - updated on JLINK_CMD_DRIVE */
|
||||
@ -229,6 +246,9 @@ typedef struct {
|
||||
|
||||
/* Fault log request (Issue #565) - set by JLINK_CMD_FAULT_LOG_GET, cleared by main loop */
|
||||
volatile uint8_t fault_log_req;
|
||||
|
||||
/* CAN stats request (Issue #597) - set by JLINK_CMD_CAN_STATS_GET, cleared by main loop */
|
||||
volatile uint8_t can_stats_req;
|
||||
} JLinkState;
|
||||
|
||||
extern volatile JLinkState jlink_state;
|
||||
@ -281,13 +301,6 @@ JLinkSchedSetBuf *jlink_get_sched_set(void);
|
||||
*/
|
||||
void jlink_send_motor_current_tlm(const jlink_tlm_motor_current_t *tlm);
|
||||
|
||||
/*
|
||||
* jlink_send_slope_tlm(tlm) - transmit JLINK_TLM_SLOPE (0x88) frame
|
||||
* (10 bytes total) to Jetson. Issue #600.
|
||||
* Rate-limiting is handled by slope_estimator_send_tlm(); call from there only.
|
||||
*/
|
||||
void jlink_send_slope_tlm(const jlink_tlm_slope_t *tlm);
|
||||
|
||||
/*
|
||||
* jlink_send_fault_log(fl) - transmit JLINK_TLM_FAULT_LOG (0x87) frame
|
||||
* (26 bytes) on boot (if fault log non-empty) and in response to
|
||||
@ -295,4 +308,18 @@ void jlink_send_slope_tlm(const jlink_tlm_slope_t *tlm);
|
||||
*/
|
||||
void jlink_send_fault_log(const jlink_tlm_fault_log_t *fl);
|
||||
|
||||
/*
|
||||
* jlink_send_slope_tlm(tlm) - transmit JLINK_TLM_SLOPE (0x88) frame
|
||||
* (10 bytes) at SLOPE_TLM_HZ (1 Hz). Called from slope_estimator_send_tlm().
|
||||
* Issue #600.
|
||||
*/
|
||||
void jlink_send_slope_tlm(const jlink_tlm_slope_t *tlm);
|
||||
|
||||
/*
|
||||
* jlink_send_can_stats(tlm) - transmit JLINK_TLM_CAN_STATS (0x89) frame
|
||||
* (22 bytes) at CAN_TLM_HZ (1 Hz) and in response to CAN_STATS_GET.
|
||||
* Issue #597.
|
||||
*/
|
||||
void jlink_send_can_stats(const jlink_tlm_can_stats_t *tlm);
|
||||
|
||||
#endif /* JLINK_H */
|
||||
|
||||
203
src/can_driver.c
Normal file
203
src/can_driver.c
Normal file
@ -0,0 +1,203 @@
|
||||
/* CAN bus driver for BLDC motor controllers (Issue #597)
|
||||
* CAN2 on PB12 (RX, AF9) / PB13 (TX, AF9) at 500 kbps
|
||||
* Filter bank 14 (first CAN2 bank; SlaveStartFilterBank=14)
|
||||
*/
|
||||
|
||||
#include "can_driver.h"
|
||||
#include "stm32f7xx_hal.h"
|
||||
#include <string.h>
|
||||
|
||||
static CAN_HandleTypeDef s_can;
|
||||
static volatile can_feedback_t s_feedback[CAN_NUM_MOTORS];
|
||||
static volatile can_stats_t s_stats;
|
||||
|
||||
void can_driver_init(void)
|
||||
{
|
||||
/* CAN2 requires CAN1 master clock for shared filter RAM */
|
||||
__HAL_RCC_CAN1_CLK_ENABLE();
|
||||
__HAL_RCC_CAN2_CLK_ENABLE();
|
||||
__HAL_RCC_GPIOB_CLK_ENABLE();
|
||||
|
||||
/* PB12 = CAN2_RX, PB13 = CAN2_TX, AF9 */
|
||||
GPIO_InitTypeDef gpio = {0};
|
||||
gpio.Pin = GPIO_PIN_12 | GPIO_PIN_13;
|
||||
gpio.Mode = GPIO_MODE_AF_PP;
|
||||
gpio.Pull = GPIO_NOPULL;
|
||||
gpio.Speed = GPIO_SPEED_FREQ_HIGH;
|
||||
gpio.Alternate = GPIO_AF9_CAN2;
|
||||
HAL_GPIO_Init(GPIOB, &gpio);
|
||||
|
||||
/* 500 kbps @ APB1=54 MHz: PSC=6, BS1=13tq, BS2=4tq, SJW=1tq
|
||||
* bit_time = 6 × (1+13+4) / 54000000 = 2 µs → 500 kbps
|
||||
* sample point = (1+13)/18 = 77.8% */
|
||||
s_can.Instance = CAN2;
|
||||
s_can.Init.Prescaler = CAN_PRESCALER;
|
||||
s_can.Init.Mode = CAN_MODE_NORMAL;
|
||||
s_can.Init.SyncJumpWidth = CAN_SJW_1TQ;
|
||||
s_can.Init.TimeSeg1 = CAN_BS1_13TQ;
|
||||
s_can.Init.TimeSeg2 = CAN_BS2_4TQ;
|
||||
s_can.Init.TimeTriggeredMode = DISABLE;
|
||||
s_can.Init.AutoBusOff = ENABLE; /* HW recovery after 128×11 bits */
|
||||
s_can.Init.AutoWakeUp = DISABLE;
|
||||
s_can.Init.AutoRetransmission = ENABLE;
|
||||
s_can.Init.ReceiveFifoLocked = DISABLE;
|
||||
s_can.Init.TransmitFifoPriority = DISABLE;
|
||||
|
||||
if (HAL_CAN_Init(&s_can) != HAL_OK) {
|
||||
s_stats.bus_off = 1u;
|
||||
return;
|
||||
}
|
||||
|
||||
/* Filter bank 14: 32-bit mask, FIFO0, accept std IDs 0x200–0x21F
|
||||
* FilterIdHigh = 0x200 << 5 = 0x4000 (base ID shifted to bit[15:5])
|
||||
* FilterMaskHigh = 0x7E0 << 5 = 0xFC00 (mask: top 6 bits must match) */
|
||||
CAN_FilterTypeDef flt = {0};
|
||||
flt.FilterBank = 14u;
|
||||
flt.FilterMode = CAN_FILTERMODE_IDMASK;
|
||||
flt.FilterScale = CAN_FILTERSCALE_32BIT;
|
||||
flt.FilterIdHigh = (uint16_t)(CAN_FILTER_STDID << 5u);
|
||||
flt.FilterIdLow = 0u;
|
||||
flt.FilterMaskIdHigh = (uint16_t)(CAN_FILTER_MASK << 5u);
|
||||
flt.FilterMaskIdLow = 0u;
|
||||
flt.FilterFIFOAssignment = CAN_RX_FIFO0;
|
||||
flt.FilterActivation = CAN_FILTER_ENABLE;
|
||||
flt.SlaveStartFilterBank = 14u;
|
||||
|
||||
if (HAL_CAN_ConfigFilter(&s_can, &flt) != HAL_OK) {
|
||||
s_stats.bus_off = 1u;
|
||||
return;
|
||||
}
|
||||
|
||||
HAL_CAN_Start(&s_can);
|
||||
|
||||
memset((void *)s_feedback, 0, sizeof(s_feedback));
|
||||
memset((void *)&s_stats, 0, sizeof(s_stats));
|
||||
}
|
||||
|
||||
void can_driver_send_cmd(uint8_t node_id, const can_cmd_t *cmd)
|
||||
{
|
||||
if (node_id >= CAN_NUM_MOTORS || s_stats.bus_off) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* Payload: [vel_lo, vel_hi, torque_lo, torque_hi] — little-endian */
|
||||
uint8_t data[4];
|
||||
data[0] = (uint8_t)((uint16_t)cmd->velocity_rpm & 0xFFu);
|
||||
data[1] = (uint8_t)((uint16_t)cmd->velocity_rpm >> 8u);
|
||||
data[2] = (uint8_t)((uint16_t)cmd->torque_x100 & 0xFFu);
|
||||
data[3] = (uint8_t)((uint16_t)cmd->torque_x100 >> 8u);
|
||||
|
||||
CAN_TxHeaderTypeDef hdr = {0};
|
||||
hdr.StdId = CAN_ID_VEL_CMD_BASE + (uint32_t)node_id;
|
||||
hdr.IDE = CAN_ID_STD;
|
||||
hdr.RTR = CAN_RTR_DATA;
|
||||
hdr.DLC = 4u;
|
||||
|
||||
uint32_t mailbox;
|
||||
if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) {
|
||||
if (HAL_CAN_AddTxMessage(&s_can, &hdr, data, &mailbox) == HAL_OK) {
|
||||
s_stats.tx_count++;
|
||||
} else {
|
||||
s_stats.err_count++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void can_driver_send_enable(uint8_t node_id, bool enable)
|
||||
{
|
||||
if (node_id >= CAN_NUM_MOTORS || s_stats.bus_off) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t data[1] = { enable ? 1u : 0u };
|
||||
|
||||
CAN_TxHeaderTypeDef hdr = {0};
|
||||
hdr.StdId = CAN_ID_ENABLE_CMD_BASE + (uint32_t)node_id;
|
||||
hdr.IDE = CAN_ID_STD;
|
||||
hdr.RTR = CAN_RTR_DATA;
|
||||
hdr.DLC = 1u;
|
||||
|
||||
uint32_t mailbox;
|
||||
if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) {
|
||||
if (HAL_CAN_AddTxMessage(&s_can, &hdr, data, &mailbox) == HAL_OK) {
|
||||
s_stats.tx_count++;
|
||||
} else {
|
||||
s_stats.err_count++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void can_driver_process(void)
|
||||
{
|
||||
/* Check for bus-off */
|
||||
if (s_can.Instance->ESR & CAN_ESR_BOFF) {
|
||||
s_stats.bus_off = 1u;
|
||||
return;
|
||||
}
|
||||
s_stats.bus_off = 0u;
|
||||
|
||||
/* Drain FIFO0 */
|
||||
while (HAL_CAN_GetRxFifoFillLevel(&s_can, CAN_RX_FIFO0) > 0u) {
|
||||
CAN_RxHeaderTypeDef rxhdr;
|
||||
uint8_t rxdata[8];
|
||||
|
||||
if (HAL_CAN_GetRxMessage(&s_can, CAN_RX_FIFO0, &rxhdr, rxdata) != HAL_OK) {
|
||||
s_stats.err_count++;
|
||||
break;
|
||||
}
|
||||
|
||||
/* Only process data frames with standard IDs */
|
||||
if (rxhdr.IDE != CAN_ID_STD || rxhdr.RTR != CAN_RTR_DATA) {
|
||||
continue;
|
||||
}
|
||||
|
||||
/* Decode feedback frames: base 0x200, one per node */
|
||||
uint32_t nid_u = rxhdr.StdId - CAN_ID_FEEDBACK_BASE;
|
||||
if (nid_u >= CAN_NUM_MOTORS || rxhdr.DLC < 8u) {
|
||||
continue;
|
||||
}
|
||||
|
||||
uint8_t nid = (uint8_t)nid_u;
|
||||
|
||||
/* Layout: [0-1] vel_rpm [2-3] current_ma [4-5] pos_x100 [6] temp_c [7] fault */
|
||||
s_feedback[nid].velocity_rpm = (int16_t)((uint16_t)rxdata[0] | ((uint16_t)rxdata[1] << 8u));
|
||||
s_feedback[nid].current_ma = (int16_t)((uint16_t)rxdata[2] | ((uint16_t)rxdata[3] << 8u));
|
||||
s_feedback[nid].position_x100 = (int16_t)((uint16_t)rxdata[4] | ((uint16_t)rxdata[5] << 8u));
|
||||
s_feedback[nid].temperature_c = (int8_t)rxdata[6];
|
||||
s_feedback[nid].fault = rxdata[7];
|
||||
s_feedback[nid].last_rx_ms = HAL_GetTick();
|
||||
|
||||
s_stats.rx_count++;
|
||||
}
|
||||
}
|
||||
|
||||
bool can_driver_get_feedback(uint8_t node_id, can_feedback_t *out)
|
||||
{
|
||||
if (node_id >= CAN_NUM_MOTORS || out == NULL) {
|
||||
return false;
|
||||
}
|
||||
if (s_feedback[node_id].last_rx_ms == 0u) {
|
||||
return false;
|
||||
}
|
||||
memcpy(out, (const void *)&s_feedback[node_id], sizeof(can_feedback_t));
|
||||
return true;
|
||||
}
|
||||
|
||||
bool can_driver_is_alive(uint8_t node_id, uint32_t now_ms)
|
||||
{
|
||||
if (node_id >= CAN_NUM_MOTORS) {
|
||||
return false;
|
||||
}
|
||||
if (s_feedback[node_id].last_rx_ms == 0u) {
|
||||
return false;
|
||||
}
|
||||
return (now_ms - s_feedback[node_id].last_rx_ms) < CAN_NODE_TIMEOUT_MS;
|
||||
}
|
||||
|
||||
void can_driver_get_stats(can_stats_t *out)
|
||||
{
|
||||
if (out == NULL) {
|
||||
return;
|
||||
}
|
||||
memcpy(out, (const void *)&s_stats, sizeof(can_stats_t));
|
||||
}
|
||||
34
src/jlink.c
34
src/jlink.c
@ -271,6 +271,10 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
|
||||
jlink_state.fault_log_req = 1u;
|
||||
break;
|
||||
|
||||
case JLINK_CMD_CAN_STATS_GET: /* Issue #597: request CAN bus statistics */
|
||||
jlink_state.can_stats_req = 1u;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
@ -569,8 +573,9 @@ void jlink_send_slope_tlm(const jlink_tlm_slope_t *tlm)
|
||||
void jlink_send_fault_log(const jlink_tlm_fault_log_t *fl)
|
||||
{
|
||||
/*
|
||||
* Frame: [STX][LEN][0x86][20 bytes fault_log][CRC_hi][CRC_lo][ETX]
|
||||
* Total: 1+1+1+20+2+1 = 26 bytes
|
||||
* Frame: [STX][LEN][0x87][20 bytes fault_log][CRC_hi][CRC_lo][ETX]
|
||||
* LEN = 1 + 20 = 21; total = 26 bytes
|
||||
* At 921600 baud: 26x10/921600 ~0.28 ms -- safe to block.
|
||||
*/
|
||||
static uint8_t frame[26];
|
||||
const uint8_t plen = (uint8_t)sizeof(jlink_tlm_fault_log_t); /* 20 */
|
||||
@ -588,3 +593,28 @@ void jlink_send_fault_log(const jlink_tlm_fault_log_t *fl)
|
||||
|
||||
jlink_tx_locked(frame, sizeof(frame));
|
||||
}
|
||||
|
||||
/* ---- jlink_send_can_stats() -- Issue #597 ---- */
|
||||
void jlink_send_can_stats(const jlink_tlm_can_stats_t *tlm)
|
||||
{
|
||||
/*
|
||||
* Frame: [STX][LEN][0x89][16 bytes CAN_STATS][CRC_hi][CRC_lo][ETX]
|
||||
* LEN = 1 + 16 = 17; total = 22 bytes
|
||||
* At 921600 baud: 22x10/921600 ~0.24 ms -- safe to block.
|
||||
*/
|
||||
static uint8_t frame[22];
|
||||
const uint8_t plen = (uint8_t)sizeof(jlink_tlm_can_stats_t); /* 16 */
|
||||
const uint8_t len = 1u + plen; /* 17 */
|
||||
|
||||
frame[0] = JLINK_STX;
|
||||
frame[1] = len;
|
||||
frame[2] = JLINK_TLM_CAN_STATS;
|
||||
memcpy(&frame[3], tlm, plen);
|
||||
|
||||
uint16_t crc = crc16_xmodem(&frame[2], len);
|
||||
frame[3 + plen] = (uint8_t)(crc >> 8);
|
||||
frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
|
||||
frame[3 + plen + 2] = JLINK_ETX;
|
||||
|
||||
jlink_tx_locked(frame, sizeof(frame));
|
||||
}
|
||||
|
||||
73
src/main.c
73
src/main.c
@ -32,6 +32,7 @@
|
||||
#include "watchdog.h"
|
||||
#include "pid_flash.h"
|
||||
#include "fault_handler.h"
|
||||
#include "can_driver.h"
|
||||
#include "servo_bus.h"
|
||||
#include "gimbal.h"
|
||||
#include <math.h>
|
||||
@ -192,6 +193,9 @@ int main(void) {
|
||||
/* Init Jetson serial binary protocol on USART1 (PB6/PB7) at 921600 baud */
|
||||
jlink_init();
|
||||
|
||||
/* Init CAN2 BLDC motor controller bus — PB12/PB13, 500 kbps (Issue #597) */
|
||||
can_driver_init();
|
||||
|
||||
/* Send fault log summary on boot if a prior fault was recorded (Issue #565) */
|
||||
if (fault_get_last_type() != FAULT_NONE) {
|
||||
fault_log_entry_t fle;
|
||||
@ -282,6 +286,8 @@ int main(void) {
|
||||
uint32_t crsf_telem_tick = 0; /* CRSF uplink telemetry TX timer */
|
||||
uint32_t jlink_tlm_tick = 0; /* Jetson binary telemetry TX timer */
|
||||
uint32_t pm_tlm_tick = 0; /* JLINK_TLM_POWER transmit timer */
|
||||
uint32_t can_cmd_tick = 0; /* CAN velocity command TX timer (Issue #597) */
|
||||
uint32_t can_tlm_tick = 0; /* JLINK_TLM_CAN_STATS transmit timer (Issue #597) */
|
||||
uint8_t pm_pwm_phase = 0; /* Software PWM counter for sleep LED */
|
||||
const float dt = 1.0f / PID_LOOP_HZ; /* 1ms at 1kHz */
|
||||
|
||||
@ -351,6 +357,9 @@ int main(void) {
|
||||
/* Drain JLink DMA circular buffer and parse frames */
|
||||
jlink_process();
|
||||
|
||||
/* CAN bus RX: drain FIFO0 and parse feedback frames (Issue #597) */
|
||||
can_driver_process();
|
||||
|
||||
/* Handle JLink one-shot flags from Jetson binary protocol */
|
||||
if (jlink_state.estop_req) {
|
||||
jlink_state.estop_req = 0u;
|
||||
@ -433,6 +442,36 @@ int main(void) {
|
||||
jlink_send_fault_log(&ftlm);
|
||||
}
|
||||
|
||||
/* CAN_STATS: send telemetry on demand or at CAN_TLM_HZ (1 Hz) (Issue #597) */
|
||||
{
|
||||
bool send_can = (jlink_state.can_stats_req != 0u);
|
||||
if (!send_can && (now - can_tlm_tick >= (1000u / CAN_TLM_HZ))) {
|
||||
send_can = true;
|
||||
}
|
||||
if (send_can) {
|
||||
jlink_state.can_stats_req = 0u;
|
||||
can_tlm_tick = now;
|
||||
can_stats_t cs;
|
||||
can_driver_get_stats(&cs);
|
||||
can_feedback_t fb0, fb1;
|
||||
jlink_tlm_can_stats_t ctlm;
|
||||
memset(&ctlm, 0, sizeof(ctlm));
|
||||
ctlm.tx_count = cs.tx_count;
|
||||
ctlm.rx_count = cs.rx_count;
|
||||
ctlm.err_count = cs.err_count;
|
||||
ctlm.bus_off = cs.bus_off;
|
||||
if (can_driver_get_feedback(CAN_NODE_LEFT, &fb0)) {
|
||||
ctlm.vel0_rpm = fb0.velocity_rpm;
|
||||
ctlm.node_faults |= (fb0.fault ? 1u : 0u);
|
||||
}
|
||||
if (can_driver_get_feedback(CAN_NODE_RIGHT, &fb1)) {
|
||||
ctlm.vel1_rpm = fb1.velocity_rpm;
|
||||
ctlm.node_faults |= (fb1.fault ? 2u : 0u);
|
||||
}
|
||||
jlink_send_can_stats(&ctlm);
|
||||
}
|
||||
}
|
||||
|
||||
/* Power management: CRSF/JLink activity or armed state resets idle timer */
|
||||
if ((crsf_state.last_rx_ms != 0 && (now - crsf_state.last_rx_ms) < 500) ||
|
||||
jlink_is_active(now) ||
|
||||
@ -589,6 +628,40 @@ int main(void) {
|
||||
}
|
||||
}
|
||||
|
||||
/* CAN BLDC: enable/disable follows arm state (Issue #597) */
|
||||
{
|
||||
static bool s_can_enabled = false;
|
||||
bool armed_now = (bal.state == BALANCE_ARMED);
|
||||
if (armed_now && !s_can_enabled) {
|
||||
can_driver_send_enable(CAN_NODE_LEFT, true);
|
||||
can_driver_send_enable(CAN_NODE_RIGHT, true);
|
||||
s_can_enabled = true;
|
||||
} else if (!armed_now && s_can_enabled) {
|
||||
can_driver_send_enable(CAN_NODE_LEFT, false);
|
||||
can_driver_send_enable(CAN_NODE_RIGHT, false);
|
||||
s_can_enabled = false;
|
||||
}
|
||||
}
|
||||
|
||||
/* CAN BLDC: send velocity/torque commands at CAN_TX_RATE_HZ (100 Hz) (Issue #597)
|
||||
* Converts balance PID output + blended steer to per-wheel RPM.
|
||||
* Left wheel: speed_rpm + steer_rpm (differential drive)
|
||||
* Right wheel: speed_rpm - steer_rpm */
|
||||
if (now - can_cmd_tick >= (1000u / CAN_TX_RATE_HZ)) {
|
||||
can_cmd_tick = now;
|
||||
int16_t can_steer = mode_manager_get_steer(&mode);
|
||||
int32_t speed_rpm = (int32_t)bal.motor_cmd * CAN_RPM_SCALE;
|
||||
int32_t steer_rpm = (int32_t)can_steer * CAN_RPM_SCALE / 2;
|
||||
can_cmd_t cmd_l = { (int16_t)(speed_rpm + steer_rpm), 0 };
|
||||
can_cmd_t cmd_r = { (int16_t)(speed_rpm - steer_rpm), 0 };
|
||||
if (bal.state != BALANCE_ARMED) {
|
||||
cmd_l.velocity_rpm = 0;
|
||||
cmd_r.velocity_rpm = 0;
|
||||
}
|
||||
can_driver_send_cmd(CAN_NODE_LEFT, &cmd_l);
|
||||
can_driver_send_cmd(CAN_NODE_RIGHT, &cmd_r);
|
||||
}
|
||||
|
||||
/* CRSF telemetry uplink — battery voltage + arm state at 1 Hz.
|
||||
* Sends battery sensor frame (0x08) and flight mode frame (0x21)
|
||||
* back to ELRS TX module so the pilot's handset OSD shows Vbat + state. */
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user