From e108bfe13a534f07ffa2fe34cacf3df74ce47ecc Mon Sep 17 00:00:00 2001 From: miketsai Date: Fri, 12 Jun 2026 15:14:17 +0800 Subject: [PATCH] feat(can): add LED control fields to can_ctrl_cmd_t, CAN speed in fire_* callbacks - can_bus.h: add can_ctrl_cmd_t struct (speed/led_gpio/led_enable/keepalive_interval_ms) - can_bus.c: rename s_keepalive_cmd->s_keepalive_speed; add s_keepalive_led_gpio/ led_enable/interval_ms; keepalive thread supports LED blink (toggle when led_enable=1) and dynamic interval; remove #define CAN_KEEPALIVE_INTERVAL_MS - event_recorder: fire_collision_warning sends SPEED_LIMIT_STOP/NORMAL via CAN - event_recorder: fire_alert sends SPEED_LIMIT_ALERT/NORMAL via CAN - event_recorder/app_header_init: update callers to use can_ctrl_cmd_t struct Co-Authored-By: Claude Sonnet 4.6 --- include/host_stream/can_bus.h | 28 ++++++++-- src/host_stream/app_header_init.c | 7 ++- src/host_stream/can_bus.c | 62 ++++++++++++++------- src/host_stream/event_recorder.c | 93 ++++++++++++++++++------------- 4 files changed, 122 insertions(+), 68 deletions(-) diff --git a/include/host_stream/can_bus.h b/include/host_stream/can_bus.h index 8100727..f5c52bb 100644 --- a/include/host_stream/can_bus.h +++ b/include/host_stream/can_bus.h @@ -37,13 +37,29 @@ int can_bus_init(const char *spidev, int can_speed_kbps, uint32_t can_id); void can_bus_send_json(const char *json); /* - * can_bus_send_control_cmd — send one classic-CAN control frame. - * Frame format (compatible with bt_proc.c reference): - * CAN ID : 0x75 (11-bit) - * DLC : 8 - * DATA : [cmd, 0, 0, 0, 0, 0, 0, 0] + * can_ctrl_cmd_t — parameters for can_bus_send_control_cmd(). + * + * CAN ID 0x75 DLC=8 Intel byte order: + * frame[0] = speed (bits 0-7, byte 0: throttle_limit_command) + * frame[6] = (led_gpio & 1) | ((led_enable & 1) << 6) + * bit 48 = led_gpio (byte 6 bit 0): LED GPIO 1=high 0=low + * bit 54 = led_enable (byte 6 bit 6): LED 1=enable 0=disable + * frame[1..5], frame[7] = 0x00 + * + * keepalive_interval_ms: >0 updates the keepalive period; 0 = no change. */ -void can_bus_send_control_cmd(uint8_t cmd); +typedef struct { + uint8_t speed; /* byte 0: throttle_limit_command, 0-255 */ + uint8_t led_gpio; /* bit 48 (byte 6 bit 0): LED GPIO 1=high, 0=low */ + uint8_t led_enable; /* bit 54 (byte 6 bit 6): 1=enable, 0=disable */ + int keepalive_interval_ms; /* >0 updates interval, 0 = no change */ +} can_ctrl_cmd_t; + +/* + * can_bus_send_control_cmd — send one 8-byte CAN control frame immediately + * and update the keepalive state to maintain the new values. + */ +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); diff --git a/src/host_stream/app_header_init.c b/src/host_stream/app_header_init.c index 5fe5507..2a3def0 100644 --- a/src/host_stream/app_header_init.c +++ b/src/host_stream/app_header_init.c @@ -204,7 +204,12 @@ static void print_yolo_result(kp_app_yolo_result_t *yolo_data) uint8_t speed = vehicle_now ? 10 : 240; /* 送 CAN 速度指令 */ - can_bus_send_control_cmd(speed); + { + can_ctrl_cmd_t ctrl = { .speed = speed, .led_gpio = 0, + .led_enable = 0, + .keepalive_interval_ms = 0 }; + can_bus_send_control_cmd(&ctrl); + } /* 透過 BLE 通知目前 CAN bus 狀態 */ { diff --git a/src/host_stream/can_bus.c b/src/host_stream/can_bus.c index 078d187..359b76d 100644 --- a/src/host_stream/can_bus.c +++ b/src/host_stream/can_bus.c @@ -40,12 +40,13 @@ static uint32_t s_ctl_can_id = 0x75; /* SPI mutex (mcp2515 is not thread-safe) */ static pthread_mutex_t s_spi_mtx = PTHREAD_MUTEX_INITIALIZER; -/* Keep-alive: resend s_keepalive_cmd every 200 ms so the motor controller - * never loses the current command (cmd=0 = normal speed, 1/2/3 = violation). */ -#define CAN_KEEPALIVE_INTERVAL_MS 200 - -static volatile uint8_t s_keepalive_cmd = 240; /* SPEED_LEVEL_5: send normal speed from startup */ -static pthread_mutex_t s_keepalive_mtx = PTHREAD_MUTEX_INITIALIZER; +/* Keep-alive: resend control frame periodically so the motor controller + * never loses the current command. Interval and values are runtime-configurable. */ +static volatile uint8_t s_keepalive_speed = 240; /* SPEED_LEVEL_5: normal speed from startup */ +static volatile uint8_t s_keepalive_led_gpio = 0; /* bit 48: LED GPIO state */ +static volatile uint8_t s_keepalive_led_enable = 0; /* bit 54: LED enable */ +static volatile int s_keepalive_interval_ms = 200; /* keepalive period in ms */ +static pthread_mutex_t s_keepalive_mtx = PTHREAD_MUTEX_INITIALIZER; static pthread_t s_keepalive_tid; static volatile int s_keepalive_running = 0; @@ -299,26 +300,40 @@ static void *can_writer_thread(void *arg) return NULL; } -/* Keep-alive thread: sends s_keepalive_cmd every CAN_KEEPALIVE_INTERVAL_MS */ +/* Keep-alive thread: periodically resends the current control frame. */ static void *can_keepalive_thread(void *arg) { (void)arg; while (s_keepalive_running) { - usleep(CAN_KEEPALIVE_INTERVAL_MS * 1000); + usleep(s_keepalive_interval_ms * 1000); if (!s_keepalive_running) break; - uint8_t cmd; + uint8_t speed, led_enable; pthread_mutex_lock(&s_keepalive_mtx); - cmd = s_keepalive_cmd; + speed = s_keepalive_speed; + led_enable = s_keepalive_led_enable; pthread_mutex_unlock(&s_keepalive_mtx); if (!s_dev) continue; + /* LED blink: led_enable==1 → toggle gpio each keepalive; led_enable==0 → fixed 0 */ + uint8_t actual_led_gpio; + if (led_enable) { + static uint8_t s_led_gpio_toggle = 0; + s_led_gpio_toggle ^= 1; + actual_led_gpio = s_led_gpio_toggle; + } else { + actual_led_gpio = 0; + } + uint8_t frame[8] = {0}; - frame[0] = cmd; + frame[0] = speed; + frame[6] = (actual_led_gpio & 0x01) | ((led_enable & 0x01) << 6); + int rc = send_one_frame_with_retry(s_ctl_can_id, frame, 8); if (rc != ERROR_OK) - printf("[CAN-KA] keepalive tx failed (cmd=%u rc=%d)\n", cmd, rc); + printf("[CAN-KA] keepalive tx failed (speed=%u led_gpio=%u led_enable=%u rc=%d)\n", + speed, actual_led_gpio, led_enable, rc); } return NULL; } @@ -436,7 +451,7 @@ int can_bus_init(const char *spidev, int can_speed_kbps, uint32_t can_id) 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); - printf("[CAN] keep-alive thread started: cmd=%d every %d ms\n", s_keepalive_cmd, CAN_KEEPALIVE_INTERVAL_MS); + printf("[CAN] keep-alive thread started: speed=%d every %d ms\n", s_keepalive_speed, s_keepalive_interval_ms); return 0; } @@ -451,28 +466,33 @@ void can_bus_send_json(const char *json) } } -void can_bus_send_control_cmd(uint8_t cmd) +void can_bus_send_control_cmd(const can_ctrl_cmd_t *ctrl) { uint8_t frame[8] = {0}; int rc; - /* Update keep-alive so it maintains this cmd until next change */ + /* Update keepalive state so it maintains these values until next change */ pthread_mutex_lock(&s_keepalive_mtx); - s_keepalive_cmd = cmd; + s_keepalive_speed = ctrl->speed; + s_keepalive_led_gpio = ctrl->led_gpio; + s_keepalive_led_enable = ctrl->led_enable; + if (ctrl->keepalive_interval_ms > 0) + s_keepalive_interval_ms = ctrl->keepalive_interval_ms; pthread_mutex_unlock(&s_keepalive_mtx); if (!s_dev || !s_running) { - printf("[CAN-CTL] drop: bus not ready (cmd=%u)\n", cmd); + printf("[CAN-CTL] drop: bus not ready (speed=%u)\n", ctrl->speed); return; } - frame[0] = cmd; + frame[0] = ctrl->speed; + frame[6] = (ctrl->led_gpio & 0x01) | ((ctrl->led_enable & 0x01) << 6); rc = send_one_frame_with_retry(s_ctl_can_id, frame, 8); if (rc == ERROR_OK) { - printf("[CAN-CTL] tx id=0x%03X dlc=8 data=[0x%02X 00 00 00 00 00 00 00]\n", - s_ctl_can_id, cmd); + printf("[CAN-CTL] tx id=0x%03X dlc=8 data=[0x%02X 00 00 00 00 00 0x%02X 00]\n", + s_ctl_can_id, ctrl->speed, frame[6]); } else { - printf("[CAN-CTL] tx failed (cmd=%u rc=%d)\n", cmd, rc); + printf("[CAN-CTL] tx failed (speed=%u rc=%d)\n", ctrl->speed, rc); } } diff --git a/src/host_stream/event_recorder.c b/src/host_stream/event_recorder.c index d2d5993..266d02d 100644 --- a/src/host_stream/event_recorder.c +++ b/src/host_stream/event_recorder.c @@ -58,7 +58,12 @@ static int msg_send(const char *fifo, /* Speed control has moved to direct CAN control frames. * Avoid MsgBroker FIFO dependency (/tmp/canbus/c0/command.fifo). */ if (cmd && strcmp(cmd, "setSpeed") == 0 && data && data_size >= 1) { - can_bus_send_control_cmd(*(const uint8_t *)data); + { + can_ctrl_cmd_t ctrl = { .speed = *(const uint8_t *)data, + .led_gpio = 0, .led_enable = 0, + .keepalive_interval_ms = 0 }; + can_bus_send_control_cmd(&ctrl); + } return 0; } @@ -109,16 +114,23 @@ static long long s_sd_max_bytes = (long long)7 * 1024 * 1024 * 1024; static int s_upload_delay_ms = 60000; static int s_enabled = 0; +#define SPEED_LIMIT_NORMAL 240//255 /* ���`��p */ +#define SPEED_LIMIT_ALERT 35 /* ��a/ĵ�� (�����a level �Τ@) */ +#define SPEED_LIMIT_STOP 10 /* �I��/��� */ + /* Speed levels sent via MsgBroker → CAN bus process. * To tune: adjust the numbers here only — no logic changes needed. * SPEED_LEVEL_5 (240) is the normal/idle speed; CAN keepalive sends this every 200ms. * Levels 0~4 are all 10 during testing; restore graduated values for production. */ -#define SPEED_LEVEL_0 10 /* collision single-shot — testing: set to 10 */ -#define SPEED_LEVEL_1 10 /* grass L3 (max severity) — testing: set to 10 */ -#define SPEED_LEVEL_2 10 /* grass L2 — testing: set to 10 */ -#define SPEED_LEVEL_3 10 /* grass L1 — testing: set to 10 */ -#define SPEED_LEVEL_4 10 /* grass L1 entry — testing: set to 10 */ -#define SPEED_LEVEL_5 240 /* normal / grass cleared (full speed) */ +#define SPEED_LEVEL_0 10 /* collision single-shot (throttle 1~ 24, flash 100ms) */ +#define SPEED_LEVEL_1 35 /* grass L3 (max severity) (throttle 25~ 49, flash 200ms) */ +#define SPEED_LEVEL_2 60 /* grass L2 (throttle 50~ 74, flash 300ms) */ +#define SPEED_LEVEL_3 100 /* grass L1 (throttle 75~129, flash 400ms) */ +#define SPEED_LEVEL_4 160 /* grass L1 entry (throttle130~199, flash 800ms) */ +#define SPEED_LEVEL_5 240 /* grass L1 entry (throttle200 ~ 249, flash 1600ms) */ +#define SPEED_LEVEL_6 255 /* normal / grass cleared (throttle>=250, no flash) */ + + /* ── VMF SNAP ────────────────────────────────────────────────────────────── */ #define SNAP_BUF_SIZE (2 * 1024 * 1024) @@ -422,6 +434,13 @@ void fire_collision_warning(int level, const char *type) "{\"response_type\":\"collision_warning\"," "\"content\":{\"level\":%d,\"type\":%s}}", level, type_str); + { + uint8_t speed = level ? SPEED_LIMIT_STOP : SPEED_LIMIT_NORMAL; + can_ctrl_cmd_t ctrl = { .speed = speed, .led_gpio = 0, .led_enable = 0, + .keepalive_interval_ms = 0 }; + can_bus_send_control_cmd(&ctrl); + printf("[EVT-CAN] collision_warning speed=%d (level=%d)\n", speed, level); + } bt_uart_send_json(json); printf("[EVT] collision_warning level=%d type=%s\n", level, type ? type : "null"); } @@ -446,6 +465,13 @@ static void fire_alert(int left_level, const char *left_type, "\"left\":{\"level\":%d,\"type\":%s}," "\"right\":{\"level\":%d,\"type\":%s}}}", left_level, l_str, right_level, r_str); + { + uint8_t speed = (left_level || right_level) ? SPEED_LIMIT_ALERT : SPEED_LIMIT_NORMAL; + can_ctrl_cmd_t ctrl = { .speed = speed, .led_gpio = 0, .led_enable = 0, + .keepalive_interval_ms = 0 }; + can_bus_send_control_cmd(&ctrl); + printf("[EVT-CAN] alert speed=%d (left=%d right=%d)\n", speed, left_level, right_level); + } bt_uart_send_json(json); printf("[EVT] alert left=%d(%s) right=%d(%s)\n", left_level, left_type ? left_type : "null", @@ -666,9 +692,8 @@ void event_recorder_update(const stdc_analysis_t *ana) g_grass.state = GRASS_L1; pthread_mutex_unlock(&g_grass_mtx); grass_enter_level(1); - speed_val = SPEED_LEVEL_4; - msg_send("/tmp/canbus/c0/command.fifo", "host_stream", "setSpeed", - &speed_val, sizeof(speed_val), 0); + speed_val = SPEED_LIMIT_STOP; + msg_send("/tmp/canbus/c0/command.fifo", "host_stream", "setSpeed",&speed_val, sizeof(speed_val), 0); return; } break; @@ -681,15 +706,13 @@ void event_recorder_update(const stdc_analysis_t *ana) g_grass.state = GRASS_L2; pthread_mutex_unlock(&g_grass_mtx); grass_enter_level(2); - speed_val = SPEED_LEVEL_3; - msg_send("/tmp/canbus/c0/command.fifo", "host_stream", "setSpeed", - &speed_val, sizeof(speed_val), 0); + speed_val = SPEED_LIMIT_STOP; + msg_send("/tmp/canbus/c0/command.fifo", "host_stream", "setSpeed",&speed_val, sizeof(speed_val), 0); return; } } else { - speed_val = SPEED_LEVEL_5; - msg_send("/tmp/canbus/c0/command.fifo", "host_stream", "setSpeed", - &speed_val, sizeof(speed_val), 0); + speed_val = SPEED_LIMIT_NORMAL; + msg_send("/tmp/canbus/c0/command.fifo", "host_stream", "setSpeed",&speed_val, sizeof(speed_val), 0); if (elapsed_ms_tv(&g_grass.t_last_active) >= GRASS_EXIT_HYSTERESIS_MS) { g_grass.state = GRASS_DONE; gettimeofday(&g_grass.t_done, NULL); @@ -698,8 +721,7 @@ void event_recorder_update(const stdc_analysis_t *ana) fire_json_async(g_grass.event_id, "lane", 0); float dur = (float)elapsed_ms_tv(&g_grass.t_l1) / 1000.0f; const char imgs[4][64] = { "level1.jpg", "", "", "" }; - launch_upload(g_grass.work_dir, g_grass.event_id, "grass", - g_grass.max_level, dur, imgs, 1, s_upload_delay_ms); + launch_upload(g_grass.work_dir, g_grass.event_id, "grass", g_grass.max_level, dur, imgs, 1, s_upload_delay_ms); return; } } @@ -713,15 +735,13 @@ void event_recorder_update(const stdc_analysis_t *ana) g_grass.state = GRASS_L3; pthread_mutex_unlock(&g_grass_mtx); grass_enter_level(3); - speed_val = SPEED_LEVEL_2; - msg_send("/tmp/canbus/c0/command.fifo", "host_stream", "setSpeed", - &speed_val, sizeof(speed_val), 0); + speed_val = SPEED_LIMIT_STOP; + msg_send("/tmp/canbus/c0/command.fifo", "host_stream", "setSpeed",&speed_val, sizeof(speed_val), 0); return; } } else { - speed_val = SPEED_LEVEL_5; - msg_send("/tmp/canbus/c0/command.fifo", "host_stream", "setSpeed", - &speed_val, sizeof(speed_val), 0); + speed_val = SPEED_LIMIT_NORMAL; + msg_send("/tmp/canbus/c0/command.fifo", "host_stream", "setSpeed",&speed_val, sizeof(speed_val), 0); if (elapsed_ms_tv(&g_grass.t_last_active) >= GRASS_EXIT_HYSTERESIS_MS) { g_grass.state = GRASS_DONE; gettimeofday(&g_grass.t_done, NULL); @@ -730,8 +750,7 @@ void event_recorder_update(const stdc_analysis_t *ana) fire_json_async(g_grass.event_id, "lane", 0); float dur = (float)elapsed_ms_tv(&g_grass.t_l1) / 1000.0f; const char imgs[4][64] = { "level1.jpg", "level2.jpg", "", "" }; - launch_upload(g_grass.work_dir, g_grass.event_id, "grass", - g_grass.max_level, dur, imgs, 2, s_upload_delay_ms); + launch_upload(g_grass.work_dir, g_grass.event_id, "grass",g_grass.max_level, dur, imgs, 2, s_upload_delay_ms); return; } } @@ -740,13 +759,11 @@ void event_recorder_update(const stdc_analysis_t *ana) case GRASS_L3: if (on_grass) { gettimeofday(&g_grass.t_last_active, NULL); - speed_val = SPEED_LEVEL_1; - msg_send("/tmp/canbus/c0/command.fifo", "host_stream", "setSpeed", - &speed_val, sizeof(speed_val), 0); + speed_val = SPEED_LIMIT_STOP; + msg_send("/tmp/canbus/c0/command.fifo", "host_stream", "setSpeed",&speed_val, sizeof(speed_val), 0); } else { - speed_val = SPEED_LEVEL_5; - msg_send("/tmp/canbus/c0/command.fifo", "host_stream", "setSpeed", - &speed_val, sizeof(speed_val), 0); + speed_val = SPEED_LIMIT_NORMAL; + msg_send("/tmp/canbus/c0/command.fifo", "host_stream", "setSpeed",&speed_val, sizeof(speed_val), 0); if (elapsed_ms_tv(&g_grass.t_last_active) >= GRASS_EXIT_HYSTERESIS_MS) { g_grass.state = GRASS_DONE; gettimeofday(&g_grass.t_done, NULL); @@ -755,8 +772,7 @@ void event_recorder_update(const stdc_analysis_t *ana) fire_json_async(g_grass.event_id, "lane", 0); float dur = (float)elapsed_ms_tv(&g_grass.t_l1) / 1000.0f; const char imgs[4][64] = { "level1.jpg", "level2.jpg", "level3.jpg", "" }; - launch_upload(g_grass.work_dir, g_grass.event_id, "grass", - g_grass.max_level, dur, imgs, 3, s_upload_delay_ms); + launch_upload(g_grass.work_dir, g_grass.event_id, "grass",g_grass.max_level, dur, imgs, 3, s_upload_delay_ms); return; } } @@ -766,9 +782,8 @@ void event_recorder_update(const stdc_analysis_t *ana) /* Upload thread will reset to IDLE when done. * Keep sending SPEED_LEVEL_5 every frame until upload finishes * (keepalive in can_bus.c also maintains this every 200ms). */ - speed_val = SPEED_LEVEL_5; - msg_send("/tmp/canbus/c0/command.fifo", "host_stream", "setSpeed", - &speed_val, sizeof(speed_val), 0); + speed_val = SPEED_LIMIT_NORMAL; + msg_send("/tmp/canbus/c0/command.fifo", "host_stream", "setSpeed",&speed_val, sizeof(speed_val), 0); break; } @@ -792,12 +807,10 @@ void event_recorder_update(const stdc_analysis_t *ana) int left_now = ana->left_alert; int right_now = ana->right_alert; if (left_now != g_last_left_alert || right_now != g_last_right_alert) { - fire_alert(left_now, left_now ? ana->left_type : NULL, - right_now, right_now ? ana->right_type : NULL); + fire_alert(left_now, left_now ? ana->left_type : NULL,right_now, right_now ? ana->right_type : NULL); g_last_left_alert = left_now; g_last_right_alert = right_now; } - g_any_col_last = col_now; #endif #if 0