diff --git a/include/host_stream/can_bus.h b/include/host_stream/can_bus.h index eb7e2fa..84636f1 100644 --- a/include/host_stream/can_bus.h +++ b/include/host_stream/can_bus.h @@ -39,20 +39,20 @@ void can_bus_send_json(const char *json); /* * 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 + * CAN ID 0x75 DLC=8 little-endian: + * bytes 0-1 : throttle_limit_command (uint16_t, little-endian) + * bytes 2-5 : 0x00 + * byte 6 : bit 0 = led_enable (1=on, 0=off) + * byte 7 : bit 0 = led_r (LED right, 1=on), bit 7 = led_l (LED left, 1=on) * * keepalive_interval_ms: >0 updates the keepalive period; 0 = no change. */ 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 */ + uint16_t speed; /* bytes 0-1: throttle_limit_command, little-endian */ + uint8_t led_enable; /* bit 48 (byte 6 bit 0): LED enable 1=on, 0=off */ + uint8_t led_r; /* bit 56 (byte 7 bit 0): LED right 1=on, 0=off */ + uint8_t led_l; /* bit 63 (byte 7 bit 7): LED left 1=on, 0=off */ + int keepalive_interval_ms; /* >0 updates keepalive period; 0 = no change */ } can_ctrl_cmd_t; /* diff --git a/src/host_stream/app_header_init.c b/src/host_stream/app_header_init.c index 23e3131..46fb8a4 100644 --- a/src/host_stream/app_header_init.c +++ b/src/host_stream/app_header_init.c @@ -206,8 +206,8 @@ static void print_yolo_result(kp_app_yolo_result_t *yolo_data) /* 送 CAN 速度指令 */ { - can_ctrl_cmd_t ctrl = { .speed = speed, .led_gpio = 0, - .led_enable = 0, + can_ctrl_cmd_t ctrl = { .speed = speed, .led_enable = 0, + .led_r = 0, .led_l = 0, .keepalive_interval_ms = 0 }; can_bus_send_control_cmd(&ctrl); } diff --git a/src/host_stream/can_bus.c b/src/host_stream/can_bus.c index 916a71f..dffcdb2 100644 --- a/src/host_stream/can_bus.c +++ b/src/host_stream/can_bus.c @@ -50,9 +50,10 @@ static pthread_mutex_t s_spi_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 uint16_t s_keepalive_speed = 240; /* SPEED_LEVEL_5: normal speed from startup */ +static volatile uint8_t s_keepalive_led_enable = 0; /* bit 48 (byte 6 bit 0): LED enable */ +static volatile uint8_t s_keepalive_led_r = 0; /* bit 56 (byte 7 bit 0): LED right */ +static volatile uint8_t s_keepalive_led_l = 0; /* bit 63 (byte 7 bit 7): LED left */ 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; @@ -482,32 +483,27 @@ static void *can_keepalive_thread(void *arg) usleep(s_keepalive_interval_ms * 1000); if (!s_keepalive_running) break; - uint8_t speed, led_enable; + uint16_t speed; + uint8_t led_enable, led_r, led_l; pthread_mutex_lock(&s_keepalive_mtx); speed = s_keepalive_speed; led_enable = s_keepalive_led_enable; + led_r = s_keepalive_led_r; + led_l = s_keepalive_led_l; 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] = speed; - frame[6] = (actual_led_gpio & 0x01) | ((led_enable & 0x01) << 6); + frame[0] = (uint8_t)(speed & 0xFF); + frame[1] = (uint8_t)((speed >> 8) & 0xFF); + frame[6] = (led_enable & 1); + frame[7] = (led_r & 1) | ((led_l & 1) << 7); int rc = send_one_frame_with_retry(s_ctl_can_id, frame, 8); if (rc != ERROR_OK) - printf("[CAN-KA] keepalive tx failed (speed=%u led_gpio=%u led_enable=%u rc=%d)\n", - speed, actual_led_gpio, led_enable, rc); + printf("[CAN-KA] keepalive tx failed (speed=%u led_enable=%u led_r=%u led_l=%u rc=%d)\n", + speed, led_enable, led_r, led_l, rc); } return NULL; } @@ -658,8 +654,9 @@ void can_bus_send_control_cmd(const can_ctrl_cmd_t *ctrl) /* Update keepalive state so it maintains these values until next change */ pthread_mutex_lock(&s_keepalive_mtx); s_keepalive_speed = ctrl->speed; - s_keepalive_led_gpio = ctrl->led_gpio; s_keepalive_led_enable = ctrl->led_enable; + s_keepalive_led_r = ctrl->led_r; + s_keepalive_led_l = ctrl->led_l; if (ctrl->keepalive_interval_ms > 0) s_keepalive_interval_ms = ctrl->keepalive_interval_ms; pthread_mutex_unlock(&s_keepalive_mtx); @@ -669,12 +666,14 @@ void can_bus_send_control_cmd(const can_ctrl_cmd_t *ctrl) return; } - frame[0] = ctrl->speed; - frame[6] = (ctrl->led_gpio & 0x01) | ((ctrl->led_enable & 0x01) << 6); + frame[0] = (uint8_t)(ctrl->speed & 0xFF); + frame[1] = (uint8_t)((ctrl->speed >> 8) & 0xFF); + frame[6] = (ctrl->led_enable & 1); + frame[7] = (ctrl->led_r & 1) | ((ctrl->led_l & 1) << 7); 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 0x%02X 00]\n", - s_ctl_can_id, ctrl->speed, frame[6]); + printf("[CAN-CTL] tx id=0x%03X dlc=8 data=[0x%02X 0x%02X 00 00 00 00 0x%02X 0x%02X]\n", + s_ctl_can_id, frame[0], frame[1], frame[6], frame[7]); } else { 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 2a39230..7f9aeb9 100644 --- a/src/host_stream/event_recorder.c +++ b/src/host_stream/event_recorder.c @@ -62,7 +62,7 @@ static int msg_send(const char *fifo, if (cmd && strcmp(cmd, "setSpeed") == 0 && data && data_size >= 1) { { can_ctrl_cmd_t ctrl = { .speed = *(const uint8_t *)data, - .led_gpio = 0, .led_enable = 0, + .led_enable = 0, .led_r = 0, .led_l = 0, .keepalive_interval_ms = 0 }; can_bus_send_control_cmd(&ctrl); } @@ -452,7 +452,7 @@ static void test_mode_exit_internal(const char *reason) g_test_mode = 0; s_test_mode_entered = 0; can_ctrl_cmd_t ctrl = { .speed = SPEED_LIMIT_NORMAL, - .led_gpio = 0, .led_enable = 0, + .led_enable = 0, .led_r = 0, .led_l = 0, .keepalive_interval_ms = 0 }; can_bus_send_control_cmd(&ctrl); buzzer_set_pattern(BUZZER_PATTERN_OFF); @@ -516,10 +516,10 @@ static void test_ble_alert(int ll, const char *lt, int rl, const char *rt) test_reply(ack); } -static void test_can_event(const char *event, int level, uint8_t speed) +static void test_can_event(const char *event, int level, uint16_t speed) { - can_ctrl_cmd_t ctrl = { .speed = speed, .led_gpio = 0, - .led_enable = 0, .keepalive_interval_ms = 0 }; + can_ctrl_cmd_t ctrl = { .speed = speed, .led_enable = 0, + .led_r = 0, .led_l = 0, .keepalive_interval_ms = 0 }; can_bus_send_control_cmd(&ctrl); char ack[64]; snprintf(ack, sizeof(ack), "TEST_ACK CAN %s %d speed=%u", event, level, (unsigned)speed); @@ -727,8 +727,8 @@ void fire_collision_warning(int level, const char *type) 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_ctrl_cmd_t ctrl = { .speed = speed, .led_enable = 0, + .led_r = 0, .led_l = 0, .keepalive_interval_ms = 0 }; can_bus_send_control_cmd(&ctrl); printf("[EVT-CAN] collision_warning speed=%d (level=%d)\n", speed, level); buzzer_set_pattern(level ? BUZZER_PATTERN_COLLISION : BUZZER_PATTERN_OFF); @@ -759,8 +759,8 @@ static void fire_alert(int left_level, const char *left_type, 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_ctrl_cmd_t ctrl = { .speed = speed, .led_enable = 0, + .led_r = 0, .led_l = 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); buzzer_set_pattern((left_level || right_level) ? BUZZER_PATTERN_ALERT : BUZZER_PATTERN_OFF);