xiaotiangua/tc264smart
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
843447835d9f168a4e771152f531bdc1f0cddce5
tc264smart/code/user_line_process.c
T
32343 843447835d Initial commit: TC264 smart car project with IPS200 menu system 
- IPS200 display menu with real-time status and parameter settings
- Camera image processing and line tracking
- IMU attitude solution and PID control
- WiFi SPI oscilloscope data transmission
2026-05-10 21:38:49 +08:00

713 lines
23 KiB
C
Raw Blame History

#include "user_line_process.h"
#include "user_image_process.h"
#include <math.h>
#include <string.h>
/*============================================================
* 全局变量定义
*============================================================*/
IFX_ALIGN(4) float center_line[MAX_EDGE_PTS][2];
uint16 center_line_num = 0;
IFX_ALIGN(4) float left_edge[MAX_EDGE_PTS][2];
IFX_ALIGN(4) float right_edge[MAX_EDGE_PTS][2];
uint16 left_edge_num = 0, right_edge_num = 0;
IFX_ALIGN(4) float inflection_point[2] = {0.0f, 0.0f};
uint8 inflection_found = 0;
IFX_ALIGN(4) float inflection_point_r[2] = {0.0f, 0.0f};
uint8 inflection_found_r = 0;
#define MAZE_THRESHOLD 128 /* 二值化阈值: <128 背景(黑), >=128 走线(白) */
/* 拐点检测参数 */
#define INF_LOOKAHEAD 10 /* 前后步长 */
#define INF_ANGLE_COS 0.7f /* cos(45.6°) */
/* 中线平滑参数 */
#define CENTER_SMOOTH_WIN 7
#define CENTER_OUTLIER_LIMIT 7.0f
#define CENTER_MAX_DX_PER_ROW 3.0f
static float clamp_float(float value, float min_value, float max_value)
{
if (value < min_value) return min_value;
if (value > max_value) return max_value;
return value;
}
static int abs_int(int value)
{
return value < 0 ? -value : value;
}
static void sort_small_float(float data[], int num)
{
int i, j;
for (i = 1; i < num; i++) {
float key = data[i];
j = i - 1;
while (j >= 0 && data[j] > key) {
data[j + 1] = data[j];
j--;
}
data[j + 1] = key;
}
}
static void interpolate_edge_rows(const int16 edge_x[], float interp_x[], uint8 valid[], int h)
{
int y, prev_y, next_y, fill_y;
for (y = 0; y < h; y++) {
if (edge_x[y] >= 0 && edge_x[y] < 32767) {
interp_x[y] = (float)edge_x[y];
valid[y] = 1;
} else {
interp_x[y] = 0.0f;
valid[y] = 0;
}
}
prev_y = -1;
for (y = 0; y < h; y++) {
if (!valid[y]) continue;
if (prev_y >= 0 && y - prev_y > 1) {
next_y = y;
for (fill_y = prev_y + 1; fill_y < next_y; fill_y++) {
float ratio = (float)(fill_y - prev_y) / (float)(next_y - prev_y);
interp_x[fill_y] = interp_x[prev_y] + (interp_x[next_y] - interp_x[prev_y]) * ratio;
valid[fill_y] = 1;
}
}
prev_y = y;
}
}
static uint8 solve_3x3(float mat[3][4], float result[3])
{
int col, row, pivot_row, i;
for (col = 0; col < 3; col++) {
float pivot_abs = fabsf(mat[col][col]);
pivot_row = col;
for (row = col + 1; row < 3; row++) {
float value_abs = fabsf(mat[row][col]);
if (value_abs > pivot_abs) {
pivot_abs = value_abs;
pivot_row = row;
}
}
if (pivot_abs < 1e-6f) return 0;
if (pivot_row != col) {
for (i = col; i < 4; i++) {
float temp = mat[col][i];
mat[col][i] = mat[pivot_row][i];
mat[pivot_row][i] = temp;
}
}
for (row = 0; row < 3; row++) {
float factor;
if (row == col) continue;
factor = mat[row][col] / mat[col][col];
for (i = col; i < 4; i++) {
mat[row][i] -= factor * mat[col][i];
}
}
}
for (row = 0; row < 3; row++) {
result[row] = mat[row][3] / mat[row][row];
}
return 1;
}
static float eval_center_quad(const float coef[3], int y, int h)
{
float yn = ((float)y - (float)(h - 1)) / (float)h;
return coef[0] + coef[1] * yn + coef[2] * yn * yn;
}
static uint8 fit_center_quad(const float x[], const uint8 valid[], int h,
const float *ref_coef, float residual_limit,
float coef[3])
{
float mat[3][4] = {{0}};
int y, count = 0;
for (y = 0; y < h; y++) {
float yn, yn2, w, ref_x;
if (!valid[y]) continue;
if (ref_coef != 0) {
ref_x = eval_center_quad(ref_coef, y, h);
if (fabsf(x[y] - ref_x) > residual_limit) continue;
}
yn = ((float)y - (float)(h - 1)) / (float)h;
yn2 = yn * yn;
w = 1.0f + 2.0f * (float)y / (float)(h - 1);
mat[0][0] += w;
mat[0][1] += w * yn;
mat[0][2] += w * yn2;
mat[0][3] += w * x[y];
mat[1][0] += w * yn;
mat[1][1] += w * yn2;
mat[1][2] += w * yn2 * yn;
mat[1][3] += w * yn * x[y];
mat[2][0] += w * yn2;
mat[2][1] += w * yn2 * yn;
mat[2][2] += w * yn2 * yn2;
mat[2][3] += w * yn2 * x[y];
count++;
}
if (count < 8) return 0;
return solve_3x3(mat, coef);
}
static void smooth_center_rows(const float raw_x[], const uint8 valid[], float smooth_x[], int h)
{
static float median_x[MAX_EDGE_PTS];
static float avg_x[MAX_EDGE_PTS];
float coef_first[3], coef_second[3];
uint8 fit_ok;
int y, k, half, has_base;
float base_x;
half = CENTER_SMOOTH_WIN / 2;
for (y = 0; y < h; y++) {
float window[CENTER_SMOOTH_WIN];
int num = 0;
median_x[y] = raw_x[y];
avg_x[y] = raw_x[y];
smooth_x[y] = raw_x[y];
if (!valid[y]) continue;
for (k = y - half; k <= y + half; k++) {
if (0 <= k && k < h && valid[k]) {
window[num++] = raw_x[k];
}
}
if (num > 0) {
sort_small_float(window, num);
median_x[y] = window[num / 2];
}
}
for (y = 0; y < h; y++) {
float sum = 0.0f;
float weight_sum = 0.0f;
if (!valid[y]) continue;
for (k = y - half; k <= y + half; k++) {
if (0 <= k && k < h && valid[k]) {
float weight = (float)(half + 1 - abs_int(k - y));
sum += median_x[k] * weight;
weight_sum += weight;
}
}
if (weight_sum > 1e-6f) {
avg_x[y] = sum / weight_sum;
}
}
fit_ok = fit_center_quad(avg_x, valid, h, 0, 0.0f, coef_first);
if (fit_ok) {
fit_ok = fit_center_quad(avg_x, valid, h, coef_first, CENTER_OUTLIER_LIMIT, coef_second);
}
for (y = 0; y < h; y++) {
if (!valid[y]) continue;
if (fit_ok) {
float fit_x = eval_center_quad(coef_second, y, h);
smooth_x[y] = avg_x[y] * 0.45f + fit_x * 0.55f;
} else {
smooth_x[y] = avg_x[y];
}
smooth_x[y] = clamp_float(smooth_x[y], 0.0f, (float)(MT9V03X_1_W - 1));
}
has_base = 0;
base_x = 0.0f;
for (y = h - 1; y >= 0; y--) {
if (!valid[y]) continue;
if (has_base) {
float dx = smooth_x[y] - base_x;
smooth_x[y] = base_x + clamp_float(dx, -CENTER_MAX_DX_PER_ROW, CENTER_MAX_DX_PER_ROW);
}
base_x = smooth_x[y];
has_base = 1;
}
}
/*============================================================
* 4方向表 (迷宫法贴墙走)
* dir: 0=上 1=右 2=下 3=左
*============================================================*/
static const int8 dir_front[4][2] = {{ 0,-1}, { 1, 0}, { 0, 1}, {-1, 0}};
static const int8 dir_frontleft[4][2] = {{-1,-1}, { 1,-1}, { 1, 1}, {-1, 1}};
static const int8 dir_frontright[4][2] = {{ 1,-1}, { 1, 1}, {-1, 1}, {-1,-1}};
/*============================================================
* 检查指定列在拐点行上下3行范围内是否存在黑白跳变
*============================================================*/
static uint8 check_col_transition(uint8 *binary, int iw, int ih, int col, int row)
{
int r, r_start = row - 3;
int r_end = row + 3;
if (r_start < 0) r_start = 0;
if (r_end >= ih) r_end = ih - 1;
if (r_start >= r_end) return 0;
for (r = r_start + 1; r <= r_end; r++) {
if (binary[(r - 1) * iw + col] != binary[r * iw + col]) {
return 1;
}
}
return 0;
}
/*============================================================
* 左手定则迷宫寻线 (4方向贴墙走)
*
* 沿白色赛道左边缘追踪: 始终让"左前方"贴着黑色背景.
*
* 左手规则:
* - 前方为黑 → 右转 (dir+1), turn++
* - 前方为白、左前为黑 → 直走 (贴左墙)
* - 前方为白、左前为白 → 左前走 (赛道左弯), 同时左转 (dir+3)
* - turn >= 4 视为死路退出
*============================================================*/
static uint16 findline_lefthand(uint8 *binary, int iw, int ih,
int start_x, int start_y,
int16 pts[][2], uint16 max_pts) {
int x = start_x, y = start_y;
int dir = 0;
int turn = 0;
uint16 step = 0;
while (step < max_pts && 1 <= x && x < iw - 1 && 1 <= y && y < ih - 1 && turn < 4) {
int fx = x + dir_front[dir][0];
int fy = y + dir_front[dir][1];
int flx = x + dir_frontleft[dir][0];
int fly = y + dir_frontleft[dir][1];
uint8 front = binary[fy * iw + fx];
uint8 frontleft = binary[fly * iw + flx];
if (front < MAZE_THRESHOLD) {
dir = (dir + 1) & 3;
turn++;
} else if (frontleft < MAZE_THRESHOLD) {
x = fx;
y = fy;
pts[step][0] = (int16)x;
pts[step][1] = (int16)y;
step++;
turn = 0;
} else {
x = flx;
y = fly;
dir = (dir + 3) & 3;
pts[step][0] = (int16)x;
pts[step][1] = (int16)y;
step++;
turn = 0;
}
}
/* 拐点检测: 角度突变 → 记录拐点, 继续追踪 */
if (!inflection_found) {
int n = (int)step, s = INF_LOOKAHEAD;
if (n >= 2 * s) {
int i;
for (i = s + 10; i + s < n; i++) {
if (pts[i][1] >= MT9V03X_1_H - 8) continue;
float dx_in = (float)(pts[i][0] - pts[i - s][0]);
float dy_in = (float)(pts[i][1] - pts[i - s][1]);
float dx_out = (float)(pts[i + s][0] - pts[i][0]);
float dy_out = (float)(pts[i + s][1] - pts[i][1]);
float len1 = sqrtf(dx_in * dx_in + dy_in * dy_in);
float len2 = sqrtf(dx_out * dx_out + dy_out * dy_out);
if (len1 < 1e-6f || len2 < 1e-6f) continue;
if ((dx_in * dx_out + dy_in * dy_out) / (len1 * len2) < INF_ANGLE_COS) {
int candidate_row = pts[i][1];
if (candidate_row <= 30) {
int j, has_another = 0;
for (j = i + 1; j + s < n; j++) {
float dx_in2 = (float)(pts[j][0] - pts[j - s][0]);
float dy_in2 = (float)(pts[j][1] - pts[j - s][1]);
float dx_out2 = (float)(pts[j + s][0] - pts[j][0]);
float dy_out2 = (float)(pts[j + s][1] - pts[j][1]);
float len1b = sqrtf(dx_in2 * dx_in2 + dy_in2 * dy_in2);
float len2b = sqrtf(dx_out2 * dx_out2 + dy_out2 * dy_out2);
if (len1b < 1e-6f || len2b < 1e-6f) continue;
if ((dx_in2 * dx_out2 + dy_in2 * dy_out2) / (len1b * len2b) < INF_ANGLE_COS) {
has_another = 1;
break;
}
}
if (has_another) continue;
}
if (check_col_transition(binary, iw, ih, 2, pts[i][1])) {
inflection_point[0] = (float)pts[i][0];
inflection_point[1] = (float)pts[i][1];
inflection_found = 1;
break;
}
}
}
}
}
return step;
}
/*============================================================
* 右手定则迷宫寻线 (4方向贴墙走)
*
* 沿白色赛道右边缘追踪: 始终让"右前方"贴着黑色背景.
*
* 右手规则:
* - 前方为黑 → 左转 (dir+3), turn++
* - 前方为白、右前为黑 → 直走 (贴右墙)
* - 前方为白、右前为白 → 右前走 (赛道右弯), 同时右转 (dir+1)
* - turn >= 4 视为死路退出
*============================================================*/
static uint16 findline_righthand(uint8 *binary, int iw, int ih,
int start_x, int start_y,
int16 pts[][2], uint16 max_pts) {
int x = start_x, y = start_y;
int dir = 0;
int turn = 0;
uint16 step = 0;
while (step < max_pts && 1 <= x && x < iw - 1 && 1 <= y && y < ih - 1 && turn < 4) {
int fx = x + dir_front[dir][0];
int fy = y + dir_front[dir][1];
int frx = x + dir_frontright[dir][0];
int fry = y + dir_frontright[dir][1];
uint8 front = binary[fy * iw + fx];
uint8 frontright = binary[fry * iw + frx];
if (front < MAZE_THRESHOLD) {
dir = (dir + 3) & 3;
turn++;
} else if (frontright < MAZE_THRESHOLD) {
x = fx;
y = fy;
pts[step][0] = (int16)x;
pts[step][1] = (int16)y;
step++;
turn = 0;
} else {
x = frx;
y = fry;
dir = (dir + 1) & 3;
pts[step][0] = (int16)x;
pts[step][1] = (int16)y;
step++;
turn = 0;
}
}
/* 拐点检测: 角度突变 → 记录拐点, 继续追踪 */
if (!inflection_found_r) {
int n = (int)step, s = INF_LOOKAHEAD;
if (n >= 2 * s) {
int i;
for (i = s + 10; i + s < n; i++) {
if (pts[i][1] >= MT9V03X_1_H - 8) continue;
float dx_in = (float)(pts[i][0] - pts[i - s][0]);
float dy_in = (float)(pts[i][1] - pts[i - s][1]);
float dx_out = (float)(pts[i + s][0] - pts[i][0]);
float dy_out = (float)(pts[i + s][1] - pts[i][1]);
float len1 = sqrtf(dx_in * dx_in + dy_in * dy_in);
float len2 = sqrtf(dx_out * dx_out + dy_out * dy_out);
if (len1 < 1e-6f || len2 < 1e-6f) continue;
if ((dx_in * dx_out + dy_in * dy_out) / (len1 * len2) < INF_ANGLE_COS) {
int candidate_row = pts[i][1];
if (candidate_row <= 30) {
int j, has_another = 0;
for (j = i + 1; j + s < n; j++) {
float dx_in2 = (float)(pts[j][0] - pts[j - s][0]);
float dy_in2 = (float)(pts[j][1] - pts[j - s][1]);
float dx_out2 = (float)(pts[j + s][0] - pts[j][0]);
float dy_out2 = (float)(pts[j + s][1] - pts[j][1]);
float len1b = sqrtf(dx_in2 * dx_in2 + dy_in2 * dy_in2);
float len2b = sqrtf(dx_out2 * dx_out2 + dy_out2 * dy_out2);
if (len1b < 1e-6f || len2b < 1e-6f) continue;
if ((dx_in2 * dx_out2 + dy_in2 * dy_out2) / (len1b * len2b) < INF_ANGLE_COS) {
has_another = 1;
break;
}
}
if (has_another) continue;
}
if (check_col_transition(binary, iw, ih, 138, pts[i][1])) {
inflection_point_r[0] = (float)pts[i][0];
inflection_point_r[1] = (float)pts[i][1];
inflection_found_r = 1;
break;
}
}
}
}
}
return step;
}
/*============================================================
* 赛道边界检测 (迷宫法)
*
* 从图像底部中心找到白色走线上的一点作为起点,
* 分别用左手定则和右手定则向上跟踪走线的左右边缘.
*
* 新增: 当中心点未命中赛道时, 全宽扫描底行找最近赛道段,
* 解决车体偏航导致整帧丢弃的问题.
*============================================================*/
void find_circuit_edges(uint8 *binary, uint16 w, uint16 h,
int16 start_x, int16 start_y,
int16 left_pts[][2], uint16 *left_num,
int16 right_pts[][2], uint16 *right_num) {
int iw = (int)w, ih = (int)h;
int cy = (int)start_y;
int left_start, right_start;
int row_base, x;
if (cy > ih - 2) cy = ih - 2;
if (cy < 1) cy = 1;
/* ---- 1. 底行从左右两侧向内扫描找边界 ---- */
row_base = cy * iw;
left_start = -1;
right_start = -1;
for (x = 1; x < iw - 1; x++) {
if (binary[row_base + x] >= MAZE_THRESHOLD &&
binary[row_base + x - 1] < MAZE_THRESHOLD) {
left_start = x;
break;
}
}
for (x = iw - 2; x >= 1; x--) {
if (binary[row_base + x] >= MAZE_THRESHOLD &&
binary[row_base + x + 1] < MAZE_THRESHOLD) {
right_start = x;
break;
}
}
if (left_start < 0 || right_start < 0) {
*left_num = 0;
*right_num = 0;
return;
}
/* ---- 2. 左右手定则分别从各自边缘向上追踪 ---- */
*left_num = findline_lefthand(binary, iw, ih,
left_start, cy,
left_pts, *left_num);
*right_num = findline_righthand(binary, iw, ih,
right_start, cy,
right_pts, *right_num);
}
/*============================================================
* 中线计算
*
* 同行左取最大 x、右取最小 x, 先插值补齐缺失行,
* 再做中值/三角平滑和鲁棒二次拟合.
*============================================================*/
void compute_center_line_interp(int16 left[][2], uint16 left_num,
int16 right[][2], uint16 right_num,
float center[][2], uint16 *center_num)
{
static int16 left_max_x[MAX_EDGE_PTS];
static int16 right_min_x[MAX_EDGE_PTS];
static float left_interp_x[MAX_EDGE_PTS];
static float right_interp_x[MAX_EDGE_PTS];
static float cx_raw[MAX_EDGE_PTS];
static float cx_smooth[MAX_EDGE_PTS];
static uint8 left_valid[MAX_EDGE_PTS];
static uint8 right_valid[MAX_EDGE_PTS];
static uint8 center_valid[MAX_EDGE_PTS];
uint16 i;
int y;
if (left_num < 2 || right_num < 2) {
*center_num = 0;
return;
}
/* ---- 1. 初始化 ---- */
for (i = 0; i < MAX_EDGE_PTS; i++) {
left_max_x[i] = -1;
right_min_x[i] = 32767;
cx_raw[i] = -1.0f;
cx_smooth[i] = -1.0f;
left_valid[i] = 0;
right_valid[i] = 0;
center_valid[i] = 0;
}
/* ---- 2. 左取最大, 右取最小 (取内侧边界点) ---- */
for (i = 0; i < left_num; i++) {
y = (int)left[i][1];
if (0 <= y && y < MAX_EDGE_PTS) {
if (left[i][0] > left_max_x[y])
left_max_x[y] = left[i][0];
}
}
for (i = 0; i < right_num; i++) {
y = (int)right[i][1];
if (0 <= y && y < MAX_EDGE_PTS) {
if (right[i][0] < right_min_x[y])
right_min_x[y] = right[i][0];
}
}
/* ---- 3. 边线缺失行插值, 避免同一行没有配对点造成中线稀疏 ---- */
interpolate_edge_rows(left_max_x, left_interp_x, left_valid, (int)MT9V03X_1_H);
interpolate_edge_rows(right_min_x, right_interp_x, right_valid, (int)MT9V03X_1_H);
/* ---- 4. 每行原始中线 ---- */
for (y = 0; y < (int)MT9V03X_1_H; y++) {
if (left_valid[y] && right_valid[y]) {
cx_raw[y] = (left_interp_x[y] + right_interp_x[y]) * 0.5f;
center_valid[y] = 1;
}
}
smooth_center_rows(cx_raw, center_valid, cx_smooth, (int)MT9V03X_1_H);
/* ---- 5. 输出所有有效行的平滑中线 ---- */
*center_num = 0;
for (y = 0; y < (int)MT9V03X_1_H && *center_num < MAX_EDGE_PTS; y++) {
if (center_valid[y]) {
center[*center_num][0] = cx_smooth[y];
center[*center_num][1] = (float)y;
(*center_num)++;
}
}
if (*center_num < 2) {
*center_num = 0;
return;
}
}
/*============================================================
* 一帧图像处理主入口
*
* 流水线:
* binary → find_circuit_edges (迷宫法)
* → compute_center_line_interp (插值、平滑、拟合中线)
* → 左右边线直接输出
* → detect_inflection_point (拐点检测)
*
* 输出全局变量: center_line, left_edge, right_edge, inflection_point
*============================================================*/
void line_process_run(uint8 *binary)
{
static int16 left_pts[MAX_EDGE_PTS][2];
static int16 right_pts[MAX_EDGE_PTS][2];
uint16 left_num = MAX_EDGE_PTS;
uint16 right_num = MAX_EDGE_PTS;
uint16 i;
/* 清除上帧拐点标志 (trace 函数只置位, 不清零) */
inflection_found = 0;
inflection_found_r = 0;
/* ---- 1. 迷宫法赛道边缘检测 ---- */
find_circuit_edges(
binary,
MT9V03X_1_W, MT9V03X_1_H,
(int16)(MT9V03X_1_W / 2),
(int16)(MT9V03X_1_H - 1),
left_pts, &left_num,
right_pts, &right_num);
if (left_num < 2 || right_num < 2) {
center_line_num = 0;
left_edge_num = 0;
right_edge_num = 0;
return;
}
/* ---- 2. 中线计算 (插值、平滑、拟合) ---- */
compute_center_line_interp(left_pts, left_num, right_pts, right_num,
center_line, &center_line_num);
/* ---- 3. 左边线输出 ---- */
left_edge_num = left_num < MAX_EDGE_PTS ? left_num : MAX_EDGE_PTS;
for (i = 0; i < left_edge_num; i++) {
left_edge[i][0] = (float)left_pts[i][0];
left_edge[i][1] = (float)left_pts[i][1];
}
/* ---- 4. 右边线输出 ---- */
right_edge_num = right_num < MAX_EDGE_PTS ? right_num : MAX_EDGE_PTS;
for (i = 0; i < right_edge_num; i++) {
right_edge[i][0] = (float)right_pts[i][0];
right_edge[i][1] = (float)right_pts[i][1];
}
}
/*============================================================
* 中线加权偏差 (越靠图像底部权重越大)
*
* 仅计入 y >= min_row 的中线点, 上半部分忽略.
* weight = y + 1 (y=顶部0, y=底部≈79)
* 底部(近车)权重大, 顶部(远处)权重小
*
* 返回: 加权平均中线偏差 (像素), 正=偏右, 负=偏左
*============================================================*/
float line_compute_weighted_deviation(uint16 image_center_x, uint16 min_row)
{
float w_sum = 0.0f, dev_sum = 0.0f;
uint16 i;
if (center_line_num < 2)
return 0.0f;
for (i = 0; i < center_line_num; i++)
{
float y = center_line[i][1];
if (y < (float)min_row) continue;
float weight = y + 1.0f;
float dev = center_line[i][0] - (float)image_center_x;
dev_sum += dev * weight;
w_sum += weight;
}
return (w_sum > 0.0f) ? (dev_sum / w_sum) : 0.0f;
}
Reference in new issue View Git Blame Copy Permalink