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 <noreply@anthropic.com>
This commit is contained in:
parent
e108bfe13a
commit
fd54f5cc0b
@ -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 */
|
||||
|
||||
@ -27,6 +27,10 @@
|
||||
#include <string.h>
|
||||
#include <pthread.h>
|
||||
#include <unistd.h>
|
||||
#include <poll.h>
|
||||
#include <fcntl.h>
|
||||
#include <sys/time.h>
|
||||
#include <errno.h>
|
||||
|
||||
#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);
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user