sl-firmware
f59bc9931e
- CAN1_SCE_IRQHandler: detects bus-off/error-passive/error-warning from ESR - can_driver_watchdog_tick(): polls ESR each cycle, auto-restarts after CAN_WDOG_RESTART_MS (200ms) - can_wdog_t: tracks restart_count, busoff_count, errpassive_count, errwarn_count, tec, rec - JLink TLM code 0x8F (JLINK_TLM_CAN_WDOG) with jlink_send_can_wdog_tlm() - main.c: calls watchdog_tick() each loop, sends CAN wdog TLM at 1 Hz - TEST_HOST: inject_esr() stub + busoff_pending flag fixes t=0 sentinel ambiguity - test/test_can_watchdog.c: 23 unit tests, all pass Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
254 lines
9.8 KiB
C
254 lines
9.8 KiB
C
/* CAN bus driver (Issues #597, #676, #674, #694) */
|
|
#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;
|
|
static can_ext_frame_cb_t s_ext_cb = NULL;
|
|
static can_std_frame_cb_t s_std_cb = NULL;
|
|
static volatile can_wdog_t s_wdog;
|
|
|
|
#ifdef TEST_HOST
|
|
static volatile uint32_t s_test_esr = 0u;
|
|
void can_driver_inject_esr(uint32_t v) { s_test_esr = v; }
|
|
static uint32_t _read_esr(void) { return s_test_esr; }
|
|
static HAL_StatusTypeDef _can_restart(void) {
|
|
HAL_CAN_Stop(&s_can); s_test_esr = 0u; return HAL_CAN_Start(&s_can);
|
|
}
|
|
#else
|
|
static uint32_t _read_esr(void) { return s_can.Instance->ESR; }
|
|
static HAL_StatusTypeDef _can_restart(void) {
|
|
HAL_CAN_Stop(&s_can); return HAL_CAN_Start(&s_can);
|
|
}
|
|
#endif
|
|
|
|
void can_driver_init(void)
|
|
{
|
|
__HAL_RCC_CAN1_CLK_ENABLE();
|
|
__HAL_RCC_GPIOB_CLK_ENABLE();
|
|
GPIO_InitTypeDef gpio = {0};
|
|
gpio.Pin = GPIO_PIN_8 | GPIO_PIN_9;
|
|
gpio.Mode = GPIO_MODE_AF_PP; gpio.Pull = GPIO_NOPULL;
|
|
gpio.Speed = GPIO_SPEED_FREQ_HIGH; gpio.Alternate = GPIO_AF9_CAN1;
|
|
HAL_GPIO_Init(GPIOB, &gpio);
|
|
s_can.Instance = CAN1;
|
|
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;
|
|
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; }
|
|
CAN_FilterTypeDef flt = {0};
|
|
flt.FilterBank = 0u; flt.FilterMode = CAN_FILTERMODE_IDMASK;
|
|
flt.FilterScale = CAN_FILTERSCALE_32BIT;
|
|
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; }
|
|
CAN_FilterTypeDef flt2 = {0};
|
|
flt2.FilterBank = 15u; flt2.FilterMode = CAN_FILTERMODE_IDMASK;
|
|
flt2.FilterScale = CAN_FILTERSCALE_32BIT;
|
|
flt2.FilterIdLow = 0x0004u; flt2.FilterMaskIdLow = 0x0004u;
|
|
flt2.FilterFIFOAssignment = CAN_RX_FIFO1;
|
|
flt2.FilterActivation = CAN_FILTER_ENABLE;
|
|
flt2.SlaveStartFilterBank = 14u;
|
|
if (HAL_CAN_ConfigFilter(&s_can, &flt2) != HAL_OK) { s_stats.bus_off = 1u; return; }
|
|
HAL_CAN_Start(&s_can);
|
|
#ifndef TEST_HOST
|
|
HAL_NVIC_SetPriority(CAN1_SCE_IRQn, 5u, 0u);
|
|
HAL_NVIC_EnableIRQ(CAN1_SCE_IRQn);
|
|
HAL_CAN_ActivateNotification(&s_can, CAN_IT_BUSOFF | CAN_IT_ERROR_PASSIVE | CAN_IT_ERROR_WARNING);
|
|
#endif
|
|
memset((void *)s_feedback, 0, sizeof(s_feedback));
|
|
memset((void *)&s_stats, 0, sizeof(s_stats));
|
|
memset((void *)&s_wdog, 0, sizeof(s_wdog));
|
|
}
|
|
|
|
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;
|
|
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)
|
|
{
|
|
if (_read_esr() & CAN_ESR_BOFF) { s_stats.bus_off = 1u; return; }
|
|
s_stats.bus_off = 0u;
|
|
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;
|
|
}
|
|
if (rxhdr.IDE != CAN_ID_STD || rxhdr.RTR != CAN_RTR_DATA) continue;
|
|
if (s_std_cb != NULL) s_std_cb((uint16_t)rxhdr.StdId, rxdata, (uint8_t)rxhdr.DLC);
|
|
uint32_t nid_u = rxhdr.StdId - CAN_ID_FEEDBACK_BASE;
|
|
if (nid_u < CAN_NUM_MOTORS && rxhdr.DLC >= 8u) {
|
|
uint8_t nid = (uint8_t)nid_u;
|
|
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++;
|
|
}
|
|
while (HAL_CAN_GetRxFifoFillLevel(&s_can, CAN_RX_FIFO1) > 0u) {
|
|
CAN_RxHeaderTypeDef rxhdr; uint8_t rxdata[8];
|
|
if (HAL_CAN_GetRxMessage(&s_can, CAN_RX_FIFO1, &rxhdr, rxdata) != HAL_OK) {
|
|
s_stats.err_count++; break;
|
|
}
|
|
if (rxhdr.IDE != CAN_ID_EXT || rxhdr.RTR != CAN_RTR_DATA) continue;
|
|
if (s_ext_cb != NULL) s_ext_cb(rxhdr.ExtId, rxdata, (uint8_t)rxhdr.DLC);
|
|
s_stats.rx_count++;
|
|
}
|
|
}
|
|
|
|
void can_driver_set_ext_cb(can_ext_frame_cb_t cb) { s_ext_cb = cb; }
|
|
void can_driver_set_std_cb(can_std_frame_cb_t cb) { s_std_cb = cb; }
|
|
|
|
void can_driver_send_ext(uint32_t ext_id, const uint8_t *data, uint8_t len)
|
|
{
|
|
if (s_stats.bus_off || len > 8u) return;
|
|
CAN_TxHeaderTypeDef hdr = {0};
|
|
hdr.ExtId = ext_id; hdr.IDE = CAN_ID_EXT; hdr.RTR = CAN_RTR_DATA; hdr.DLC = len;
|
|
uint32_t mailbox;
|
|
if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) {
|
|
if (HAL_CAN_AddTxMessage(&s_can, &hdr, (uint8_t *)data, &mailbox) == HAL_OK)
|
|
s_stats.tx_count++;
|
|
else s_stats.err_count++;
|
|
}
|
|
}
|
|
|
|
void can_driver_send_std(uint16_t std_id, const uint8_t *data, uint8_t len)
|
|
{
|
|
if (s_stats.bus_off || len > 8u) return;
|
|
CAN_TxHeaderTypeDef hdr = {0};
|
|
hdr.StdId = std_id; hdr.IDE = CAN_ID_STD; hdr.RTR = CAN_RTR_DATA; hdr.DLC = len;
|
|
uint32_t mailbox;
|
|
if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) {
|
|
if (HAL_CAN_AddTxMessage(&s_can, &hdr, (uint8_t *)data, &mailbox) == HAL_OK)
|
|
s_stats.tx_count++;
|
|
else s_stats.err_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 || 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));
|
|
}
|
|
|
|
/* SCE interrupt handler -- Issue #694 */
|
|
#ifndef TEST_HOST
|
|
void CAN1_SCE_IRQHandler(void)
|
|
{
|
|
uint32_t esr = s_can.Instance->ESR;
|
|
if (esr & CAN_ESR_BOFF) {
|
|
if (s_wdog.error_state != CAN_ERR_BUS_OFF) s_wdog.busoff_count++;
|
|
s_wdog.error_state = CAN_ERR_BUS_OFF; s_stats.bus_off = 1u;
|
|
} else if (esr & CAN_ESR_EPVF) {
|
|
if (s_wdog.error_state < CAN_ERR_ERROR_PASSIVE) {
|
|
s_wdog.errpassive_count++; s_wdog.error_state = CAN_ERR_ERROR_PASSIVE;
|
|
}
|
|
} else if (esr & CAN_ESR_EWGF) {
|
|
if (s_wdog.error_state < CAN_ERR_WARNING) {
|
|
s_wdog.errwarn_count++; s_wdog.error_state = CAN_ERR_WARNING;
|
|
}
|
|
}
|
|
__HAL_CAN_CLEAR_FLAG(&s_can, CAN_FLAG_ERRI);
|
|
HAL_CAN_IRQHandler(&s_can);
|
|
}
|
|
#endif
|
|
|
|
/* Watchdog tick -- Issue #694 */
|
|
can_error_state_t can_driver_watchdog_tick(uint32_t now_ms)
|
|
{
|
|
uint32_t esr = _read_esr();
|
|
if (esr & CAN_ESR_BOFF) {
|
|
if (s_wdog.error_state != CAN_ERR_BUS_OFF) {
|
|
s_wdog.busoff_count++; s_wdog.busoff_ms = now_ms; s_wdog.busoff_pending = 1u;
|
|
}
|
|
s_wdog.error_state = CAN_ERR_BUS_OFF; s_stats.bus_off = 1u;
|
|
} else if (esr & CAN_ESR_EPVF) {
|
|
if (s_wdog.error_state < CAN_ERR_ERROR_PASSIVE) {
|
|
s_wdog.errpassive_count++; s_wdog.error_state = CAN_ERR_ERROR_PASSIVE;
|
|
}
|
|
} else if (esr & CAN_ESR_EWGF) {
|
|
if (s_wdog.error_state < CAN_ERR_WARNING) {
|
|
s_wdog.errwarn_count++; s_wdog.error_state = CAN_ERR_WARNING;
|
|
}
|
|
} else {
|
|
if (s_wdog.error_state > CAN_ERR_NOMINAL && s_wdog.error_state < CAN_ERR_BUS_OFF)
|
|
s_wdog.error_state = CAN_ERR_NOMINAL;
|
|
}
|
|
if (s_wdog.busoff_pending && s_wdog.error_state == CAN_ERR_BUS_OFF &&
|
|
(now_ms - s_wdog.busoff_ms) >= CAN_WDOG_RESTART_MS) {
|
|
if (_can_restart() == HAL_OK) {
|
|
s_wdog.restart_count++; s_wdog.error_state = CAN_ERR_NOMINAL;
|
|
s_wdog.busoff_pending = 0u; s_stats.bus_off = 0u;
|
|
} else {
|
|
s_wdog.busoff_ms = now_ms;
|
|
}
|
|
}
|
|
s_wdog.tec = (uint8_t)((esr >> CAN_ESR_TEC_Pos) & 0xFFu);
|
|
s_wdog.rec = (uint8_t)((esr >> CAN_ESR_REC_Pos) & 0xFFu);
|
|
return s_wdog.error_state;
|
|
}
|
|
|
|
void can_driver_get_wdog(can_wdog_t *out)
|
|
{
|
|
if (out == NULL) return;
|
|
memcpy(out, (const void *)&s_wdog, sizeof(can_wdog_t));
|
|
}
|