diff --git a/include/can_driver.h b/include/can_driver.h new file mode 100644 index 0000000..15f067d --- /dev/null +++ b/include/can_driver.h @@ -0,0 +1,81 @@ +#ifndef CAN_DRIVER_H +#define CAN_DRIVER_H + +#include +#include + +/* 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 */ diff --git a/include/config.h b/include/config.h index 158b59f..dba294c 100644 --- a/include/config.h +++ b/include/config.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 diff --git a/include/jlink.h b/include/jlink.h index ab8dedf..e5db1aa 100644 --- a/include/jlink.h +++ b/include/jlink.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 @@ -87,6 +91,8 @@ #define JLINK_TLM_SCHED 0x85u /* jlink_tlm_sched_t (1+N*16 bytes, Issue #550) */ #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)) { @@ -178,6 +184,26 @@ typedef struct __attribute__((packed)) { uint32_t hfsr; /* SCB->HFSR */ } jlink_tlm_fault_log_t; /* 20 bytes */ +/* ---- Telemetry SLOPE payload (4 bytes, packed) Issue #600 ---- */ +/* Sent at SLOPE_TLM_HZ (1 Hz) by slope_estimator_send_tlm(). */ +typedef struct __attribute__((packed)) { + 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 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 */ @@ -220,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; @@ -279,4 +308,18 @@ void jlink_send_motor_current_tlm(const jlink_tlm_motor_current_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 */ diff --git a/src/can_driver.c b/src/can_driver.c new file mode 100644 index 0000000..fbdc73d --- /dev/null +++ b/src/can_driver.c @@ -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 + +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)); +} diff --git a/src/jlink.c b/src/jlink.c index fba8f08..a61faab 100644 --- a/src/jlink.c +++ b/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; } @@ -545,8 +549,9 @@ void jlink_send_sched_telemetry(const jlink_tlm_sched_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 */ @@ -564,3 +569,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)); +} diff --git a/src/main.c b/src/main.c index d880632..1aefc79 100644 --- a/src/main.c +++ b/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 @@ -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. */