miketsai
c22b81dfba
- can_ctrl_cmd_t.speed: uint8_t -> uint16_t (bytes 0-1, little-endian) - Remove led_gpio field; add led_r (byte7 bit0) and led_l (byte7 bit7) - byte6 now carries only led_enable (bit0), not the old gpio/enable packed format - Update keepalive thread and can_bus_send_control_cmd() frame assembly accordingly - Update all 6 can_ctrl_cmd_t call sites in event_recorder.c and app_header_init.c - test_can_event() parameter type: uint8_t speed -> uint16_t speed
667 lines
28 KiB
C
667 lines
28 KiB
C
#include <stdio.h>
|
||
#include <stdlib.h>
|
||
#include <string.h>
|
||
#include <unistd.h>
|
||
#include <stdint.h>
|
||
#include <sys/time.h>
|
||
|
||
#include <MemBroker/mem_broker.h>
|
||
#include "kp_struct.h"
|
||
#include "model_type.h"
|
||
#include "kdp2_inf_app_yolo.h"
|
||
//#include "demo_customize_inf_single_model.h"
|
||
#include "stdc_app.h"
|
||
#include "kdp2_host_stream.h"
|
||
#include "event_recorder.h"
|
||
#include "can_bus.h"
|
||
#include "bt_uart.h"
|
||
|
||
//#define ENABLE_DBG_LOG
|
||
|
||
#ifdef ENABLE_DBG_LOG
|
||
#define dbg_log(__format__, ...) printf(""__format__, ##__VA_ARGS__)
|
||
#else
|
||
#define dbg_log(__format__, ...)
|
||
#endif
|
||
|
||
#define FDFR_MODEL_SIZE_ROW 90
|
||
#define FDFR_MODEL_SIZE_COL 90
|
||
#define MAX_RESULT_BOX 80
|
||
|
||
#define STDC_ROI_SEGMENTS 12
|
||
#define STDC_ROI_EDGE_THICKNESS 2
|
||
|
||
static bool _init_config_yolo_params = false;
|
||
extern unsigned int g_dwDrawBoxEnable;
|
||
extern unsigned int g_dwDrawBoxType;
|
||
extern unsigned int g_verbose_log;
|
||
|
||
/* ── In-place log trimmer ──────────────────────────────────────────────────────
|
||
* Keeps /tmp/fw.log under 2 MB by overwriting in-place via STDOUT_FILENO.
|
||
* Works only when stdout is redirected to a file (nohup ... > /tmp/fw.log).
|
||
*/
|
||
#define LOG_MAX_BYTES (2u * 1024u * 1024u)
|
||
#define LOG_KEEP_BYTES (1u * 1024u * 1024u)
|
||
|
||
static void log_trim_if_needed(void)
|
||
{
|
||
off_t sz = lseek(STDOUT_FILENO, 0, SEEK_END);
|
||
if (sz < (off_t)LOG_MAX_BYTES) return;
|
||
|
||
uint8_t *buf = (uint8_t *)malloc(LOG_KEEP_BYTES);
|
||
if (!buf) return;
|
||
fflush(stdout);
|
||
off_t start = sz - (off_t)LOG_KEEP_BYTES;
|
||
ssize_t got = pread(STDOUT_FILENO, buf, LOG_KEEP_BYTES, start);
|
||
if (got > 0) {
|
||
lseek(STDOUT_FILENO, 0, SEEK_SET);
|
||
write(STDOUT_FILENO, buf, (size_t)got);
|
||
ftruncate(STDOUT_FILENO, (off_t)got);
|
||
lseek(STDOUT_FILENO, 0, SEEK_END);
|
||
printf("[LOG] trimmed to %u KB\n", LOG_KEEP_BYTES / 1024);
|
||
fflush(stdout);
|
||
}
|
||
free(buf);
|
||
}
|
||
|
||
static kp_app_yolo_post_proc_config_t post_proc_params_v5s = {
|
||
.prob_thresh = 0.15,
|
||
.nms_thresh = 0.5,
|
||
.max_detection_per_class = 100,
|
||
.anchor_row = 3,
|
||
.anchor_col = 6,
|
||
.stride_size = 3,
|
||
.reserved_size = 0,
|
||
.data = {
|
||
// anchors[3][6]
|
||
10, 13, 16, 30, 33, 23,
|
||
30, 61, 62, 45, 59, 119,
|
||
116, 90, 156, 198, 373, 326,
|
||
// strides[3]
|
||
8, 16, 32,
|
||
},
|
||
};
|
||
|
||
static kp_app_yolo_post_proc_config_t post_proc_params_v5_vd = {
|
||
.prob_thresh = 0.15,
|
||
.nms_thresh = 0.5,
|
||
.max_detection_per_class = 100,
|
||
.anchor_row = 3,
|
||
.anchor_col = 6,
|
||
.stride_size = 3,
|
||
.reserved_size = 0,
|
||
.data = {
|
||
// anchors[3][6]
|
||
7, 8, 16, 12, 11, 26,
|
||
27, 20, 48, 36, 28, 69,
|
||
95, 62, 70, 172, 184, 242,
|
||
// strides[3]
|
||
8, 16, 32,
|
||
},
|
||
};
|
||
|
||
static void print_yolo_result(kp_app_yolo_result_t *yolo_data)
|
||
{
|
||
unsigned int i = 0;
|
||
static unsigned int dwResultCounts = 0;
|
||
|
||
if (dwResultCounts > MAX_RESULT_BOX)
|
||
dwResultCounts = MAX_RESULT_BOX;
|
||
#if 0
|
||
if (g_dwDrawBoxEnable) {
|
||
for (i = 0 ; i < dwResultCounts; i++) {
|
||
if (g_atDrawInfo[i].bDrawFlag && !g_dwDrawBoxType)
|
||
setup_isp_privacy_mask(0, g_atDrawInfo[i].dwStartX, g_atDrawInfo[i].dwStartY, g_atDrawInfo[i].dwWidth, g_atDrawInfo[i].dwHeight);
|
||
}
|
||
memset(&g_atDrawInfo, 0, sizeof(g_atDrawInfo));
|
||
dwResultCounts = yolo_data->box_count;
|
||
|
||
g_dwResultCounts = 0;
|
||
for (i = 0; i < yolo_data->box_count; i++) {
|
||
if (g_dwOnlyPerson) {
|
||
if (yolo_data->boxes[i].class_num != 0 && yolo_data->boxes[i].class_num != 2)
|
||
continue;
|
||
}
|
||
calculate_bbox_postion(&g_atDrawInfo[i], yolo_data->boxes[i].x1, yolo_data->boxes[i].y1,
|
||
yolo_data->boxes[i].x2, yolo_data->boxes[i].y2, yolo_data->boxes[i].score, yolo_data->boxes[i].class_num);
|
||
g_atDrawInfo[i].bDrawFlag = true;
|
||
if(!g_dwDrawBoxType) {
|
||
setup_isp_privacy_mask(1, g_atDrawInfo[i].dwStartX, g_atDrawInfo[i].dwStartY, g_atDrawInfo[i].dwWidth, g_atDrawInfo[i].dwHeight);
|
||
}
|
||
g_dwResultCounts++;
|
||
}
|
||
if(g_dwResultCounts > MAX_RESULT_BOX) { g_dwResultCounts = MAX_RESULT_BOX; }
|
||
}
|
||
|
||
for (i = 0; i < yolo_data->box_count; i++) {
|
||
if (g_dwOnlyPerson) {
|
||
if (yolo_data->boxes[i].class_num != 0 && yolo_data->boxes[i].class_num != 2)
|
||
continue;
|
||
}
|
||
|
||
printf("[%s][%d] [AI coordinate] Count = %d x1 = %f y1 = %f x2 = %f y2 = %f score = %f class_num = %d\n", __func__, i,
|
||
yolo_data->box_count, yolo_data->boxes[i].x1, yolo_data->boxes[i].y1,
|
||
yolo_data->boxes[i].x2, yolo_data->boxes[i].y2, yolo_data->boxes[i].score, yolo_data->boxes[i].class_num);
|
||
}
|
||
#else
|
||
if (g_test_mode) return;
|
||
/* ── [YOLO] collision_warning（2.2.5）─────────────────────────────
|
||
* 偵測到 vehicle(class=2) 時送 BLE notify。
|
||
* 狀態有變化（進入/離開）才送，debounce 2 秒，避免頻繁發送。
|
||
* 將 #else 改為 #if 1 可啟用原有僅列印的邏輯。
|
||
* ---------------------------------------------------------------- */
|
||
{
|
||
static int s_cw_level = 0;
|
||
static char s_cw_type[16] = "";
|
||
static struct timeval s_cw_last_sent = {0, 0};
|
||
|
||
int cur_level = 0;
|
||
char cur_type[16] = "";
|
||
|
||
if (g_dwDrawBoxEnable) {
|
||
for (i = 0 ; i < dwResultCounts; i++) {
|
||
if (g_atDrawInfo[i].bDrawFlag && !g_dwDrawBoxType)
|
||
setup_isp_privacy_mask(0, g_atDrawInfo[i].dwStartX, g_atDrawInfo[i].dwStartY, g_atDrawInfo[i].dwWidth, g_atDrawInfo[i].dwHeight);
|
||
}
|
||
memset(&g_atDrawInfo, 0, sizeof(g_atDrawInfo));
|
||
dwResultCounts = yolo_data->box_count;
|
||
|
||
g_dwResultCounts = 0;
|
||
for (i = 0; i < yolo_data->box_count; i++) {
|
||
if (g_dwOnlyPerson) {
|
||
if (yolo_data->boxes[i].class_num != 0 && yolo_data->boxes[i].class_num != 2)
|
||
continue;
|
||
}
|
||
calculate_bbox_postion(&g_atDrawInfo[i], yolo_data->boxes[i].x1, yolo_data->boxes[i].y1,
|
||
yolo_data->boxes[i].x2, yolo_data->boxes[i].y2, yolo_data->boxes[i].score, yolo_data->boxes[i].class_num);
|
||
g_atDrawInfo[i].bDrawFlag = true;
|
||
if(!g_dwDrawBoxType) {
|
||
setup_isp_privacy_mask(1, g_atDrawInfo[i].dwStartX, g_atDrawInfo[i].dwStartY, g_atDrawInfo[i].dwWidth, g_atDrawInfo[i].dwHeight);
|
||
}
|
||
g_dwResultCounts++;
|
||
}
|
||
if(g_dwResultCounts > MAX_RESULT_BOX) { g_dwResultCounts = MAX_RESULT_BOX; }
|
||
}
|
||
|
||
for (i = 0; i < yolo_data->box_count; i++) {
|
||
int cls = (int)yolo_data->boxes[i].class_num;
|
||
if (cls != 2) continue; /* 只看 vehicle(2) */
|
||
|
||
cur_level = 1;
|
||
snprintf(cur_type, sizeof(cur_type), "vehicle");
|
||
break; /* 找到一輛車就夠了 */
|
||
}
|
||
|
||
/* ── [CAN] vehicle speed control + BLE status notify ────────────
|
||
* 偵測到 vehicle(class=2)：限速 10，並透過 BLE 通知目前 CAN 狀態。
|
||
* 無 vehicle：恢復正常速度 240，並通知解除。
|
||
* 只在狀態改變時呼叫，keepalive thread 會持續維持最新速度。
|
||
* ---------------------------------------------------------------- */
|
||
{
|
||
static int s_can_vehicle_detected = -1; /* -1 = 未初始化 */
|
||
int vehicle_now = (cur_level == 1) ? 1 : 0;
|
||
|
||
if (vehicle_now != s_can_vehicle_detected) {
|
||
uint8_t speed = vehicle_now ? 10 : 240;
|
||
|
||
/* 送 CAN 速度指令 */
|
||
{
|
||
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);
|
||
}
|
||
|
||
/* 透過 BLE 通知目前 CAN bus 狀態 */
|
||
{
|
||
char json[128];
|
||
snprintf(json, sizeof(json),
|
||
"{\"response_type\":\"can_status\","
|
||
"\"content\":{"
|
||
"\"vehicle_detected\":%d,"
|
||
"\"speed_cmd\":%u}}",
|
||
vehicle_now, (unsigned)speed);
|
||
bt_uart_send_json(json);
|
||
}
|
||
|
||
printf("[YOLO] CAN speed cmd: %u (%s)\n",
|
||
speed, vehicle_now ? "vehicle detected" : "clear");
|
||
s_can_vehicle_detected = vehicle_now;
|
||
}
|
||
}
|
||
|
||
/* 狀態有變化時，檢查 debounce */
|
||
if (cur_level != s_cw_level ||
|
||
strcmp(cur_type, s_cw_type) != 0)
|
||
{
|
||
struct timeval now;
|
||
gettimeofday(&now, NULL);
|
||
|
||
long elapsed_ms = (now.tv_sec - s_cw_last_sent.tv_sec) * 1000
|
||
+ (now.tv_usec - s_cw_last_sent.tv_usec) / 1000;
|
||
|
||
if (s_cw_last_sent.tv_sec == 0 || elapsed_ms >= 2000) {
|
||
fire_collision_warning(cur_level, cur_level ? cur_type : NULL);
|
||
s_cw_level = cur_level;
|
||
snprintf(s_cw_type, sizeof(s_cw_type), "%s", cur_type);
|
||
s_cw_last_sent = now;
|
||
printf("[YOLO] collision_warning sent level=%d type=%s\n",
|
||
cur_level, cur_level ? cur_type : "null");
|
||
} else {
|
||
printf("[YOLO] collision_warning debounce bypass (%ldms < 5000ms)\n",
|
||
elapsed_ms);
|
||
}
|
||
}
|
||
}
|
||
#endif
|
||
}
|
||
|
||
static unsigned int stdc_u32_lerp(unsigned int a, unsigned int b, unsigned int i, unsigned int n)
|
||
{
|
||
int64_t diff;
|
||
int64_t v;
|
||
|
||
if (n == 0)
|
||
return a;
|
||
|
||
diff = (int64_t)b - (int64_t)a;
|
||
v = (int64_t)a + (diff * (int64_t)i) / (int64_t)n;
|
||
if (v < 0)
|
||
return 0;
|
||
return (unsigned int)v;
|
||
}
|
||
|
||
static void stdc_add_rect(DETECT_INFO *di, unsigned int *cnt,
|
||
unsigned int x, unsigned int y,
|
||
unsigned int w, unsigned int h)
|
||
{
|
||
if (*cnt >= MAX_RESULT_BOX || w == 0 || h == 0)
|
||
return;
|
||
|
||
di[*cnt].dwStartX = x;
|
||
di[*cnt].dwStartY = y;
|
||
di[*cnt].dwWidth = w;
|
||
di[*cnt].dwHeight = h;
|
||
di[*cnt].fScore = 0;
|
||
di[*cnt].dwClass = 0;
|
||
di[*cnt].bDrawFlag = true;
|
||
(*cnt)++;
|
||
}
|
||
|
||
static void stdc_add_trapezoid_outline(DETECT_INFO *di, unsigned int *cnt,
|
||
unsigned int x_tl, unsigned int x_tr,
|
||
unsigned int x_bl, unsigned int x_br,
|
||
unsigned int y_top, unsigned int y_bottom)
|
||
{
|
||
/* Top and bottom edges */
|
||
if (x_tr > x_tl) {
|
||
stdc_add_rect(di, cnt, x_tl, y_top, x_tr - x_tl, STDC_ROI_EDGE_THICKNESS);
|
||
}
|
||
if (x_br > x_bl) {
|
||
stdc_add_rect(di, cnt, x_bl, y_bottom, x_br - x_bl, STDC_ROI_EDGE_THICKNESS);
|
||
}
|
||
|
||
/* Left and right slanted edges as sampled points */
|
||
for (unsigned int i = 0; i <= STDC_ROI_SEGMENTS; i++) {
|
||
unsigned int y = stdc_u32_lerp(y_top, y_bottom, i, STDC_ROI_SEGMENTS);
|
||
unsigned int xl = stdc_u32_lerp(x_tl, x_bl, i, STDC_ROI_SEGMENTS);
|
||
unsigned int xr = stdc_u32_lerp(x_tr, x_br, i, STDC_ROI_SEGMENTS);
|
||
stdc_add_rect(di, cnt, xl, y, STDC_ROI_EDGE_THICKNESS, STDC_ROI_EDGE_THICKNESS);
|
||
stdc_add_rect(di, cnt, xr, y, STDC_ROI_EDGE_THICKNESS, STDC_ROI_EDGE_THICKNESS);
|
||
}
|
||
}
|
||
|
||
/****************************************************************
|
||
* application header/result callback function (Please override the callback functions for other application verify)
|
||
****************************************************************/
|
||
int app_header_send_inference(uint32_t buf_addr, bool *bl_run_next_inference, void* arg, unsigned char* image_buffer, VMF_VSRC_SSM_OUTPUT_INFO_T* vsrc_ssm_info)
|
||
{
|
||
HOST_STREAM_INIT_OPT_T* pInitOpt=(HOST_STREAM_INIT_OPT_T*)arg;
|
||
unsigned int dwWidth = pInitOpt->tVEncoder[pInitOpt->dwInferenceStream].dwWidth;
|
||
unsigned int dwHeight = pInitOpt->tVEncoder[pInitOpt->dwInferenceStream].dwHeight;
|
||
int ret = 0;
|
||
|
||
kp_image_format_t kp_image_format = KP_IMAGE_FORMAT_YUV420; //The default image format of host_stream/hico_mipi is KP_IMAGE_FORMAT_YUV420.
|
||
|
||
if(pInitOpt->dwJobId == KDP2_INF_ID_APP_YOLO)
|
||
{
|
||
if (false == _init_config_yolo_params) {
|
||
/****************************************************************
|
||
* configure application pre/post-processing params
|
||
****************************************************************/
|
||
_init_config_yolo_params = true;
|
||
|
||
kdp2_ipc_app_yolo_post_proc_config_t *app_yolo_post_proc_config = (kdp2_ipc_app_yolo_post_proc_config_t *)buf_addr;
|
||
kp_inference_header_stamp_t *header_stamp = &app_yolo_post_proc_config->header_stamp;
|
||
|
||
header_stamp->magic_type = KDP2_MAGIC_TYPE_INFERENCE;
|
||
header_stamp->total_size = sizeof(kdp2_ipc_app_yolo_post_proc_config_t);
|
||
header_stamp->total_image = 1;
|
||
header_stamp->image_index = 1;
|
||
header_stamp->job_id = KDP2_JOB_ID_APP_YOLO_CONFIG_POST_PROC;
|
||
|
||
app_yolo_post_proc_config->model_id = pInitOpt->dwModelId;
|
||
app_yolo_post_proc_config->param_size = sizeof(kp_app_yolo_post_proc_config_t);
|
||
app_yolo_post_proc_config->set_or_get = 1;
|
||
|
||
|
||
switch (pInitOpt->dwModelId)
|
||
{
|
||
case KNERON_YOLOV5S_COCO80_640_640_3:
|
||
case KNERON_YOLOV5S_PersonBottleChairPottedplant4_288_640_3:
|
||
case KNERON_YOLOV5S_PersonBicycleCarMotorcycleBusTruckCatDog8_256_480_3:
|
||
post_proc_params_v5s.prob_thresh = pInitOpt->fThreshold;
|
||
memcpy((void *)app_yolo_post_proc_config->param_data, (void *)&post_proc_params_v5s, sizeof(kp_app_yolo_post_proc_config_t));
|
||
break;
|
||
case KNERON_YOLOV5S0375_PersonBicycleCarMotorcycleBusTruck6_256_352_3:
|
||
post_proc_params_v5_vd.prob_thresh = pInitOpt->fThreshold;
|
||
memcpy((void *)app_yolo_post_proc_config->param_data, (void *)&post_proc_params_v5_vd, sizeof(kp_app_yolo_post_proc_config_t));
|
||
break;
|
||
default:
|
||
// cannot find matched post-proc config
|
||
printf("%s, model_id = %d, cannot find matched post-proc config\n", __func__, pInitOpt->dwModelId);
|
||
break;
|
||
}
|
||
|
||
return KP_SUCCESS;
|
||
}
|
||
|
||
kdp2_ipc_app_yolo_inf_header_t *app_yolo_header = (kdp2_ipc_app_yolo_inf_header_t *)buf_addr;
|
||
kp_inference_header_stamp_t *header_stamp = &app_yolo_header->header_stamp;
|
||
static int inf_number = 0;
|
||
inf_number = inf_number+1;
|
||
inf_number &= 65535;
|
||
|
||
header_stamp->magic_type = KDP2_MAGIC_TYPE_INFERENCE;
|
||
header_stamp->total_size = sizeof(kdp2_ipc_app_yolo_inf_header_t) + (uint32_t)(vsrc_ssm_info->dwWidth*vsrc_ssm_info->dwHeight*1.5);
|
||
header_stamp->total_image = 1;
|
||
header_stamp->image_index = 1;
|
||
header_stamp->job_id = KDP2_INF_ID_APP_YOLO;
|
||
|
||
app_yolo_header->inf_number = inf_number;
|
||
app_yolo_header->model_id = pInitOpt->dwModelId;
|
||
app_yolo_header->width = dwWidth;
|
||
app_yolo_header->height = dwHeight;
|
||
|
||
app_yolo_header->image_format = kp_image_format;
|
||
app_yolo_header->model_normalize = KP_NORMALIZE_KNERON;//KP_NORMALIZE_DISABLE;
|
||
//printf("[SEND] inf_number = %d \n", inf_number);
|
||
//memcpy((void *)(buf_addr + sizeof(kdp2_ipc_app_yolo_inf_header_t)), image_buffer, image_buffer_size);
|
||
ret = dma2d_copy(pInitOpt->ptDmaInfo->ptDmaHandle, pInitOpt->ptDmaInfo->ptDmaDesc, (void *)(buf_addr + sizeof(kdp2_ipc_app_yolo_inf_header_t)), (void*)image_buffer, vsrc_ssm_info);
|
||
if(ret){
|
||
printf("dma2d_copy failed\n");
|
||
}
|
||
|
||
//_inference_number++;
|
||
|
||
/****************************************************************
|
||
* finish application - close send thead
|
||
****************************************************************/
|
||
//if (_loop_time <= _inference_number)
|
||
// *bl_run_next_inference = false;
|
||
}
|
||
/*
|
||
else if(pInitOpt->dwJobId == DEMO_KL630_CUSTOMIZE_INF_SINGLE_MODEL_JOB_ID)
|
||
{
|
||
demo_customize_inf_single_model_header_t *pAppSingle = (demo_customize_inf_single_model_header_t *)buf_addr;
|
||
kp_inference_header_stamp_t *header_stamp = &pAppSingle->header_stamp;
|
||
|
||
header_stamp->magic_type = KDP2_MAGIC_TYPE_INFERENCE;
|
||
header_stamp->total_size = sizeof(demo_customize_inf_single_model_header_t) + (uint32_t)(vsrc_ssm_info->dwWidth*vsrc_ssm_info->dwHeight*1.5);
|
||
header_stamp->total_image = 1;
|
||
header_stamp->image_index = 0;
|
||
header_stamp->job_id = DEMO_KL630_CUSTOMIZE_INF_SINGLE_MODEL_JOB_ID;
|
||
|
||
pAppSingle->width = dwWidth;
|
||
pAppSingle->height = dwHeight;
|
||
|
||
//memcpy((void *)(buf_addr + sizeof(demo_customize_inf_single_model_header_t)), image_buffer, image_buffer_size);
|
||
ret = dma2d_copy(pInitOpt->ptDmaInfo->ptDmaHandle, pInitOpt->ptDmaInfo->ptDmaDesc, (void *)(buf_addr + sizeof(demo_customize_inf_single_model_header_t)), (void*)image_buffer, vsrc_ssm_info);
|
||
if(ret){
|
||
printf("dma2d_copy failed\n");
|
||
}
|
||
}
|
||
*/
|
||
else if(pInitOpt->dwJobId == STDC_JOB_ID)
|
||
{
|
||
stdc_inf_header_t *pStdcHdr = (stdc_inf_header_t *)buf_addr;
|
||
kp_inference_header_stamp_t *header_stamp = &pStdcHdr->header_stamp;
|
||
|
||
header_stamp->magic_type = KDP2_MAGIC_TYPE_INFERENCE;
|
||
header_stamp->total_size = sizeof(stdc_inf_header_t) + (uint32_t)(vsrc_ssm_info->dwWidth*vsrc_ssm_info->dwHeight*1.5);
|
||
header_stamp->total_image = 1;
|
||
header_stamp->image_index = 0;
|
||
header_stamp->job_id = STDC_JOB_ID;
|
||
|
||
pStdcHdr->width = dwWidth;
|
||
pStdcHdr->height = dwHeight;
|
||
pStdcHdr->model_id = pInitOpt->dwModelId;
|
||
pStdcHdr->fps = pInitOpt->fFps;
|
||
|
||
static int stdc_send_count = 0;
|
||
if (++stdc_send_count % 30 == 1)
|
||
printf("[STDC SEND] frame=%d %dx%d\n", stdc_send_count, dwWidth, dwHeight);
|
||
|
||
ret = dma2d_copy(pInitOpt->ptDmaInfo->ptDmaHandle, pInitOpt->ptDmaInfo->ptDmaDesc, (void *)(buf_addr + sizeof(stdc_inf_header_t)), (void*)image_buffer, vsrc_ssm_info);
|
||
if(ret){
|
||
printf("dma2d_copy failed\n");
|
||
}
|
||
}
|
||
else
|
||
{
|
||
printf("[%s] Error: Job ID %u\n", __FUNCTION__, pInitOpt->dwJobId);
|
||
return KP_FW_ERROR_UNKNOWN_APP;
|
||
}
|
||
|
||
/* Take VMF_SNAP if event_recorder has a pending request */
|
||
event_recorder_provide_frame();
|
||
|
||
return KP_SUCCESS;
|
||
}
|
||
|
||
int app_header_recv_inference(uint32_t buf_addr, bool *bl_run_next_inference)
|
||
{
|
||
kp_inference_header_stamp_t *header_stamp = (kp_inference_header_stamp_t *)buf_addr;
|
||
|
||
if (KDP2_INF_ID_APP_YOLO == header_stamp->job_id) {
|
||
/****************************************************************
|
||
* receive & dump application result
|
||
****************************************************************/
|
||
if (KP_SUCCESS != header_stamp->status_code) {
|
||
printf("[%s] Error: Run application fail %u\n", __func__, header_stamp->status_code);
|
||
return header_stamp->status_code;
|
||
}
|
||
|
||
kdp2_ipc_app_yolo_result_t *app_yolo_result = (kdp2_ipc_app_yolo_result_t *)header_stamp;
|
||
kp_app_yolo_result_t *yolo_data = &app_yolo_result->yolo_data;
|
||
//printf("[RECV] inf_number = %d \n", app_yolo_result->inf_number);
|
||
print_yolo_result(yolo_data);
|
||
/****************************************************************
|
||
* finish application - close receive thead
|
||
****************************************************************/
|
||
//if ((_loop_time - 1) <= app_yolo_result->inf_number)
|
||
// *bl_run_next_inference = false;
|
||
}
|
||
else if (KDP2_JOB_ID_APP_YOLO_CONFIG_POST_PROC == header_stamp->job_id) {
|
||
/****************************************************************
|
||
* receive pre/post-processing configuration respones
|
||
****************************************************************/
|
||
if (KP_SUCCESS != header_stamp->status_code) {
|
||
printf("[%s] Error: config post-processing params fail %u\n", __func__, header_stamp->status_code);
|
||
return header_stamp->status_code;
|
||
}
|
||
|
||
printf("[%s] config post-processing params success.\n", __func__);
|
||
}
|
||
/*
|
||
else if(DEMO_KL630_CUSTOMIZE_INF_SINGLE_MODEL_JOB_ID == header_stamp->job_id)
|
||
{
|
||
printf("[%s] Job ID %u\n", __FUNCTION__, header_stamp->job_id);
|
||
}
|
||
*/
|
||
else if(STDC_JOB_ID == header_stamp->job_id)
|
||
{
|
||
if (KP_SUCCESS != header_stamp->status_code) {
|
||
printf("[%s] STDC inference error %u\n", __func__, header_stamp->status_code);
|
||
return header_stamp->status_code;
|
||
}
|
||
if (g_test_mode) return KP_SUCCESS;
|
||
stdc_inf_result_t *stdc_res = (stdc_inf_result_t *)header_stamp;
|
||
stdc_analysis_t *ana = &stdc_res->stdc_result.analysis;
|
||
|
||
/* Copy seg map to shared globals for draw-box thread */
|
||
if (ana->seg_width > 0 && ana->seg_height > 0 &&
|
||
ana->seg_width * ana->seg_height <= STDC_SEG_MAP_MAX) {
|
||
pthread_mutex_lock(&g_stdc_seg_mutex);
|
||
memcpy(g_stdc_seg_map, stdc_res->stdc_result.seg_map,
|
||
ana->seg_width * ana->seg_height);
|
||
g_stdc_seg_w = ana->seg_width;
|
||
g_stdc_seg_h = ana->seg_height;
|
||
pthread_mutex_unlock(&g_stdc_seg_mutex);
|
||
}
|
||
|
||
/* ── 1. Console log — class ratios + status (at most once per second) ── */
|
||
{
|
||
static struct timeval s_last_log = {0, 0};
|
||
struct timeval now;
|
||
gettimeofday(&now, NULL);
|
||
long elapsed_us = (now.tv_sec - s_last_log.tv_sec) * 1000000L
|
||
+ (now.tv_usec - s_last_log.tv_usec);
|
||
if (elapsed_us >= 1000000L) {
|
||
s_last_log = now;
|
||
log_trim_if_needed(); /* keep /tmp/fw.log under LOG_MAX_BYTES */
|
||
if (g_verbose_log) {
|
||
printf("[STDC] frame=%u mov=%d diff=%.1f "
|
||
"bunker=%.1f%% car=%.1f%% grass=%.1f%% greenery=%.1f%% "
|
||
"person=%.1f%% pond=%.1f%% road=%.1f%% tree=%.1f%%\n",
|
||
ana->frame_count, ana->is_moving, ana->motion_diff,
|
||
ana->class_ratio[STDC_CLASS_BUNKER] * 100.0f,
|
||
ana->class_ratio[STDC_CLASS_CAR] * 100.0f,
|
||
ana->class_ratio[STDC_CLASS_GRASS] * 100.0f,
|
||
ana->class_ratio[STDC_CLASS_GREENERY] * 100.0f,
|
||
ana->class_ratio[STDC_CLASS_PERSON] * 100.0f,
|
||
ana->class_ratio[STDC_CLASS_POND] * 100.0f,
|
||
ana->class_ratio[STDC_CLASS_ROAD] * 100.0f,
|
||
ana->class_ratio[STDC_CLASS_TREE] * 100.0f);
|
||
|
||
/* ── 2. Grass / motion state ── */
|
||
if (ana->on_grass)
|
||
printf("[STDC] ON GRASS: %.1fs%s\n",
|
||
ana->grass_duration_sec,
|
||
ana->grass_warning ? " *** GRASS WARNING ***" : "");
|
||
else
|
||
printf("[STDC] ON ROAD\n");
|
||
}
|
||
}
|
||
}
|
||
|
||
/* ── 3. Warning log (on state change — matches Python sections H/I/J) ── */
|
||
{
|
||
static int s_last_warn = -1;
|
||
int warn_mask = (ana->collision_risk ? 0x001 : 0)
|
||
| (ana->person_warning ? 0x002 : 0)
|
||
| (ana->car_warning ? 0x004 : 0)
|
||
| (ana->grass_warning ? 0x008 : 0)
|
||
| (ana->tree_dense ? 0x010 : 0)
|
||
| (ana->tree_approaching ? 0x020 : 0)
|
||
| (ana->bunker_warning ? 0x040 : 0)
|
||
| (ana->pond_warning ? 0x080 : 0)
|
||
| (ana->greenery_warning ? 0x100 : 0);
|
||
if (warn_mask != s_last_warn) {
|
||
if (ana->collision_risk)
|
||
printf("[STDC WARN] COLLISION RISK (person=%.1f%% car=%.1f%% "
|
||
"tree=%.1f%% bunker=%.1f%% pond=%.1f%%)\n",
|
||
ana->col_person_ratio*100, ana->col_car_ratio*100,
|
||
ana->col_tree_ratio*100, ana->col_bunker_ratio*100,
|
||
ana->col_pond_ratio*100);
|
||
if (ana->person_warning)
|
||
printf("[STDC WARN] PERSON %.1f%%\n",
|
||
ana->class_ratio[STDC_CLASS_PERSON]*100);
|
||
if (ana->car_warning)
|
||
printf("[STDC WARN] CAR %.1f%%\n",
|
||
ana->class_ratio[STDC_CLASS_CAR]*100);
|
||
if (ana->greenery_warning)
|
||
printf("[STDC WARN] GREENERY AREA %.1f%%\n",
|
||
ana->class_ratio[STDC_CLASS_GREENERY]*100);
|
||
if (ana->bunker_warning)
|
||
printf("[STDC WARN] BUNKER %.1f%%\n",
|
||
ana->class_ratio[STDC_CLASS_BUNKER]*100);
|
||
if (ana->pond_warning)
|
||
printf("[STDC WARN] POND %.1f%%\n",
|
||
ana->class_ratio[STDC_CLASS_POND]*100);
|
||
if (ana->tree_approaching)
|
||
printf("[STDC WARN] TREE APPROACHING (roi_growth=%.1f%%)\n",
|
||
ana->tree_roi_growth*100);
|
||
if (ana->tree_dense)
|
||
printf("[STDC WARN] TREE DENSE AREA %.1f%%\n",
|
||
ana->class_ratio[STDC_CLASS_TREE]*100);
|
||
s_last_warn = warn_mask;
|
||
}
|
||
}
|
||
|
||
/* ── 4. Visual overlay via g_atDrawInfo (rendered by draw_box DMA thread) ──
|
||
*
|
||
* Requires DrawBoxEnable=1 in host_stream.ini / demo_rtsp.sh.
|
||
* The draw_box thread calls draw_rect() for each entry 0..g_dwResultCounts-1.
|
||
*
|
||
* Shared ROI trapezoid (same as web overlay polygon):
|
||
* points="45,55 55,55 70,95 30,95"
|
||
* i.e. top 45%~55%, bottom 30%~70%, y 55%~95%.
|
||
*
|
||
* [top-left] Warning boxes 200×55px, stride 63px (when warning active):
|
||
* y=10 collision_risk y=199 grass_warning
|
||
* y=73 person_warning y=262 tree_dense
|
||
* y=136 car_warning y=325 tree_approaching
|
||
*
|
||
* [right edge] Class presence 40×55px, stride 63px (when ratio > threshold):
|
||
* ci=0 bunker y=10 (thr 1%)
|
||
* ci=1 car y=73 (thr 5%)
|
||
* ci=2 grass y=136 (thr 10%)
|
||
* ci=3 greenery y=199 (thr 20%)
|
||
* ci=4 person y=262 (thr 1%)
|
||
* ci=5 pond y=325 (thr 1%)
|
||
* ci=6 road y=388 (thr 50%)
|
||
* ci=7 tree y=451 (thr 30%)
|
||
*/
|
||
if (g_dwDrawBoxEnable) {
|
||
unsigned int cnt = 0;
|
||
DETECT_INFO *di = g_atDrawInfo;
|
||
const unsigned int draw_w = 1920u;
|
||
const unsigned int draw_h = 1080u;
|
||
|
||
unsigned int x_tl = (draw_w * 45u) / 100u;
|
||
unsigned int x_tr = (draw_w * 55u) / 100u;
|
||
unsigned int x_bl = (draw_w * 30u) / 100u;
|
||
unsigned int x_br = (draw_w * 70u) / 100u;
|
||
unsigned int y_top = (draw_h * 55u) / 100u;
|
||
unsigned int y_bottom = (draw_h * 95u) / 100u;
|
||
|
||
stdc_add_trapezoid_outline(di, &cnt,
|
||
x_tl, x_tr, x_bl, x_br,
|
||
y_top, y_bottom);
|
||
|
||
/* Collision emphasis: draw a second inner trapezoid when active. */
|
||
if (ana->collision_risk && x_tr > x_tl + 8u && x_br > x_bl + 16u && y_bottom > y_top + 8u) {
|
||
stdc_add_trapezoid_outline(di, &cnt,
|
||
x_tl + 4u, x_tr - 4u,
|
||
x_bl + 8u, x_br - 8u,
|
||
y_top + 4u, y_bottom - 4u);
|
||
}
|
||
|
||
/* Side warning/class panels are intentionally disabled.
|
||
* Keep only central ROI overlays to avoid per-class side boxes. */
|
||
|
||
g_dwResultCounts = cnt;
|
||
}
|
||
|
||
/* Drive violation event state machine */
|
||
event_recorder_update(ana);
|
||
}
|
||
else {
|
||
printf("[%s] Error: Job ID %u\n", __FUNCTION__, header_stamp->job_id);
|
||
return KP_FW_ERROR_UNKNOWN_APP;
|
||
}
|
||
|
||
return KP_SUCCESS;
|
||
}
|
||
|