From fd54f5cc0b8ecc522b7d72f38451768183a51f39 Mon Sep 17 00:00:00 2001 From: miketsai Date: Fri, 12 Jun 2026 16:47:18 +0800 Subject: [PATCH] feat(can/rx): MCP2515 CAN RX interrupt thread for ECU ID 0x50 - can_bus.h: add can_ecu_status_t struct + can_bus_get_ecu_status() declaration - can_bus.c: add gpio_interrupt_init() (gpio sysfs, falling-edge poll) - can_bus.c: add can_rx_thread() - poll gpio63 INT pin, parse 0x50 frame (throttle_status, pedal_volt, led_flash_timer, can_lose_ecu, throttle_limit_off, heart_beam) with CAN_LOSE timeout detection (100ms) - can_bus.c: start/join can_rx thread in can_bus_init()/can_bus_close() - can_bus.c: add can_bus_get_ecu_status() thread-safe snapshot Co-Authored-By: Claude Sonnet 4.6 --- include/host_stream/can_bus.h | 26 +++++ src/host_stream/can_bus.c | 174 ++++++++++++++++++++++++++++++++++ 2 files changed, 200 insertions(+) diff --git a/include/host_stream/can_bus.h b/include/host_stream/can_bus.h index f5c52bb..eb7e2fa 100644 --- a/include/host_stream/can_bus.h +++ b/include/host_stream/can_bus.h @@ -64,4 +64,30 @@ void can_bus_send_control_cmd(const can_ctrl_cmd_t *ctrl); /* can_bus_close — drain queue, join writer thread, close SPI device. */ void can_bus_close(void); +/* + * can_ecu_status_t — parsed signals from ECU CAN ID 0x50 (throttle_status frame). + * + * Frame layout (Intel byte order): + * frame[0..1] = throttle_status (bits 0-15, uint16) + * frame[2..3] = throttle_pedal_volt (bits 16-31, uint16) + * frame[4..5] = led_flash_timer (bits 32-47, uint16) + * frame[6] bit 0 = can_lose_ecu (bit 48) + * frame[6] bit 7 = throttle_limit_off (bit 55) + * frame[7] = heart_beam (bits 56-63, uint8) + * + * can_lose_timeout: KL630 self-detected, set to 1 when no ECU frame for >100ms. + */ +typedef struct { + uint16_t throttle_status; /* bits 0-15: current throttle value, 0-8191 */ + uint16_t throttle_pedal_volt; /* bits 16-31: pedal voltage */ + uint16_t led_flash_timer; /* bits 32-47: LED flash timer */ + uint8_t can_lose_ecu; /* bit 48: ECU-reported CAN loss (0/1) */ + uint8_t throttle_limit_off; /* bit 55: throttle limit off flag (0/1) */ + uint8_t heart_beam; /* bits 56-63: heartbeat counter */ + int can_lose_timeout; /* KL630 self-detected: 1 if ECU silent >100ms*/ +} can_ecu_status_t; + +/* can_bus_get_ecu_status — thread-safe snapshot of latest ECU status. */ +void can_bus_get_ecu_status(can_ecu_status_t *out); + #endif /* CAN_BUS_H */ diff --git a/src/host_stream/can_bus.c b/src/host_stream/can_bus.c index 359b76d..9a8586c 100644 --- a/src/host_stream/can_bus.c +++ b/src/host_stream/can_bus.c @@ -27,6 +27,10 @@ #include #include #include +#include +#include +#include +#include #include "mcp2515.h" #include "can_bus.h" @@ -37,6 +41,10 @@ static uint32_t s_can_id = 0x100; static int s_can_speed_kbps = 250; static uint32_t s_ctl_can_id = 0x75; +#define MCP2515_INT_GPIO 63 /* GPIO_1: MCP2515 INT pin, kernel gpio63 */ +#define CAN_ECU_ID 0x50 /* ECU → KL630: throttle_status frame */ +#define CAN_LOSE_TIMEOUT_MS 100 /* 5 × 20ms ECU cycle = lose threshold */ + /* SPI mutex (mcp2515 is not thread-safe) */ static pthread_mutex_t s_spi_mtx = PTHREAD_MUTEX_INITIALIZER; @@ -50,6 +58,12 @@ static pthread_mutex_t s_keepalive_mtx = PTHREAD_MUTEX_INITIALIZER; static pthread_t s_keepalive_tid; static volatile int s_keepalive_running = 0; +/* ECU RX state */ +static can_ecu_status_t s_ecu_status = {0}; +static pthread_mutex_t s_ecu_mtx = PTHREAD_MUTEX_INITIALIZER; +static pthread_t s_rx_tid; +static volatile int s_rx_running = 0; + /* Message queue -- same design as bt_uart.c */ typedef struct CanMsg { char *json; /* heap-allocated JSON string */ @@ -300,6 +314,144 @@ static void *can_writer_thread(void *arg) return NULL; } +/* GPIO sysfs setup for MCP2515 INT (falling-edge interrupt). */ +static int gpio_interrupt_init(int gpio_num) +{ + char path[64], buf[8]; + int fd; + + fd = open("/sys/class/gpio/export", O_WRONLY); + if (fd >= 0) { + snprintf(buf, sizeof(buf), "%d", gpio_num); + write(fd, buf, strlen(buf)); + close(fd); + /* EBUSY means already exported — not an error */ + } + + snprintf(path, sizeof(path), "/sys/class/gpio/gpio%d/direction", gpio_num); + fd = open(path, O_WRONLY); + if (fd < 0) { perror("[CAN-RX] direction open"); return -1; } + write(fd, "in", 2); + close(fd); + + snprintf(path, sizeof(path), "/sys/class/gpio/gpio%d/edge", gpio_num); + fd = open(path, O_WRONLY); + if (fd < 0) { perror("[CAN-RX] edge open"); return -1; } + write(fd, "falling", 7); + close(fd); + + snprintf(path, sizeof(path), "/sys/class/gpio/gpio%d/value", gpio_num); + fd = open(path, O_RDONLY | O_NONBLOCK); + if (fd < 0) perror("[CAN-RX] value open"); + return fd; +} + +/* RX interrupt thread: poll GPIO sysfs, read MCP2515, parse ECU frame 0x50. */ +static void *can_rx_thread(void *arg) +{ + (void)arg; + struct pollfd pfd; + char dummy[4]; + canid_t rid; + uint8_t rdata[8]; + uint8_t rlen; + struct timeval last_rx; + + pfd.fd = gpio_interrupt_init(MCP2515_INT_GPIO); + if (pfd.fd < 0) { + printf("[CAN-RX] gpio_interrupt_init failed, thread exit\n"); + return NULL; + } + pfd.events = POLLPRI | POLLERR; + + /* consume initial level so first real edge is not missed */ + lseek(pfd.fd, 0, SEEK_SET); + read(pfd.fd, dummy, sizeof(dummy)); + gettimeofday(&last_rx, NULL); + + pthread_mutex_lock(&s_ecu_mtx); + s_ecu_status.can_lose_timeout = 1; + pthread_mutex_unlock(&s_ecu_mtx); + + printf("[CAN-RX] thread started gpio%d ECU_ID=0x%03X LOSE_MS=%d\n", + MCP2515_INT_GPIO, CAN_ECU_ID, CAN_LOSE_TIMEOUT_MS); + + while (s_rx_running) { + int ret = poll(&pfd, 1, CAN_LOSE_TIMEOUT_MS); + if (!s_rx_running) break; + if (ret < 0) { + if (errno == EINTR) continue; + perror("[CAN-RX] poll"); + break; + } + + if (ret == 0) { + struct timeval now; + gettimeofday(&now, NULL); + long elapsed_ms = (now.tv_sec - last_rx.tv_sec) * 1000 + + (now.tv_usec - last_rx.tv_usec) / 1000; + if (elapsed_ms >= CAN_LOSE_TIMEOUT_MS) { + pthread_mutex_lock(&s_ecu_mtx); + if (!s_ecu_status.can_lose_timeout) { + s_ecu_status.can_lose_timeout = 1; + printf("[CAN-RX] CAN_LOSE timeout: no ECU frame for %ldms\n", elapsed_ms); + } + pthread_mutex_unlock(&s_ecu_mtx); + } + continue; + } + + if (!(pfd.revents & POLLPRI)) continue; + + lseek(pfd.fd, 0, SEEK_SET); + read(pfd.fd, dummy, sizeof(dummy)); + + rid = 0; rlen = 0; memset(rdata, 0, 8); + pthread_mutex_lock(&s_spi_mtx); + int rc = mcp2515_readMessage(s_dev, &rid, rdata, &rlen); + pthread_mutex_unlock(&s_spi_mtx); + if (rc != ERROR_OK) continue; + + if ((rid & CAN_SFF_MASK) == CAN_ECU_ID && rlen == 8) { + gettimeofday(&last_rx, NULL); + + uint16_t throttle_status = (uint16_t)rdata[0] | ((uint16_t)rdata[1] << 8); + uint16_t throttle_pedal_volt = (uint16_t)rdata[2] | ((uint16_t)rdata[3] << 8); + uint16_t led_flash_timer = (uint16_t)rdata[4] | ((uint16_t)rdata[5] << 8); + uint8_t can_lose_ecu = (rdata[6] >> 0) & 0x01; + uint8_t throttle_limit_off = (rdata[6] >> 7) & 0x01; + uint8_t heart_beam = rdata[7]; + + int was_lose; + pthread_mutex_lock(&s_ecu_mtx); + was_lose = s_ecu_status.can_lose_timeout; + s_ecu_status.throttle_status = throttle_status; + s_ecu_status.throttle_pedal_volt = throttle_pedal_volt; + s_ecu_status.led_flash_timer = led_flash_timer; + s_ecu_status.can_lose_ecu = can_lose_ecu; + s_ecu_status.throttle_limit_off = throttle_limit_off; + s_ecu_status.heart_beam = heart_beam; + s_ecu_status.can_lose_timeout = 0; + pthread_mutex_unlock(&s_ecu_mtx); + + if (was_lose) + printf("[CAN-RX] CAN_LOSE cleared: throttle=%u pedal_v=%u " + "led_timer=%u can_lose_ecu=%u limit_off=%u heart=%u\n", + throttle_status, throttle_pedal_volt, led_flash_timer, + can_lose_ecu, throttle_limit_off, heart_beam); + else + printf("[CAN-RX] 0x50 throttle=%u pedal_v=%u led_timer=%u " + "can_lose_ecu=%u limit_off=%u heart=%u\n", + throttle_status, throttle_pedal_volt, led_flash_timer, + can_lose_ecu, throttle_limit_off, heart_beam); + } + } + + close(pfd.fd); + printf("[CAN-RX] thread exit\n"); + return NULL; +} + /* Keep-alive thread: periodically resends the current control frame. */ static void *can_keepalive_thread(void *arg) { @@ -448,6 +600,16 @@ int can_bus_init(const char *spidev, int can_speed_kbps, uint32_t can_id) pthread_setname_np(s_keepalive_tid, "can_keepalive"); } + s_rx_running = 1; + if (pthread_create(&s_rx_tid, NULL, can_rx_thread, NULL) != 0) { + perror("[CAN-RX] pthread_create"); + s_rx_running = 0; + /* non-fatal: RX disabled, TX still works */ + } else { + pthread_setname_np(s_rx_tid, "can_rx"); + printf("[CAN] RX interrupt thread started (gpio%d)\n", MCP2515_INT_GPIO); + } + printf("[CAN] can_bus_init OK: %s @ %d kbps, CAN ID=0x%03X\n", s_dev->spi_dev->spidev_path, can_speed_kbps, can_id); printf("[CAN] control-frame mode enabled (ID=0x%03X, DLC=8, DATA[0]=cmd)\n", s_ctl_can_id); @@ -500,6 +662,10 @@ void can_bus_close(void) { if (!s_running) return; + s_rx_running = 0; + if (s_rx_tid) + pthread_join(s_rx_tid, NULL); + s_keepalive_running = 0; if (s_keepalive_tid) pthread_join(s_keepalive_tid, NULL); @@ -525,3 +691,11 @@ void can_bus_close(void) } printf("[CAN] bus closed\n"); } + +void can_bus_get_ecu_status(can_ecu_status_t *out) +{ + if (!out) return; + pthread_mutex_lock(&s_ecu_mtx); + *out = s_ecu_status; + pthread_mutex_unlock(&s_ecu_mtx); +}