miketsai/gf_ai_box
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(can): add LED control fields to can_ctrl_cmd_t, CAN speed in fire_* callbacks

Browse Source 
- 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 <noreply@anthropic.com>
This commit is contained in:
miketsai 2026-06-12 15:14:17 +08:00
parent 118d0f4d93
commit e108bfe13a
4 changed files with 122 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
include/host_stream/can_bus.h
View File

@ -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);

7
src/host_stream/app_header_init.c
View File

@ -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 狀態 */
{

62
src/host_stream/can_bus.c
View File

@ -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);
}
}

93
src/host_stream/event_recorder.c
View File

@ -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 /* <20><><EFBFBD>`<60><>p */
#define SPEED_LIMIT_ALERT 35 /* <20><>a/ĵ<><C4B5> (<28><><EFBFBD><EFBFBD><EFBFBD>a level <20>Τ@) */
#define SPEED_LIMIT_STOP 10 /* <20>I<EFBFBD><49>/<2F><><EFBFBD> */
/* 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