32343
843447835d
- 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
992 lines
36 KiB
C
992 lines
36 KiB
C
/*============================================================
|
||
* TC264 智能车 - IPS200 显示菜单模块 (user_menu.c)
|
||
*
|
||
* 功能说明：
|
||
* 基于 IPS200 基本绘图函数实现的菜单系统。
|
||
* 提供实时状态显示页和参数设置页，支持按键切换与参数调整。
|
||
*
|
||
* 页面0 - 实时状态页：
|
||
* - 顶部状态栏：电池电量 + CPU 占用率
|
||
* - 中部：摄像头图像 (140x80)
|
||
* - 底部：实时数据（编码器、IMU角度、电池电压等）
|
||
*
|
||
* 页面1 - 参数设置页：
|
||
* - 编号参数列表，支持 KEY1/KEY2/KEY3 选择与调整
|
||
*
|
||
* 硬件依赖：
|
||
* - IPS200 显示屏 (SPI 或 并口)
|
||
* - MT9V03X 摄像头
|
||
* - 4个按键 (KEY1~KEY4)
|
||
*============================================================*/
|
||
|
||
#include "zf_common_font.h" /* 字库（chinese_test等） */
|
||
#include "zf_common_function.h" /* 通用函数（zf_sprintf等）*/
|
||
#include "zf_common_interrupt.h" /* 中断控制 */
|
||
#include "zf_driver_adc.h" /* ADC 驱动 */
|
||
#include "zf_device_key.h" /* 按键驱动 */
|
||
#include "zf_device_imu660rb.h" /* IMU660RB 驱动 (转换因子) */
|
||
#include "attitude_solution.h" /* 姿态解算 (欧拉角) */
|
||
#include "user_menu_device.h" /* 菜单设备层（变量定义） */
|
||
#include "user_menu.h" /* 菜单头文件 */
|
||
#include "user_line_process.h" /* 中线/边线/拐点 */
|
||
#include "zf_device_wifi_spi.h" /* WiFi SPI 无线模块 */
|
||
#include "zf_driver_pwm.h" /* PWM 驱动 (真空泵) */
|
||
#include "seekfree_assistant_interface.h"
|
||
#include "seekfree_assistant.h"
|
||
#include "user_image_process.h"
|
||
#include "user_line_process.h"
|
||
#include "pid_tuning.h"
|
||
|
||
/*============================================================
|
||
* WiFi 配置宏 (请修改为你实际使用的 WiFi)
|
||
*============================================================*/
|
||
#define WIFI_SSID "smartcar"
|
||
#define WIFI_PASSWORD "zsg372901"
|
||
//WIFI_SPI_TARGET_IP（修改ip地址）
|
||
|
||
/*============================================================
|
||
* 浮点参数数组 (菜单参数6-9的值)
|
||
* 数组下标 0=WiFi占位, 1-4=浮点参数值
|
||
*
|
||
* 注意: 参数1=WiFi, 参数2-5=PID参数(来自test_pid_value),
|
||
* 参数6-9=浮点参数(来自本数组)
|
||
*============================================================*/
|
||
float test_user_value[PARAM_TOTAL_NUM] = {
|
||
0, /* 参数1(WiFi): 占位 */
|
||
100, /* 参数6: 基础速度 (手动速度模式) */
|
||
10, /* 参数7: 负压占空比 */
|
||
200, /* 参数8: 最大时速 */
|
||
50, /* 参数9: 弯道补偿 */
|
||
};
|
||
|
||
/*============================================================
|
||
* PID 参数组定义
|
||
* 三个控制环：左速度、右速度、转向
|
||
* 每组包含 P/I/D 三个参数
|
||
*============================================================*/
|
||
char test_pid_loop[PID_TUNING_LOOP_COUNT][12] = {
|
||
"SpdL", /* 组0: 左速度环 */
|
||
"SpdR", /* 组1: 右速度环 */
|
||
"Steering", /* 组2: 转向环 */
|
||
};
|
||
|
||
/* PID 参数值 (3组 x 3参数 = P/I/D), 从零开始, 通过菜单实时调参 */
|
||
float test_pid_value[PID_TUNING_LOOP_COUNT][3] = {
|
||
{0.0, 1.0, 0.0}, /* 左速度环: P=0, I=1, D=0 */
|
||
{0.0, 1.0, 0.0}, /* 右速度环: P=0, I=1, D=0 */
|
||
{0.0, 0.0, 0.0}, /* 转向环: P=0, I=0, D=0 */
|
||
};
|
||
|
||
/*============================================================
|
||
* 参数名称列表 (GB2312 编码)
|
||
* 索引0=WiFi, 1-9=浮点参数, 10=PID环, 11-13=P/I/D值
|
||
*============================================================*/
|
||
static const char *param_name[] = {
|
||
"WiFi SPI", /* 参数1: WiFi连接 */
|
||
"PID Sel", /* 参数2: PID选择 */
|
||
"Curr P", /* 参数3: 当前P参 */
|
||
"Curr I", /* 参数4: 当前I参 */
|
||
"Curr D", /* 参数5: 当前D参 */
|
||
"Base Spd", /* 参数6: 基础速度 */
|
||
"Vac Duty", /* 参数7: 负压占空比 */
|
||
"Max Spd", /* 参数8: 最大时速 */
|
||
"Crn Comp", /* 参数9: 弯道补偿 */
|
||
};
|
||
|
||
/*============================================================
|
||
* 全局页面状态变量
|
||
*============================================================*/
|
||
static uint8 current_page = MENU_PAGE_REALTIME; /* 当前页面编号 */
|
||
static int16 param_selected = 0; /* 当前选中的参数行 */
|
||
static int16 param_scroll_offset = 0; /* 参数页滚动偏移量 */
|
||
int16 pid_switch_num = 0; /* PID 环切换索引 */
|
||
static int16 value_refresh_flag = 0; /* 值刷新标志 */
|
||
|
||
/* PID 调试变量 (来自 pid_control.c, 避免包含 pid_control.h 导致 bool 类型冲突) */
|
||
extern uint8 g_control_enabled;
|
||
extern float g_target_speed;
|
||
|
||
/* 真空泵状态 */
|
||
static uint8 vac_enabled = 0; /* 真空泵开关 0=关 1=开 */
|
||
|
||
/* WiFi 状态变量 */
|
||
static uint8 wifi_state = 0; /* 0=idle,1=connecting,2=connected,3=failed */
|
||
static uint8 wifi_request_flag = 0; /* KEY2触发WiFi连接 */
|
||
static uint8 wifi_init_done = 0; /* WiFi模块是否已完成初始化 */
|
||
static char wifi_status_str[20] = "Idle"; /* WiFi状态显示文字 */
|
||
|
||
/* 屏幕分辨率参数 */
|
||
static uint16 screen_w = SCREEN_WIDTH; /* 屏幕实际宽度 */
|
||
static uint16 screen_h = SCREEN_HEIGHT; /* 屏幕实际高度 */
|
||
|
||
/* 双核同步标志 (CPU0 ↔ CPU1 图像处理管线) */
|
||
|
||
/* 摄像头显示帧缓冲区：用于避免 DMA 连续写入导致画面撕裂 */
|
||
uint8 camera_frame_buffer[MT9V03X_1_H * MT9V03X_1_W];
|
||
|
||
/* WiFi 分块非阻塞发送状态 (每帧发送多个 SPI 块，平衡速度与流畅度) */
|
||
#define WIFI_CHUNK_SIZE (4088) /* 单次 SPI 最大传输字节 */
|
||
#define WIFI_CHUNKS_PER_FRAME (4) /* 每帧最多发送的块数: 7=1帧, 4=2帧, 3=3帧完成传输 */
|
||
static uint8 wifi_chunk_send_active = 0;
|
||
static uint8 wifi_chunk_send_phase = 0; /* 0=发送帧头, 1..=发送图像数据块 */
|
||
static uint32 wifi_chunk_remaining = 0;
|
||
static uint8 *wifi_chunk_ptr = NULL;
|
||
static uint8 wifi_header[8]; /* 逐飞助手协议帧头 */
|
||
static uint16 wifi_send_cooldown = 0; /* 发送间隔控制 */
|
||
|
||
/*============================================================
|
||
* 获取参数值字符串 (格式化存储)
|
||
* 根据参数编号将对应的值格式化为字符串
|
||
*============================================================*/
|
||
static void get_param_value_str(int16 index, char *buf, uint8 buf_size)
|
||
{
|
||
uint32 slen;
|
||
if(index < 0 || index >= PARAM_TOTAL_NUM) return;
|
||
|
||
if(index == 0)
|
||
{
|
||
/* 参数1: WiFi 状态 */
|
||
slen = zf_sprintf(buf, "%s", wifi_status_str);
|
||
}
|
||
else if(index == 1)
|
||
{
|
||
/* 参数2: PID 环名称 */
|
||
slen = zf_sprintf(buf, "%s", test_pid_loop[pid_switch_num]);
|
||
}
|
||
else if(index >= 2 && index <= 4)
|
||
{
|
||
/* 参数3-5: PID P/I/D 值，手动格式化 */
|
||
float val = test_pid_value[pid_switch_num][(uint8)(index - 2)];
|
||
int16 int_part = (int16)val;
|
||
int16 dec_part = (int16)((val - (float)int_part) * 10.0f);
|
||
if(dec_part < 0) dec_part = -dec_part;
|
||
slen = zf_sprintf(buf, "%d.%d", int_part, dec_part);
|
||
}
|
||
else
|
||
{
|
||
/* 参数6-9: 浮点值，手动格式化（zf_sprintf不支持%.1f精度） */
|
||
float val = test_user_value[index - 4];
|
||
int16 int_part = (int16)val;
|
||
int16 dec_part = (int16)((val - (float)int_part) * 10.0f);
|
||
if(dec_part < 0) dec_part = -dec_part;
|
||
slen = zf_sprintf(buf, "%d.%d", int_part, dec_part);
|
||
}
|
||
buf[slen] = '\0';
|
||
}
|
||
|
||
/*============================================================
|
||
* 绘制实心进度条
|
||
* x, y: 左上角坐标
|
||
* w: 宽度, h: 高度
|
||
* percent: 百分比 (0-100)
|
||
* bar_color: 进度条颜色
|
||
*============================================================*/
|
||
void user_menu_draw_progress_bar(uint16 x, uint16 y, uint16 w, uint16 percent, uint16 bar_color)
|
||
{
|
||
uint16 fill_w = 0; /* 填充宽度 */
|
||
uint16 i = 0;
|
||
|
||
/* 确保百分比在有效范围 */
|
||
if(percent > 100) percent = 100;
|
||
if(percent < 0) percent = 0;
|
||
|
||
/* 绘制进度条背景（外框） */
|
||
ips200_draw_line(x, y, x + w, y, COLOR_BAR_BG); /* 上边线 */
|
||
ips200_draw_line(x, y + 10, x + w, y + 10, COLOR_BAR_BG); /* 下边线 */
|
||
ips200_draw_line(x, y, x, y + 10, COLOR_BAR_BG); /* 左边线 */
|
||
ips200_draw_line(x + w, y, x + w, y + 10, COLOR_BAR_BG); /* 右边线 */
|
||
|
||
/* 计算填充宽度 = 总宽度 * 百分比 */
|
||
fill_w = (uint16)((uint32)w * percent / 100);
|
||
|
||
/* 逐像素绘制进度条填充 */
|
||
for(i = 0; i < fill_w; i++)
|
||
{
|
||
ips200_draw_line(x + i, y + 1, x + i, y + 9, bar_color);
|
||
}
|
||
}
|
||
|
||
/*============================================================
|
||
* 绘制顶部状态栏
|
||
* 显示电池电量百分比和 CPU 占用率百分比
|
||
* 每一栏包含：文字标签 + 进度条 + 百分比数值
|
||
*============================================================*/
|
||
void user_menu_draw_status_bar(void)
|
||
{
|
||
char str_buf[32];
|
||
uint32 slen;
|
||
|
||
/* 设置字体颜色：白字黑底 */
|
||
ips200_set_color(COLOR_TEXT, COLOR_BG);
|
||
|
||
/* --- 第一行：电池电量 --- */
|
||
ips200_show_string(0, STATUSBAR_Y, "BAT:");
|
||
|
||
/* 显示百分比数值 */
|
||
slen = zf_sprintf(str_buf, "%d%", (uint8)battery_percentage_filter);
|
||
str_buf[slen] = '\0';
|
||
ips200_show_string(178, STATUSBAR_Y, str_buf);
|
||
|
||
/* 绘制电池进度条：根据电量选择颜色 */
|
||
uint16 bar_color = COLOR_BAR_LOW;
|
||
if(battery_percentage_filter > 60)
|
||
bar_color = COLOR_BAR_HIGH;
|
||
else if(battery_percentage_filter > 20)
|
||
bar_color = COLOR_BAR_MID;
|
||
|
||
user_menu_draw_progress_bar(50, STATUSBAR_Y, 120, (uint8)battery_percentage_filter, bar_color);
|
||
|
||
/* --- 第二行：CPU 占用率 --- */
|
||
ips200_show_string(0, STATUSBAR_Y + 16, "CPU:");
|
||
slen = zf_sprintf(str_buf, "%d%%", (uint8)cpu_usage_percentage_filter);
|
||
str_buf[slen] = '\0';
|
||
ips200_show_string(178, STATUSBAR_Y + 16, str_buf);
|
||
|
||
/* CPU 占用率进度条：颜色反转，低占用绿->高占用红 */
|
||
uint8 cpu_inv = 100 - (uint8)cpu_usage_percentage_filter;
|
||
bar_color = COLOR_BAR_LOW;
|
||
if(cpu_inv > 60)
|
||
bar_color = COLOR_BAR_HIGH;
|
||
else if(cpu_inv > 20)
|
||
bar_color = COLOR_BAR_MID;
|
||
|
||
user_menu_draw_progress_bar(50, STATUSBAR_Y + 16, 120, cpu_inv, bar_color);
|
||
}
|
||
|
||
/*============================================================
|
||
* 绘制摄像头图像
|
||
* 在屏幕中间区域显示 MT9V03X 的灰度图像
|
||
* 如果摄像头图像数据异常，显示提示文字
|
||
*============================================================*/
|
||
void user_menu_draw_camera(void)
|
||
{
|
||
/* 先将摄像头图像复制到帧缓冲区，避免 DMA 写入导致显示撕裂 */
|
||
memcpy(camera_frame_buffer, (const uint8 *)mt9v03x_image_1, (uint32)(MT9V03X_1_H * MT9V03X_1_W));
|
||
|
||
/* 显示灰度图像，从帧缓冲区读取，DMA 写入不会影响显示 */
|
||
ips200_show_gray_image(IMAGE_X, IMAGE_Y,
|
||
(const uint8 *)camera_frame_buffer,
|
||
MT9V03X_1_W, MT9V03X_1_H,
|
||
IMAGE_CAM_W, IMAGE_CAM_H,
|
||
0);
|
||
}
|
||
|
||
static void user_menu_draw_line_overlay(void);
|
||
|
||
static void user_menu_show_padded_data_line(uint16 y, char *buf, uint16 buf_size, uint32 slen)
|
||
{
|
||
uint16 char_count = (uint16)slen;
|
||
uint16 line_chars = SCREEN_WIDTH / 8;
|
||
|
||
if(char_count >= buf_size)
|
||
char_count = buf_size - 1;
|
||
|
||
while(char_count < line_chars && char_count < buf_size - 1)
|
||
{
|
||
buf[char_count++] = ' ';
|
||
}
|
||
buf[char_count] = '\0';
|
||
|
||
ips200_show_string(0, y, buf);
|
||
}
|
||
|
||
void user_menu_draw_camera_binary(void)
|
||
{
|
||
/* 图像处理 (二值化+边界追踪+中线) 已在 CPU0 帧同步中完成 */
|
||
|
||
ips200_show_gray_image(IMAGE_X, IMAGE_Y,
|
||
(const uint8 *)image_binary_1,
|
||
MT9V03X_1_W, MT9V03X_1_H,
|
||
IMAGE_CAM_W, IMAGE_CAM_H,
|
||
0);
|
||
|
||
/* 赛道中线与边界线叠加显示 */
|
||
user_menu_draw_line_overlay();
|
||
}
|
||
|
||
/*============================================================
|
||
* 赛道线叠加绘制 (在摄像头图像上叠加中线与边线)
|
||
*============================================================*/
|
||
static void user_menu_draw_line_overlay(void)
|
||
{
|
||
uint16 i;
|
||
uint16 sx, sy, ex, ey;
|
||
|
||
/* 中线 — 绿色 */
|
||
if (center_line_num >= 2)
|
||
{
|
||
for (i = 0; i < center_line_num - 1; i++)
|
||
{
|
||
sx = IMAGE_X + (uint16)(center_line[i][0] + 0.5f);
|
||
sy = IMAGE_Y + (uint16)(center_line[i][1] + 0.5f);
|
||
ex = IMAGE_X + (uint16)(center_line[i + 1][0] + 0.5f);
|
||
ey = IMAGE_Y + (uint16)(center_line[i + 1][1] + 0.5f);
|
||
ips200_draw_line(sx, sy, ex, ey, RGB565_GREEN);
|
||
}
|
||
}
|
||
|
||
/* 左边线 — 红色 */
|
||
if (left_edge_num >= 2)
|
||
{
|
||
for (i = 0; i < left_edge_num - 1; i++)
|
||
{
|
||
sx = IMAGE_X + (uint16)(left_edge[i][0] + 0.5f);
|
||
sy = IMAGE_Y + (uint16)(left_edge[i][1] + 0.5f);
|
||
ex = IMAGE_X + (uint16)(left_edge[i + 1][0] + 0.5f);
|
||
ey = IMAGE_Y + (uint16)(left_edge[i + 1][1] + 0.5f);
|
||
ips200_draw_line(sx, sy, ex, ey, RGB565_RED);
|
||
}
|
||
}
|
||
|
||
/* 右边线 — 蓝色 */
|
||
if (right_edge_num >= 2)
|
||
{
|
||
for (i = 0; i < right_edge_num - 1; i++)
|
||
{
|
||
sx = IMAGE_X + (uint16)(right_edge[i][0] + 0.5f);
|
||
sy = IMAGE_Y + (uint16)(right_edge[i][1] + 0.5f);
|
||
ex = IMAGE_X + (uint16)(right_edge[i + 1][0] + 0.5f);
|
||
ey = IMAGE_Y + (uint16)(right_edge[i + 1][1] + 0.5f);
|
||
ips200_draw_line(sx, sy, ex, ey, RGB565_BLUE);
|
||
}
|
||
}
|
||
|
||
/* ---- 拐点标记 + 红色矩形框 ---- */
|
||
{
|
||
uint8 idx;
|
||
float *ipts[2] = { inflection_point, inflection_point_r };
|
||
uint8 ifnd[2] = { inflection_found, inflection_found_r };
|
||
uint16 colors[2] = { RGB565_YELLOW, RGB565_CYAN };
|
||
|
||
/* 画矩形框: 拐点行在30-79时，以整幅图像为中心画100x60矩形框 */
|
||
{
|
||
uint8 draw_rect = 0;
|
||
for (idx = 0; idx < 2; idx++) {
|
||
if (ifnd[idx] && ipts[idx][1] >= 30.0f && ipts[idx][1] <= 79.0f) {
|
||
draw_rect = 1;
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (draw_rect) {
|
||
int16 img_cx = (int16)IMAGE_X + (int16)(IMAGE_CAM_W / 2);
|
||
int16 img_cy = (int16)IMAGE_Y + (int16)(IMAGE_CAM_H / 2);
|
||
int16 rx0 = img_cx - 50;
|
||
int16 ry0 = img_cy - 30;
|
||
int16 rx1 = img_cx + 50;
|
||
int16 ry1 = img_cy + 30;
|
||
|
||
ips200_draw_line((uint16)rx0, (uint16)ry0, (uint16)rx1, (uint16)ry0, RGB565_RED);
|
||
ips200_draw_line((uint16)rx1, (uint16)ry0, (uint16)rx1, (uint16)ry1, RGB565_RED);
|
||
ips200_draw_line((uint16)rx1, (uint16)ry1, (uint16)rx0, (uint16)ry1, RGB565_RED);
|
||
ips200_draw_line((uint16)rx0, (uint16)ry1, (uint16)rx0, (uint16)ry0, RGB565_RED);
|
||
}
|
||
}
|
||
|
||
/* 画十字标记 (两个都画, 裁剪到图像区域内) */
|
||
for (idx = 0; idx < 2; idx++) {
|
||
if (!ifnd[idx]) continue;
|
||
int16 cx = IMAGE_X + (int16)(ipts[idx][0] + 0.5f);
|
||
int16 cy = IMAGE_Y + (int16)(ipts[idx][1] + 0.5f);
|
||
int16 img_x1 = (int16)IMAGE_X, img_x2 = (int16)(IMAGE_X + IMAGE_CAM_W - 1);
|
||
int16 img_y1 = (int16)IMAGE_Y, img_y2 = (int16)(IMAGE_Y + IMAGE_CAM_H - 1);
|
||
int16 xl = cx - 3, xr = cx + 3, yt = cy - 3, yb = cy + 3;
|
||
if (xl < img_x1) xl = img_x1; if (xr > img_x2) xr = img_x2;
|
||
if (yt < img_y1) yt = img_y1; if (yb > img_y2) yb = img_y2;
|
||
if (cy >= img_y1 && cy <= img_y2) {
|
||
ips200_draw_line((uint16)xl, (uint16)cy, (uint16)xr, (uint16)cy, colors[idx]);
|
||
}
|
||
if (cx >= img_x1 && cx <= img_x2) {
|
||
ips200_draw_line((uint16)cx, (uint16)yt, (uint16)cx, (uint16)yb, colors[idx]);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/*============================================================
|
||
* 绘制实时数据
|
||
* 在图像下方的区域显示编码器、IMU 等实时数据
|
||
*============================================================*/
|
||
void user_menu_draw_realtime_data(void)
|
||
{
|
||
char str_buf[48];
|
||
uint32 slen;
|
||
uint16 y = DATA_Y;
|
||
|
||
ips200_set_color(COLOR_TEXT, COLOR_BG);
|
||
|
||
/* 第一行：编码器1和编码器2 */
|
||
slen = zf_sprintf(str_buf, "ENC1:%d ENC2:%d",
|
||
encoder_data_1, encoder_data_2);
|
||
user_menu_show_padded_data_line(y, str_buf, sizeof(str_buf), slen);
|
||
y += DATA_LINE_H;
|
||
|
||
/* 第二行：IMU 加速度数据 (G, 1位小数) */
|
||
/* 8G量程: transition_factor[0]=4098, 4098 LSB = 1G */
|
||
{
|
||
float ax = (float)imu660rb_acc_x / imu660rb_transition_factor[0];
|
||
float ay = (float)imu660rb_acc_y / imu660rb_transition_factor[0];
|
||
float az = (float)imu660rb_acc_z / imu660rb_transition_factor[0];
|
||
int16 axi = (int16)ax, ayi = (int16)ay, azi = (int16)az;
|
||
int16 axd = (int16)((ax - (float)axi) * 10.0f);
|
||
int16 ayd = (int16)((ay - (float)ayi) * 10.0f);
|
||
int16 azd = (int16)((az - (float)azi) * 10.0f);
|
||
if(axd < 0) axd = -axd;
|
||
if(ayd < 0) ayd = -ayd;
|
||
if(azd < 0) azd = -azd;
|
||
slen = zf_sprintf(str_buf, "ACC:%d.%d %d.%d %d.%dG",
|
||
axi, axd, ayi, ayd, azi, azd);
|
||
}
|
||
user_menu_show_padded_data_line(y, str_buf, sizeof(str_buf), slen);
|
||
y += DATA_LINE_H;
|
||
|
||
/* 第三行：IMU 陀螺仪数据 (°/s, 1位小数) */
|
||
/* 2000dps量程: transition_factor[1]=14.3, 14.3 LSB = 1°/s */
|
||
{
|
||
float gx = (float)imu660rb_gyro_x / imu660rb_transition_factor[1];
|
||
float gy = (float)imu660rb_gyro_y / imu660rb_transition_factor[1];
|
||
float gz = (float)imu660rb_gyro_z / imu660rb_transition_factor[1];
|
||
int16 gxi = (int16)gx, gyi = (int16)gy, gzi = (int16)gz;
|
||
int16 gxd = (int16)((gx - (float)gxi) * 10.0f);
|
||
int16 gyd = (int16)((gy - (float)gyi) * 10.0f);
|
||
int16 gzd = (int16)((gz - (float)gzi) * 10.0f);
|
||
if(gxd < 0) gxd = -gxd;
|
||
if(gyd < 0) gyd = -gyd;
|
||
if(gzd < 0) gzd = -gzd;
|
||
slen = zf_sprintf(str_buf, "GYRO:%d.%d %d.%d %d.%d",
|
||
gxi, gxd, gyi, gyd, gzi, gzd);
|
||
}
|
||
user_menu_show_padded_data_line(y, str_buf, sizeof(str_buf), slen);
|
||
y += DATA_LINE_H;
|
||
|
||
|
||
/* 第四行：姿态角 Yaw/Pitch/Roll (°) */
|
||
{
|
||
int16 yi = (int16)eulerAngle.yaw;
|
||
int16 pi = (int16)eulerAngle.pitch;
|
||
int16 ri = (int16)eulerAngle.roll;
|
||
int16 yd = (int16)((eulerAngle.yaw - (float)yi) * 10.0f);
|
||
int16 pd = (int16)((eulerAngle.pitch - (float)pi) * 10.0f);
|
||
int16 rd = (int16)((eulerAngle.roll - (float)ri) * 10.0f);
|
||
if(yd < 0) yd = -yd;
|
||
if(pd < 0) pd = -pd;
|
||
if(rd < 0) rd = -rd;
|
||
slen = zf_sprintf(str_buf, "ATT:%d.%d %d.%d %d.%d",
|
||
yi, yd, pi, pd, ri, rd);
|
||
}
|
||
user_menu_show_padded_data_line(y, str_buf, sizeof(str_buf), slen);
|
||
y += DATA_LINE_H;
|
||
|
||
/* 第六行：电池电压（拆分为整数+小数，避免zf_sprintf不支持%f） */
|
||
{
|
||
uint16 volt_int = (uint16)battery_voltage;
|
||
uint16 volt_dec = (uint16)((battery_voltage - (float)volt_int) * 100.0f + 0.5f);
|
||
slen = zf_sprintf(str_buf, "BAT:%d.%d%dV %d%%",
|
||
volt_int,
|
||
volt_dec / 10, volt_dec % 10,
|
||
(uint8)battery_percentage_filter);
|
||
}
|
||
user_menu_show_padded_data_line(y, str_buf, sizeof(str_buf), slen);
|
||
y += DATA_LINE_H;
|
||
|
||
/* 第七行：左右速度环PWM输出 + 控制状态 (调试用) */
|
||
slen = zf_sprintf(str_buf, "M:L%d R%d T%d E%d",
|
||
(int16)g_last_pwm_left,
|
||
(int16)g_last_pwm_right,
|
||
(int16)g_target_speed,
|
||
(int16)g_control_enabled);
|
||
user_menu_show_padded_data_line(y, str_buf, sizeof(str_buf), slen);
|
||
y += DATA_LINE_H;
|
||
|
||
/* 第八行：真空泵状态 */
|
||
slen = zf_sprintf(str_buf, "VAC:%s %d%%",
|
||
vac_enabled ? "ON " : "OFF",
|
||
(int16)test_user_value[2]);
|
||
user_menu_show_padded_data_line(y, str_buf, sizeof(str_buf), slen);
|
||
y += DATA_LINE_H;
|
||
}
|
||
|
||
/*============================================================
|
||
* 绘制参数表格页面
|
||
* 显示可编辑的参数列表，当前行高亮显示
|
||
*============================================================*/
|
||
void user_menu_draw_param_table(void)
|
||
{
|
||
char str_buf[24];
|
||
uint32 slen;
|
||
uint16 y = PARAM_START_Y;
|
||
int16 i = 0;
|
||
uint8 j;
|
||
|
||
/* 用背景色填充参数区域，高效清除旧内容 */
|
||
ips200_set_color(COLOR_BG, COLOR_BG);
|
||
for(i = 0; i < 5; i++)
|
||
{
|
||
ips200_draw_line(0, (int16)(PARAM_START_Y + i * 32),
|
||
SCREEN_WIDTH - 1, (int16)(PARAM_START_Y + i * 32),
|
||
COLOR_BG);
|
||
}
|
||
|
||
/* 逐行绘制参数 */
|
||
for(i = 0; i < PARAM_PER_PAGE && (i + param_scroll_offset) < PARAM_TOTAL_NUM; i++)
|
||
{
|
||
int16 param_idx = i + param_scroll_offset;
|
||
|
||
/* 高亮当前选中的行 */
|
||
if(param_idx == param_selected)
|
||
{
|
||
ips200_set_color(COLOR_HIGHLIGHT, COLOR_BG);
|
||
ips200_show_string(0, y, "> ");
|
||
|
||
slen = zf_sprintf(str_buf, "%2d:", param_idx + 1);
|
||
str_buf[slen] = '\0';
|
||
ips200_show_string(20, y, str_buf);
|
||
|
||
ips200_show_string(45, y, param_name[param_idx]);
|
||
|
||
get_param_value_str(param_idx, str_buf, sizeof(str_buf));
|
||
/* 追加空格覆盖旧文字残影 */
|
||
for(j = 0; str_buf[j]; j++) ;
|
||
while(j < 9) str_buf[j++] = ' ';
|
||
str_buf[j] = '\0';
|
||
ips200_show_string(150, y, str_buf);
|
||
}
|
||
else
|
||
{
|
||
ips200_set_color(COLOR_TEXT, COLOR_BG);
|
||
|
||
slen = zf_sprintf(str_buf, "%2d:", param_idx + 1);
|
||
str_buf[slen] = '\0';
|
||
ips200_show_string(0, y, str_buf);
|
||
|
||
/* 名称 x=30 避免与编号重叠（编号3字符×8px=24px） */
|
||
ips200_show_string(30, y, param_name[param_idx]);
|
||
|
||
get_param_value_str(param_idx, str_buf, sizeof(str_buf));
|
||
for(j = 0; str_buf[j]; j++) ;
|
||
while(j < 9) str_buf[j++] = ' ';
|
||
str_buf[j] = '\0';
|
||
ips200_show_string(135, y, str_buf);
|
||
}
|
||
|
||
y += PARAM_LINE_H;
|
||
}
|
||
}
|
||
|
||
/*============================================================
|
||
* 绘制参数设置页面标题
|
||
* 页面上方显示标题栏和操作提示
|
||
*============================================================*/
|
||
void user_menu_draw_param_page(void)
|
||
{
|
||
ips200_set_color(COLOR_TEXT, COLOR_BG);
|
||
|
||
/* 绘制标题栏 */
|
||
ips200_show_string(0, PARAM_PAGE_TITLE_Y,
|
||
"PARAM SET "
|
||
"K1:- K2:+ K3:Sel");
|
||
|
||
/* 标题下方画一条分隔线 */
|
||
ips200_draw_line(0, 16, SCREEN_WIDTH - 1, 16, COLOR_BAR_BG);
|
||
|
||
/* 绘制参数表格 */
|
||
user_menu_draw_param_table();
|
||
}
|
||
|
||
/*============================================================
|
||
* 页面切换
|
||
* 清除屏幕，切换到目标页面
|
||
*============================================================*/
|
||
void user_menu_switch_page(void)
|
||
{
|
||
if(current_page == MENU_PAGE_REALTIME)
|
||
{
|
||
/* 切换到参数设置页 */
|
||
current_page = MENU_PAGE_PARAMETER;
|
||
ips200_clear();
|
||
user_menu_draw_param_page();
|
||
}
|
||
else
|
||
{
|
||
/* 切换回实时状态页 */
|
||
current_page = MENU_PAGE_REALTIME;
|
||
ips200_clear();
|
||
}
|
||
}
|
||
|
||
/*============================================================
|
||
* 按键处理函数
|
||
*
|
||
* 按键功能定义：
|
||
* 页面0 (实时状态)：
|
||
* KEY4 - 切换到参数设置页
|
||
*
|
||
* 页面1 (参数设置)：
|
||
* KEY1 - 当前参数值减少
|
||
* KEY2 - 当前参数值增加
|
||
* KEY3 - 选择上一个参数
|
||
* KEY4 - 切换到实时状态页
|
||
*============================================================*/
|
||
void user_menu_key_dispose(void)
|
||
{
|
||
/* ---- KEY4: 页面切换 ---- */
|
||
if(key_get_state(KEY_4) == KEY_SHORT_PRESS)
|
||
{
|
||
key_clear_state(KEY_4);
|
||
user_menu_switch_page();
|
||
}
|
||
|
||
/* ---- 页面0(实时页)的按键处理 ---- */
|
||
if(current_page == MENU_PAGE_REALTIME)
|
||
{
|
||
/* KEY3: 真空泵 开/关 切换 */
|
||
if(key_get_state(KEY_3) == KEY_SHORT_PRESS)
|
||
{
|
||
key_clear_state(KEY_3);
|
||
vac_enabled = !vac_enabled;
|
||
user_menu_draw_realtime_data();
|
||
}
|
||
}
|
||
|
||
/* ---- 页面1的按键处理 ---- */
|
||
if(current_page == MENU_PAGE_PARAMETER)
|
||
{
|
||
/* KEY3: 选择下一个参数（自上而下） */
|
||
if(key_get_state(KEY_3) == KEY_SHORT_PRESS)
|
||
{
|
||
key_clear_state(KEY_3);
|
||
|
||
param_selected++;
|
||
if(param_selected >= PARAM_TOTAL_NUM)
|
||
{
|
||
param_selected = 0;
|
||
}
|
||
|
||
/* 滚动调整：如果选中行移出可视范围，滚动偏移量跟随 */
|
||
if(param_selected < param_scroll_offset)
|
||
{
|
||
param_scroll_offset = param_selected;
|
||
}
|
||
else if(param_selected >= param_scroll_offset + PARAM_PER_PAGE)
|
||
{
|
||
param_scroll_offset = param_selected - PARAM_PER_PAGE + 1;
|
||
}
|
||
|
||
user_menu_draw_param_table();
|
||
}
|
||
|
||
/* KEY1: 参数值减少 */
|
||
if(key_get_state(KEY_1) == KEY_SHORT_PRESS)
|
||
{
|
||
key_clear_state(KEY_1);
|
||
|
||
switch(param_selected)
|
||
{
|
||
case 0:
|
||
{
|
||
/* WiFi: 重置连接状态 */
|
||
wifi_state = 0;
|
||
wifi_request_flag = 0;
|
||
wifi_init_done = 0;
|
||
zf_sprintf(wifi_status_str, "Idle");
|
||
}break;
|
||
case 1:
|
||
{
|
||
if(-- pid_switch_num < 0)
|
||
pid_switch_num = PID_TUNING_LOOP_COUNT - 1;
|
||
}break;
|
||
case 2: test_pid_value[pid_switch_num][0] -= 0.1; break;
|
||
case 3: test_pid_value[pid_switch_num][1] -= 0.1; break;
|
||
case 4: test_pid_value[pid_switch_num][2] -= 0.1; break;
|
||
case 5: test_user_value[1] -= 5; break; /* 基础速度 */
|
||
case 6: test_user_value[2] -= 5; break; /* 负压占空比 */
|
||
case 7: test_user_value[3] -= 10; break; /* 最大时速 */
|
||
case 8: test_user_value[4] -= 5; break; /* 弯道补偿 */
|
||
default: break;
|
||
}
|
||
|
||
/* 刷新参数表格显示 */
|
||
user_menu_draw_param_table();
|
||
}
|
||
|
||
/* KEY2: 参数值增加 */
|
||
if(key_get_state(KEY_2) == KEY_SHORT_PRESS)
|
||
{
|
||
key_clear_state(KEY_2);
|
||
|
||
switch(param_selected)
|
||
{
|
||
case 0:
|
||
{
|
||
/* WiFi: 触发连接 (仅在空闲或失败时可重试) */
|
||
if(wifi_state != 2)
|
||
wifi_request_flag = 1;
|
||
}break;
|
||
case 1:
|
||
{
|
||
if(++ pid_switch_num >= PID_TUNING_LOOP_COUNT)
|
||
pid_switch_num = 0;
|
||
}break;
|
||
case 2: test_pid_value[pid_switch_num][0] += 0.1; break;
|
||
case 3: test_pid_value[pid_switch_num][1] += 0.1; break;
|
||
case 4: test_pid_value[pid_switch_num][2] += 0.1; break;
|
||
case 5: test_user_value[1] += 5; break; /* 基础速度 */
|
||
case 6: test_user_value[2] += 5; break; /* 负压占空比 */
|
||
case 7: test_user_value[3] += 10; break; /* 最大时速 */
|
||
case 8: test_user_value[4] += 5; break; /* 弯道补偿 */
|
||
default: break;
|
||
}
|
||
|
||
user_menu_draw_param_table();
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
/* WiFi 分块非阻塞发送: 启动新一次图像传输 */
|
||
static void wifi_chunked_send_start(void)
|
||
{
|
||
/* 构建逐飞助手协议帧头 (8 bytes) */
|
||
wifi_header[0] = 0xAA;
|
||
wifi_header[1] = 0x02;
|
||
wifi_header[2] = (uint8)((SEEKFREE_ASSISTANT_MT9V03X << 5) | (0 << 4) | 0);
|
||
wifi_header[3] = 8;
|
||
wifi_header[4] = (uint8)(MT9V03X_1_W & 0xFF);
|
||
wifi_header[5] = (uint8)(MT9V03X_1_W >> 8);
|
||
wifi_header[6] = (uint8)(MT9V03X_1_H & 0xFF);
|
||
wifi_header[7] = (uint8)(MT9V03X_1_H >> 8);
|
||
|
||
wifi_chunk_send_active = 1;
|
||
wifi_chunk_send_phase = 0;
|
||
wifi_chunk_ptr = (uint8 *)camera_frame_buffer;
|
||
wifi_chunk_remaining = (uint32)(MT9V03X_1_W * MT9V03X_1_H);
|
||
}
|
||
|
||
/* WiFi 分块非阻塞发送: 每帧发送最多 WIFI_CHUNKS_PER_FRAME 个 SPI 块 */
|
||
static uint8 wifi_chunked_send_poll(void)
|
||
{
|
||
uint32 send_len;
|
||
uint32 result;
|
||
uint8 chunks_sent = 0;
|
||
|
||
if (!wifi_chunk_send_active) return 1;
|
||
|
||
/* 阶段0: 发送协议帧头 (8 bytes) */
|
||
if (wifi_chunk_send_phase == 0)
|
||
{
|
||
result = wifi_spi_send_buffer(wifi_header, 8);
|
||
if (result != 0) return 0;
|
||
wifi_chunk_send_phase = 1;
|
||
chunks_sent++;
|
||
}
|
||
|
||
/* 阶段1: 发送图像数据块，每块最多 4088 字节 */
|
||
while (wifi_chunk_remaining > 0 && chunks_sent < WIFI_CHUNKS_PER_FRAME)
|
||
{
|
||
send_len = wifi_chunk_remaining > WIFI_CHUNK_SIZE ? WIFI_CHUNK_SIZE : wifi_chunk_remaining;
|
||
result = wifi_spi_send_buffer(wifi_chunk_ptr, send_len);
|
||
if (result != 0) return 0;
|
||
wifi_chunk_ptr += send_len;
|
||
wifi_chunk_remaining -= send_len;
|
||
chunks_sent++;
|
||
}
|
||
|
||
if (wifi_chunk_remaining == 0)
|
||
{
|
||
wifi_chunk_send_active = 0;
|
||
return 1;
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
/*============================================================
|
||
* 菜单主循环
|
||
* 每次摄像头帧完成时调用一次
|
||
* 包含：按键处理、数据采集、界面刷新
|
||
*============================================================*/
|
||
void user_menu_loop(void)
|
||
{
|
||
/* 采集电池电压（均值滤波 5 次） */
|
||
battery_voltage_gather(adc_mean_filter_convert(VOLTAGE_PORT, 5));
|
||
|
||
/* 扫描按键状态 */
|
||
key_scanner();
|
||
|
||
/* 处理按键事件 */
|
||
user_menu_key_dispose();
|
||
|
||
/* 真空泵 PWM 实时控制 (Vac Duty 参数 0-100%, vac_enabled 控制开关) */
|
||
{
|
||
uint32 vac_duty = vac_enabled ? (uint32)(test_user_value[2] * 100.0f) : 0;
|
||
if(vac_duty > 10000) vac_duty = 10000;
|
||
pwm_set_duty(VACUUM_PWM, vac_duty);
|
||
}
|
||
|
||
/* 根据当前页面刷新显示 */
|
||
if(current_page == MENU_PAGE_REALTIME)
|
||
{
|
||
/* ======== 页面0: 实时状态显示 ======== */
|
||
|
||
/* 绘制顶部状态栏 */
|
||
user_menu_draw_status_bar();
|
||
|
||
/* 绘制摄像头二值化图像 (大津法阈值) */
|
||
user_menu_draw_camera_binary();
|
||
|
||
/* 绘制实时数据 */
|
||
user_menu_draw_realtime_data();
|
||
|
||
/* WiFi 示波器数据发送 — 发送实时调试数据到逐飞助手虚拟示波器 */
|
||
if(wifi_state == 2)
|
||
{
|
||
/* 通道0: 编码器1 (左轮速度) */
|
||
seekfree_assistant_oscilloscope_data.data[0] = (float)encoder_data_1;
|
||
/* 通道1: 编码器2 (右轮速度) */
|
||
seekfree_assistant_oscilloscope_data.data[1] = (float)encoder_data_2;
|
||
/* 通道2: 偏航角 Yaw */
|
||
seekfree_assistant_oscilloscope_data.data[2] = eulerAngle.yaw;
|
||
/* 通道3: 电池电压 */
|
||
seekfree_assistant_oscilloscope_data.data[3] = battery_voltage;
|
||
/* 通道4: 左轮 PWM 输出 */
|
||
seekfree_assistant_oscilloscope_data.data[4] = g_last_pwm_left;
|
||
/* 通道5: 右轮 PWM 输出 */
|
||
seekfree_assistant_oscilloscope_data.data[5] = g_last_pwm_right;
|
||
/* 通道6: 目标速度 */
|
||
seekfree_assistant_oscilloscope_data.data[6] = g_target_speed;
|
||
/* 通道7: 俯仰角 Pitch */
|
||
seekfree_assistant_oscilloscope_data.data[7] = eulerAngle.pitch;
|
||
|
||
seekfree_assistant_oscilloscope_data.channel_num = 8;
|
||
seekfree_assistant_oscilloscope_send(&seekfree_assistant_oscilloscope_data);
|
||
|
||
seekfree_assistant_data_analysis();
|
||
}
|
||
}
|
||
else
|
||
{
|
||
/* ======== 页面1: 参数设置页 ======== */
|
||
/* 如需额外刷新，在此添加 */
|
||
}
|
||
|
||
/* ---- WiFi 连接请求处理 (在按键处理后执行) ---- */
|
||
if(wifi_request_flag)
|
||
{
|
||
wifi_request_flag = 0;
|
||
wifi_state = 1;
|
||
|
||
/* 第一步：初始化 WiFi 模块（仅首次，避免重复硬件复位） */
|
||
if(!wifi_init_done)
|
||
{
|
||
{
|
||
uint32 slen_ = zf_sprintf(wifi_status_str, "WiFi...");
|
||
wifi_status_str[slen_] = '\0';
|
||
}
|
||
user_menu_draw_param_table();
|
||
|
||
if(wifi_spi_init(WIFI_SSID, WIFI_PASSWORD))
|
||
{
|
||
wifi_state = 3;
|
||
{
|
||
uint32 slen_ = zf_sprintf(wifi_status_str, "WErr");
|
||
wifi_status_str[slen_] = '\0';
|
||
}
|
||
user_menu_draw_param_table();
|
||
return;
|
||
}
|
||
|
||
wifi_init_done = 1;
|
||
|
||
/* 打印模块信息便于调试 */
|
||
printf("\r\n[WiFi] Ver:%s MAC:%s", wifi_spi_version, wifi_spi_mac_addr);
|
||
printf("\r\n[WiFi] IP:%s", wifi_spi_ip_addr_port);
|
||
|
||
/* 等待 WiFi 模块完全连上路由器并获取 IP，再继续后续操作 */
|
||
system_delay_ms(1000);
|
||
|
||
/* 第二步：配置逐飞助手传输接口（仅首次） */
|
||
{
|
||
uint32 slen_ = zf_sprintf(wifi_status_str, "Asst");
|
||
wifi_status_str[slen_] = '\0';
|
||
}
|
||
user_menu_draw_param_table();
|
||
seekfree_assistant_interface_init(SEEKFREE_ASSISTANT_WIFI_SPI);
|
||
seekfree_assistant_camera_information_config(
|
||
SEEKFREE_ASSISTANT_MT9V03X,
|
||
(const uint8 *)camera_frame_buffer,
|
||
MT9V03X_1_W, MT9V03X_1_H);
|
||
}
|
||
|
||
/* 第三步：连接电脑端 TCP Server（带重试） */
|
||
{
|
||
uint32 slen_ = zf_sprintf(wifi_status_str, "TCP...");
|
||
wifi_status_str[slen_] = '\0';
|
||
}
|
||
user_menu_draw_param_table();
|
||
|
||
{
|
||
int8 tcp_retry = 0;
|
||
while(wifi_spi_socket_connect("TCP",
|
||
WIFI_SPI_TARGET_IP,
|
||
WIFI_SPI_TARGET_PORT,
|
||
WIFI_SPI_LOCAL_PORT))
|
||
{
|
||
tcp_retry++;
|
||
if(tcp_retry >= 10)
|
||
{
|
||
wifi_state = 3;
|
||
{
|
||
uint32 slen_ = zf_sprintf(wifi_status_str, "TCPFail");
|
||
wifi_status_str[slen_] = '\0';
|
||
}
|
||
break;
|
||
}
|
||
system_delay_ms(500);
|
||
}
|
||
|
||
if(tcp_retry < 10)
|
||
{
|
||
wifi_state = 2;
|
||
{
|
||
uint32 slen_ = zf_sprintf(wifi_status_str, "OK");
|
||
wifi_status_str[slen_] = '\0';
|
||
}
|
||
}
|
||
}
|
||
|
||
user_menu_draw_param_table();
|
||
}
|
||
}
|
||
|
||
/*============================================================
|
||
* 菜单初始化
|
||
* 初始化 IPS200 显示屏，设置显示方向、字体和颜色
|
||
*============================================================*/
|
||
void user_menu_init(void)
|
||
|
||
{
|
||
/* 初始化 IPS200 显示屏 (根据硬件配置选择 SPI 或 并口) */
|
||
/* 如需修改接口类型，请在 user_menu_device.h 中修改 IPS200_HARDWARE_TYPE */
|
||
ips200_init(IPS200_HARDWARE_TYPE);
|
||
/* 设置屏幕为竖屏模式 */
|
||
ips200_set_dir(IPS200_PORTAIT);
|
||
|
||
/* 设置默认字体为 8x16 */
|
||
ips200_set_font(IPS200_8X16_FONT);
|
||
|
||
/* 设置默认颜色：白字黑底 */
|
||
ips200_set_color(COLOR_TEXT, COLOR_BG);
|
||
|
||
|
||
|
||
/* 清除屏幕，准备显示 */
|
||
ips200_clear();
|
||
|
||
/* 重置菜单状态变量 */
|
||
current_page = MENU_PAGE_REALTIME;
|
||
param_selected = 0;
|
||
param_scroll_offset = 0;
|
||
|
||
/* 显示初始页面 */
|
||
user_menu_draw_status_bar();
|
||
}
|