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
This commit is contained in:
32343 committed
commit
843447835d
660 files changed
+443632
No files matched your search
@@ -0,0 +1,270 @@
|
||||
/*********************************************************************************************************************
|
||||
* TC264 智能车 - 姿态解算模块 (attitude_solution.c)
|
||||
*
|
||||
* 基于 Mahony 互补滤波算法：
|
||||
* 陀螺仪积分得到姿态角，加速度计修正陀螺零漂。
|
||||
* 使用四元数表示姿态，避免万向节死锁。
|
||||
* 移植自 RT1064 开源库中 ICM_AHRSupdate 实现。
|
||||
*
|
||||
* 调用周期: 5ms (在 isr.c 的 CCU60_CH0 中断中调用, 陀螺仪校准完成后)
|
||||
*
|
||||
* IMU660RB 转换因子 (2000DPS / 8G):
|
||||
* 陀螺仪: raw / 14.3 = °/s, * π/180 = rad/s
|
||||
* 加速度: raw / 4098 = G (m/s^2单位)
|
||||
*********************************************************************************************************************/
|
||||
|
||||
#include "zf_common_typedef.h"
|
||||
#include "zf_driver_delay.h"
|
||||
#include "zf_device_imu660rb.h"
|
||||
#include "attitude_solution.h"
|
||||
|
||||
/* ---------------------------------------------------------------------------
|
||||
* 常数与可调参数 (匹配 RT1064 参考实现)
|
||||
* --------------------------------------------------------------------------- */
|
||||
#define DELTA_T 0.005f /* 姿态更新周期 (秒), 匹配 5ms ISR */
|
||||
#define DEG_TO_RAD 0.01745329252f /* π / 180 */
|
||||
#define RAD_TO_DEG 57.2957795131f /* 180 / π */
|
||||
|
||||
#define KP 0.17f /* 加速度计收敛比例增益 (同 RT1064) */
|
||||
#define KI 0.004f /* 陀螺仪零漂积分增益 (同 RT1064) */
|
||||
#define ACC_ALPHA 0.3f /* 加速度低通滤波系数 (同 RT1064) */
|
||||
|
||||
/* ---------------------------------------------------------------------------
|
||||
* 全局姿态角 (外部引用)
|
||||
* --------------------------------------------------------------------------- */
|
||||
euler_param_t eulerAngle;
|
||||
|
||||
/* ---------------------------------------------------------------------------
|
||||
* 静态变量 — 四元数 & 误差积分 & 加速度滤波状态
|
||||
* --------------------------------------------------------------------------- */
|
||||
static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
|
||||
static float i_ex = 0.0f, i_ey = 0.0f, i_ez = 0.0f;
|
||||
static float acc_fx = 0.0f, acc_fy = 0.0f, acc_fz = 0.0f;
|
||||
|
||||
/* ---------------------------------------------------------------------------
|
||||
* 快速反平方根 (Quake III 算法, 两次牛顿迭代)
|
||||
* --------------------------------------------------------------------------- */
|
||||
static float fast_rsqrt(float x)
|
||||
{
|
||||
float halfx = 0.5f * x;
|
||||
float y = x;
|
||||
long i = *(long *)&y;
|
||||
i = 0x5f3759df - (i >> 1);
|
||||
y = *(float *)&i;
|
||||
y = y * (1.5f - halfx * y * y);
|
||||
y = y * (1.5f - halfx * y * y);
|
||||
return y;
|
||||
}
|
||||
|
||||
/* ---------------------------------------------------------------------------
|
||||
* 姿态解算初始化
|
||||
* 多次读取加速度计取平均, 用加速度计计算初始 pitch/roll, 设置正确的初始四元数
|
||||
* --------------------------------------------------------------------------- */
|
||||
void attitude_init(void)
|
||||
{
|
||||
float ax_sum, ay_sum, az_sum;
|
||||
float ax, ay, az;
|
||||
float norm, mag;
|
||||
float roll_init, pitch_init;
|
||||
float cr, sr, cp, sp;
|
||||
int i;
|
||||
|
||||
/* 传感器上电后需要时间稳定, 丢弃前若干次读数 */
|
||||
for(i = 0; i < 10; i++)
|
||||
{
|
||||
imu660rb_get_acc();
|
||||
system_delay_ms(5);
|
||||
}
|
||||
|
||||
/* 多次采样取平均, 提高初始姿态估计的稳健性 */
|
||||
ax_sum = 0.0f; ay_sum = 0.0f; az_sum = 0.0f;
|
||||
for(i = 0; i < 20; i++)
|
||||
{
|
||||
imu660rb_get_acc();
|
||||
ax_sum += imu660rb_acc_transition(imu660rb_acc_x);
|
||||
ay_sum += imu660rb_acc_transition(imu660rb_acc_y);
|
||||
az_sum += imu660rb_acc_transition(imu660rb_acc_z);
|
||||
system_delay_ms(5);
|
||||
}
|
||||
ax = ax_sum / 20.0f;
|
||||
ay = ay_sum / 20.0f;
|
||||
az = az_sum / 20.0f;
|
||||
|
||||
/* 校验加速度幅值是否在合理范围 (0.6G ~ 1.6G), 若异常则回退到默认值 */
|
||||
mag = ax * ax + ay * ay + az * az;
|
||||
if(mag < 0.36f || mag > 2.56f)
|
||||
{
|
||||
ax = 0.0f;
|
||||
ay = 0.0f;
|
||||
az = 1.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
norm = fast_rsqrt(mag);
|
||||
ax *= norm;
|
||||
ay *= norm;
|
||||
az *= norm;
|
||||
}
|
||||
|
||||
/* 从重力矢量估计初始 roll 和 pitch */
|
||||
roll_init = atan2f(ay, az);
|
||||
pitch_init = atan2f(-ax, sqrtf(ay * ay + az * az));
|
||||
|
||||
/* 欧拉角 -> 四元数 */
|
||||
cr = cosf(roll_init * 0.5f);
|
||||
sr = sinf(roll_init * 0.5f);
|
||||
cp = cosf(pitch_init * 0.5f);
|
||||
sp = sinf(pitch_init * 0.5f);
|
||||
|
||||
q0 = cr * cp;
|
||||
q1 = sr * cp;
|
||||
q2 = cr * sp;
|
||||
q3 = -sr * sp;
|
||||
|
||||
/* 四元数归一化 */
|
||||
norm = fast_rsqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
|
||||
q0 *= norm;
|
||||
q1 *= norm;
|
||||
q2 *= norm;
|
||||
q3 *= norm;
|
||||
|
||||
/* 复位积分项 */
|
||||
i_ex = 0.0f; i_ey = 0.0f; i_ez = 0.0f;
|
||||
|
||||
/* 加速度滤波状态初始化为首次有效读数 (避免从零开始的衰减过程) */
|
||||
acc_fx = ax;
|
||||
acc_fy = ay;
|
||||
acc_fz = az;
|
||||
|
||||
/* 根据初始四元数计算初始欧拉角 */
|
||||
{
|
||||
float q0q0 = q0 * q0;
|
||||
float q0q1 = q0 * q1;
|
||||
float q0q2 = q0 * q2;
|
||||
float q0q3 = q0 * q3;
|
||||
float q1q1 = q1 * q1;
|
||||
float q1q2 = q1 * q2;
|
||||
float q1q3 = q1 * q3;
|
||||
float q2q2 = q2 * q2;
|
||||
float q2q3 = q2 * q3;
|
||||
float q3q3 = q3 * q3;
|
||||
|
||||
eulerAngle.pitch = asinf(-2.0f * q1q3 + 2.0f * q0q2) * RAD_TO_DEG;
|
||||
eulerAngle.roll = atan2f(2.0f * q2q3 + 2.0f * q0q1,
|
||||
-2.0f * q1q1 - 2.0f * q2q2 + 1.0f) * RAD_TO_DEG;
|
||||
eulerAngle.yaw = 0.0f; /* 无磁力计，yaw 从 0 开始 */
|
||||
}
|
||||
}
|
||||
|
||||
/* ---------------------------------------------------------------------------
|
||||
* 姿态更新 — 每 5ms 调用一次 (陀螺仪校准完成后)
|
||||
* 读取已去除零偏的陀螺仪数据 + 原始加速度计数据,
|
||||
* 运行 Mahony 互补滤波, 输出欧拉角
|
||||
* --------------------------------------------------------------------------- */
|
||||
void attitude_update(void)
|
||||
{
|
||||
float gx, gy, gz;
|
||||
float ax, ay, az;
|
||||
float norm;
|
||||
float vx, vy, vz;
|
||||
float ex, ey, ez;
|
||||
float halfT = 0.5f * DELTA_T;
|
||||
|
||||
/* --- 1. 获取传感器数据并转换为物理量 --- */
|
||||
/* 陀螺仪: ISR 已减零偏 (LSB) -> °/s -> rad/s, 量程因子由驱动层统一管理 */
|
||||
gx = imu660rb_gyro_transition(imu660rb_gyro_x) * DEG_TO_RAD;
|
||||
gy = imu660rb_gyro_transition(imu660rb_gyro_y) * DEG_TO_RAD;
|
||||
gz = imu660rb_gyro_transition(imu660rb_gyro_z) * DEG_TO_RAD;
|
||||
|
||||
/* 加速度计: LSB -> G, 一阶低通滤波抑制振动噪声 (同 RT1064 alpha=0.3) */
|
||||
ax = imu660rb_acc_transition(imu660rb_acc_x);
|
||||
ay = imu660rb_acc_transition(imu660rb_acc_y);
|
||||
az = imu660rb_acc_transition(imu660rb_acc_z);
|
||||
acc_fx = ACC_ALPHA * ax + (1.0f - ACC_ALPHA) * acc_fx;
|
||||
acc_fy = ACC_ALPHA * ay + (1.0f - ACC_ALPHA) * acc_fy;
|
||||
acc_fz = ACC_ALPHA * az + (1.0f - ACC_ALPHA) * acc_fz;
|
||||
ax = acc_fx;
|
||||
ay = acc_fy;
|
||||
az = acc_fz;
|
||||
|
||||
/* --- 2. 加速度归一化 (单位重力向量) --- */
|
||||
norm = fast_rsqrt(ax * ax + ay * ay + az * az);
|
||||
ax *= norm;
|
||||
ay *= norm;
|
||||
az *= norm;
|
||||
|
||||
/* --- 3. 估算重力分量 (从当前四元数) --- */
|
||||
vx = 2.0f * (q1 * q3 - q0 * q2);
|
||||
vy = 2.0f * (q0 * q1 + q2 * q3);
|
||||
vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
|
||||
|
||||
/* --- 3.5 重力方向检测: 若估计重力与实测重力反平行 (点积<0),
|
||||
* 则四元数处于翻转平衡点, 绕X轴旋转180°拉回正确方向 --- */
|
||||
if(ax * vx + ay * vy + az * vz < 0.0f)
|
||||
{
|
||||
float t0 = -q1, t1 = q0, t2 = q3, t3 = -q2;
|
||||
q0 = t0; q1 = t1; q2 = t2; q3 = t3;
|
||||
vx = 2.0f * (q1 * q3 - q0 * q2);
|
||||
vy = 2.0f * (q0 * q1 + q2 * q3);
|
||||
vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
|
||||
}
|
||||
|
||||
/* --- 4. 叉积求重力误差 --- */
|
||||
ex = ay * vz - az * vy;
|
||||
ey = az * vx - ax * vz;
|
||||
ez = ax * vy - ay * vx;
|
||||
|
||||
/* --- 5. PI 修正陀螺仪 --- */
|
||||
i_ex += halfT * ex;
|
||||
i_ey += halfT * ey;
|
||||
i_ez += halfT * ez;
|
||||
gx += KP * ex + KI * i_ex;
|
||||
gy += KP * ey + KI * i_ey;
|
||||
gz += KP * ez + KI * i_ez;
|
||||
|
||||
/* --- 6. 一阶龙格库塔更新四元数 --- */
|
||||
{
|
||||
float dq0 = -q1 * gx - q2 * gy - q3 * gz;
|
||||
float dq1 = q0 * gx + q2 * gz - q3 * gy;
|
||||
float dq2 = q0 * gy - q1 * gz + q3 * gx;
|
||||
float dq3 = q0 * gz + q1 * gy - q2 * gx;
|
||||
|
||||
q0 += dq0 * halfT;
|
||||
q1 += dq1 * halfT;
|
||||
q2 += dq2 * halfT;
|
||||
q3 += dq3 * halfT;
|
||||
}
|
||||
|
||||
/* --- 7. 四元数归一化 --- */
|
||||
norm = fast_rsqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
|
||||
q0 *= norm;
|
||||
q1 *= norm;
|
||||
q2 *= norm;
|
||||
q3 *= norm;
|
||||
|
||||
/* --- 8. 四元数 -> 欧拉角 (°) --- */
|
||||
{
|
||||
float q0q0 = q0 * q0;
|
||||
float q0q1 = q0 * q1;
|
||||
float q0q2 = q0 * q2;
|
||||
float q0q3 = q0 * q3;
|
||||
float q1q1 = q1 * q1;
|
||||
float q1q2 = q1 * q2;
|
||||
float q1q3 = q1 * q3;
|
||||
float q2q2 = q2 * q2;
|
||||
float q2q3 = q2 * q3;
|
||||
float q3q3 = q3 * q3;
|
||||
|
||||
eulerAngle.pitch = asinf(-2.0f * q1q3 + 2.0f * q0q2) * RAD_TO_DEG;
|
||||
eulerAngle.roll = atan2f(2.0f * q2q3 + 2.0f * q0q1,
|
||||
-2.0f * q1q1 - 2.0f * q2q2 + 1.0f) * RAD_TO_DEG;
|
||||
eulerAngle.yaw = atan2f(2.0f * q1q2 + 2.0f * q0q3,
|
||||
-2.0f * q2q2 - 2.0f * q3q3 + 1.0f) * RAD_TO_DEG;
|
||||
}
|
||||
|
||||
/* --- 9. Yaw 归一化到 [0, 360) --- */
|
||||
if (eulerAngle.yaw >= 360.0f)
|
||||
eulerAngle.yaw -= 360.0f;
|
||||
else if (eulerAngle.yaw < 0.0f)
|
||||
eulerAngle.yaw += 360.0f;
|
||||
}
|
||||
@@ -0,0 +1,17 @@
|
||||
#ifndef _ATTITUDE_SOLUTION_H_
|
||||
#define _ATTITUDE_SOLUTION_H_
|
||||
|
||||
#include "zf_common_typedef.h"
|
||||
|
||||
typedef struct {
|
||||
float pitch;
|
||||
float roll;
|
||||
float yaw;
|
||||
} euler_param_t;
|
||||
|
||||
extern euler_param_t eulerAngle;
|
||||
|
||||
void attitude_init (void);
|
||||
void attitude_update (void);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,347 @@
|
||||
/*********************************************************************************************************************
|
||||
* TC264 智能车 - 四轮差速速度/转向PID控制模块 (pid_control.c)
|
||||
*
|
||||
* 适用: 四轮小车, 两个电机各驱动一侧两个轮子 (差速转向)
|
||||
* 传感器: 2个编码器 + 1个陀螺仪(IMU) + 摄像头
|
||||
*
|
||||
* 控制结构:
|
||||
* 前向运动:
|
||||
* 目标速度 → 左/右速度环(内环) → 左/右PWM基础值
|
||||
*
|
||||
* 并行 (转向):
|
||||
* 转向环 → PWM差速值 (陀螺仪 yaw rate 作为 D 项阻尼)
|
||||
*
|
||||
* 最终:
|
||||
* 左轮PWM = 左速度环输出 - 转向环输出
|
||||
* 右轮PWM = 右速度环输出 + 转向环输出
|
||||
*
|
||||
* 调用周期:
|
||||
* 速度环: 100ms (CCU61_CH0 中断)
|
||||
* 转向环: 帧周期 (cpu0_main 主循环, 或 5ms ISR 中更新 gyro D 项)
|
||||
*********************************************************************************************************************/
|
||||
|
||||
#include "zf_common_headfile.h"
|
||||
#include "pid_control.h"
|
||||
|
||||
/* ==================== PID 句柄 ==================== */
|
||||
pid_t g_pid_speed_left;
|
||||
pid_t g_pid_speed_right;
|
||||
pid_t g_pid_steering;
|
||||
|
||||
/* ==================== 控制使能 ==================== */
|
||||
bool g_control_enabled = FALSE;
|
||||
|
||||
/* ==================== 级联变量 ==================== */
|
||||
|
||||
/* 目标速度 (Base Spd → 速度环输入), 单位: 编码器脉冲/100ms */
|
||||
float g_target_speed = 0.0f;
|
||||
|
||||
/* 最终下发到左右电机的 PWM 命令 (含死区补偿和转向差速) */
|
||||
float g_motor_pwm_left_cmd = 0.0f;
|
||||
float g_motor_pwm_right_cmd = 0.0f;
|
||||
|
||||
/* 陀螺仪 z 轴角速度 (°/s), 在 5ms ISR 中更新, 供转向环 D 项使用 */
|
||||
float g_gyro_z_dps = 0.0f;
|
||||
|
||||
/* 转向差速分量 (转向环输出), 单位: PWM占空比 (叠加到左右轮) */
|
||||
static float g_steering_offset = 0.0f;
|
||||
|
||||
static int8 g_speed_target_dir_last = 0;
|
||||
|
||||
/* ==================== 电机引脚 ==================== */
|
||||
/*
|
||||
* 左电机: MOTOR_PWM1 = ATOM3_CH5_P11_10, MOTOR_DIR1 = P11_9
|
||||
* 驱动左侧两个轮子
|
||||
* 右电机: MOTOR_PWM2 = ATOM2_CH7_P11_12, MOTOR_DIR2 = P11_11
|
||||
* 驱动右侧两个轮子
|
||||
*
|
||||
* 方向极性 (需实测确认, 以下为默认约定):
|
||||
* 左电机: DIR1=1 前进, DIR1=0 后退
|
||||
* 右电机: DIR2=1 后退, DIR2=0 前进 (对向安装, 极性与左相反)
|
||||
*/
|
||||
|
||||
void motor_set_left(int16 pwm)
|
||||
{
|
||||
int16 abs_pwm;
|
||||
|
||||
if(pwm > PWM_DUTY_MAX_MOTOR) pwm = PWM_DUTY_MAX_MOTOR;
|
||||
if(pwm < -PWM_DUTY_MAX_MOTOR) pwm = -PWM_DUTY_MAX_MOTOR;
|
||||
|
||||
abs_pwm = (pwm >= 0) ? pwm : -pwm;
|
||||
|
||||
if(pwm > 0)
|
||||
{
|
||||
gpio_set_level(MOTOR_DIR2, 1); /* 前进 */
|
||||
pwm_set_duty(MOTOR_PWM2, (uint32)abs_pwm);
|
||||
}
|
||||
else if(pwm < 0)
|
||||
{
|
||||
gpio_set_level(MOTOR_DIR2, 0); /* 后退 */
|
||||
pwm_set_duty(MOTOR_PWM2, (uint32)abs_pwm);
|
||||
}
|
||||
else
|
||||
{
|
||||
pwm_set_duty(MOTOR_PWM2, 0); /* 停止 */
|
||||
}
|
||||
}
|
||||
|
||||
void motor_set_right(int16 pwm)
|
||||
{
|
||||
int16 abs_pwm;
|
||||
|
||||
if(pwm > PWM_DUTY_MAX_MOTOR) pwm = PWM_DUTY_MAX_MOTOR;
|
||||
if(pwm < -PWM_DUTY_MAX_MOTOR) pwm = -PWM_DUTY_MAX_MOTOR;
|
||||
|
||||
abs_pwm = (pwm >= 0) ? pwm : -pwm;
|
||||
|
||||
if(pwm > 0)
|
||||
{
|
||||
gpio_set_level(MOTOR_DIR1, 1); /* 前进 */
|
||||
pwm_set_duty(MOTOR_PWM1, (uint32)abs_pwm);
|
||||
}
|
||||
else if(pwm < 0)
|
||||
{
|
||||
gpio_set_level(MOTOR_DIR1, 0); /* 后退 */
|
||||
pwm_set_duty(MOTOR_PWM1, (uint32)abs_pwm);
|
||||
}
|
||||
else
|
||||
{
|
||||
pwm_set_duty(MOTOR_PWM1, 0); /* 停止 */
|
||||
}
|
||||
}
|
||||
|
||||
static int16 motor_pwm_clamp_int32(int32 pwm)
|
||||
{
|
||||
if(pwm > PWM_DUTY_MAX_MOTOR) pwm = PWM_DUTY_MAX_MOTOR;
|
||||
if(pwm < -PWM_DUTY_MAX_MOTOR) pwm = -PWM_DUTY_MAX_MOTOR;
|
||||
|
||||
return (int16)pwm;
|
||||
}
|
||||
|
||||
static int8 speed_target_dir(float target_speed)
|
||||
{
|
||||
if(target_speed > 1.0f) return 1;
|
||||
if(target_speed < -1.0f) return -1;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int16 speed_pwm_add_deadband(int16 pid_output, int8 target_dir)
|
||||
{
|
||||
int32 pwm_cmd;
|
||||
|
||||
if(target_dir > 0)
|
||||
{
|
||||
pwm_cmd = (int32)MOTOR_PWM_DEADBAND + (int32)pid_output;
|
||||
if(pwm_cmd < 0) pwm_cmd = 0;
|
||||
}
|
||||
else if(target_dir < 0)
|
||||
{
|
||||
pwm_cmd = -(int32)MOTOR_PWM_DEADBAND + (int32)pid_output;
|
||||
if(pwm_cmd > 0) pwm_cmd = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
pwm_cmd = 0;
|
||||
}
|
||||
|
||||
return motor_pwm_clamp_int32(pwm_cmd);
|
||||
}
|
||||
|
||||
/* ==================== PID 核心 ==================== */
|
||||
|
||||
void pid_init(pid_t *p, float kp, float ki, float kd,
|
||||
float out_max, float out_min, float integral_limit, uint8 mode)
|
||||
{
|
||||
p->kp = kp;
|
||||
p->ki = ki;
|
||||
p->kd = kd;
|
||||
p->out_max = out_max;
|
||||
p->out_min = out_min;
|
||||
p->integral_limit = integral_limit;
|
||||
p->mode = mode;
|
||||
p->setpoint = 0.0f;
|
||||
p->feedback = 0.0f;
|
||||
p->error = 0.0f;
|
||||
p->integral = 0.0f;
|
||||
p->prev_error = 0.0f;
|
||||
p->prev_error2 = 0.0f;
|
||||
p->derivative = 0.0f;
|
||||
p->output = 0.0f;
|
||||
}
|
||||
|
||||
float pid_compute(pid_t *p)
|
||||
{
|
||||
p->error = p->setpoint - p->feedback;
|
||||
|
||||
/* 积分分离 */
|
||||
if(p->integral_limit > 0.0f)
|
||||
{
|
||||
float abs_err = (p->error >= 0.0f) ? p->error : -p->error;
|
||||
if(abs_err > p->integral_limit)
|
||||
{
|
||||
p->integral = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
/* 积分累加 + 抗饱和 (仅在 ki != 0 时累加, 避免无效累积) */
|
||||
if(p->ki != 0.0f)
|
||||
{
|
||||
if((p->output < p->out_max) && (p->output > p->out_min))
|
||||
{
|
||||
p->integral += p->error;
|
||||
}
|
||||
}
|
||||
|
||||
/* 微分先行 (仅在 kd != 0 时计算, 避免无效更新) */
|
||||
if(p->kd != 0.0f)
|
||||
{
|
||||
p->derivative = p->feedback - p->prev_error;
|
||||
p->prev_error = p->feedback;
|
||||
}
|
||||
|
||||
/* 位置式 PID */
|
||||
p->output = p->kp * p->error
|
||||
+ p->ki * p->integral
|
||||
- p->kd * p->derivative;
|
||||
|
||||
/* 输出限幅 */
|
||||
if(p->output > p->out_max) p->output = p->out_max;
|
||||
if(p->output < p->out_min) p->output = p->out_min;
|
||||
|
||||
return p->output;
|
||||
}
|
||||
|
||||
/* 增量式 PID:
|
||||
* Δu = kp*(e(k)-e(k-1)) + ki*e(k) + kd*(e(k)-2*e(k-1)+e(k-2))
|
||||
* u(k) = clamp(u(k-1) + Δu)
|
||||
* 天然抗积分饱和, 输出平滑 */
|
||||
float pid_compute_inc(pid_t *p)
|
||||
{
|
||||
float delta;
|
||||
|
||||
p->error = p->setpoint - p->feedback;
|
||||
|
||||
/* 增量式不需要显式积分, ki 项天然隐含积分效果 */
|
||||
delta = p->kp * (p->error - p->prev_error)
|
||||
+ p->ki * p->error
|
||||
+ p->kd * (p->error - 2.0f * p->prev_error + p->prev_error2);
|
||||
|
||||
/* 滚动历史误差 */
|
||||
p->prev_error2 = p->prev_error;
|
||||
p->prev_error = p->error;
|
||||
|
||||
p->output += delta;
|
||||
|
||||
if(p->output > p->out_max) p->output = p->out_max;
|
||||
if(p->output < p->out_min) p->output = p->out_min;
|
||||
|
||||
return p->output;
|
||||
}
|
||||
|
||||
void pid_reset(pid_t *p)
|
||||
{
|
||||
p->integral = 0.0f;
|
||||
p->prev_error = 0.0f;
|
||||
p->prev_error2 = 0.0f;
|
||||
|
||||
/* 增量式复位时清零累积输出, 避免突变 */
|
||||
if(p->mode == 1)
|
||||
{
|
||||
p->output = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
/* ==================== 速度环 (内环, 100ms) ==================== */
|
||||
/*
|
||||
* 输入: enc1_count, enc2_count — 过去100ms编码器脉冲增量
|
||||
* 左右速度独立闭环, enc1 默认对应左侧电机, enc2 默认对应右侧电机
|
||||
* 输出: 直接驱动左右电机 PWM
|
||||
*/
|
||||
void cascade_speed_loop(int16 enc1_count, int16 enc2_count)
|
||||
{
|
||||
float current_speed_left;
|
||||
float current_speed_right;
|
||||
int16 pwm_left;
|
||||
int16 pwm_right;
|
||||
int16 motor_pwm_left;
|
||||
int16 motor_pwm_right;
|
||||
int8 target_dir;
|
||||
|
||||
if(!g_control_enabled) return;
|
||||
|
||||
target_dir = speed_target_dir(g_target_speed);
|
||||
if(target_dir == 0)
|
||||
{
|
||||
pid_reset(&g_pid_speed_left);
|
||||
pid_reset(&g_pid_speed_right);
|
||||
g_speed_target_dir_last = 0;
|
||||
g_motor_pwm_left_cmd = 0.0f;
|
||||
g_motor_pwm_right_cmd = 0.0f;
|
||||
motor_set_left(0);
|
||||
motor_set_right(0);
|
||||
return;
|
||||
}
|
||||
|
||||
if(target_dir != g_speed_target_dir_last)
|
||||
{
|
||||
pid_reset(&g_pid_speed_left);
|
||||
pid_reset(&g_pid_speed_right);
|
||||
g_speed_target_dir_last = target_dir;
|
||||
}
|
||||
|
||||
current_speed_left = (float)enc1_count;
|
||||
current_speed_right = (float)enc2_count;
|
||||
|
||||
g_pid_speed_left.feedback = current_speed_left;
|
||||
g_pid_speed_left.setpoint = g_target_speed;
|
||||
pid_compute_inc(&g_pid_speed_left);
|
||||
|
||||
g_pid_speed_right.feedback = current_speed_right;
|
||||
g_pid_speed_right.setpoint = g_target_speed;
|
||||
pid_compute_inc(&g_pid_speed_right);
|
||||
|
||||
pwm_left = speed_pwm_add_deadband((int16)g_pid_speed_left.output, target_dir);
|
||||
pwm_right = speed_pwm_add_deadband((int16)g_pid_speed_right.output, target_dir);
|
||||
|
||||
motor_pwm_left = motor_pwm_clamp_int32((int32)((float)pwm_left - g_steering_offset));
|
||||
motor_pwm_right = motor_pwm_clamp_int32((int32)((float)pwm_right + g_steering_offset));
|
||||
g_motor_pwm_left_cmd = (float)motor_pwm_left;
|
||||
g_motor_pwm_right_cmd = (float)motor_pwm_right;
|
||||
|
||||
/* 叠加转向差速 → 左右电机 */
|
||||
motor_set_left(motor_pwm_left);
|
||||
motor_set_right(motor_pwm_right);
|
||||
}
|
||||
|
||||
/* ==================== 转向环 (并行, 帧周期) ==================== */
|
||||
/*
|
||||
* 输入: line_deviation — 摄像头中线偏差 (像素), 正=偏右需右转
|
||||
* gyro_z — IMU z轴角速度 (°/s), 用于 D 项阻尼
|
||||
* 输出: g_steering_offset — PWM差速值
|
||||
*
|
||||
* PD控制: P项根据中线偏差产生转向力, D项用gyro_z抑制过冲
|
||||
* 不加 I 项: 转向不需要消除静差 (弯道需要持续差速)
|
||||
*/
|
||||
void cascade_steering_loop(float line_deviation, float gyro_z)
|
||||
{
|
||||
if(!g_control_enabled) return;
|
||||
|
||||
/* 转向 PD:
|
||||
* P项: 中线偏差 → 差速
|
||||
* D项: gyro_z 反馈 → 阻尼 (车正在右转时 gyro_z>0, 需要抑制)
|
||||
*/
|
||||
g_pid_steering.feedback = line_deviation;
|
||||
g_pid_steering.setpoint = 0.0f; /* 目标: 中线居中 */
|
||||
pid_compute(&g_pid_steering);
|
||||
|
||||
/* 叠加 gyro D 项 (在 PID 输出基础上减去角速度反馈) */
|
||||
g_steering_offset = g_pid_steering.output
|
||||
- g_pid_steering.kd * gyro_z;
|
||||
|
||||
/* 限幅 */
|
||||
if(g_steering_offset > g_pid_steering.out_max)
|
||||
g_steering_offset = g_pid_steering.out_max;
|
||||
if(g_steering_offset < g_pid_steering.out_min)
|
||||
g_steering_offset = g_pid_steering.out_min;
|
||||
}
|
||||
@@ -0,0 +1,102 @@
|
||||
#ifndef _PID_CONTROL_H_
|
||||
#define _PID_CONTROL_H_
|
||||
|
||||
#include "zf_common_typedef.h"
|
||||
|
||||
/*============================================================
|
||||
* 电机引脚定义 (与 cpu0_main.c 保持一致)
|
||||
*============================================================*/
|
||||
#define MOTOR_DIR1 (P11_9)
|
||||
#define MOTOR_PWM1 (ATOM3_CH5_P11_10)
|
||||
#define MOTOR_DIR2 (P11_11)
|
||||
#define MOTOR_PWM2 (ATOM2_CH7_P11_12)
|
||||
|
||||
/*============================================================
|
||||
* PID 单环结构体 (支持位置式/增量式, 带积分分离 + 输出限幅)
|
||||
*
|
||||
* mode: 0=位置式 (output = f(error) 直接输出)
|
||||
* 1=增量式 (output += Δf(error) 累积输出, 天然抗饱和)
|
||||
*============================================================*/
|
||||
typedef struct {
|
||||
float kp, ki, kd; /* PID 系数 */
|
||||
float setpoint; /* 目标值 (外部写入) */
|
||||
float feedback; /* 反馈值 (外部写入) */
|
||||
float error; /* 当前误差 (只读) */
|
||||
float integral; /* 积分累加值 (位置式用) */
|
||||
float prev_error; /* e(k-1) */
|
||||
float prev_error2; /* e(k-2), 增量式 D 项用 */
|
||||
float derivative; /* 本次微分值 (只读) */
|
||||
float output; /* 本次输出 (只读) */
|
||||
float out_max, out_min; /* 输出限幅 */
|
||||
float integral_limit; /* 积分分离阈值 */
|
||||
uint8 mode; /* 0=位置式, 1=增量式 */
|
||||
} pid_t;
|
||||
|
||||
/*============================================================
|
||||
* 四轮车速度/转向 PID 全局句柄
|
||||
*
|
||||
* 前向运动:
|
||||
* 目标速度 → 左/右速度环 → 左/右PWM基础值
|
||||
*
|
||||
* 并行方向 (转向):
|
||||
* 转向环 → PWM差速值 (用陀螺仪 yaw rate 做 D 项反馈)
|
||||
*
|
||||
* 最终输出:
|
||||
* 左轮PWM = 左速度环输出 - PWM差速值
|
||||
* 右轮PWM = 右速度环输出 + PWM差速值
|
||||
*============================================================*/
|
||||
extern pid_t g_pid_speed_left; /* 左轮速度环 (内环, 100ms) */
|
||||
extern pid_t g_pid_speed_right; /* 右轮速度环 (内环, 100ms) */
|
||||
extern pid_t g_pid_steering; /* 转向环 (并行, 帧周期) */
|
||||
|
||||
/*============================================================
|
||||
* 电机输出接口 (每个电机驱动一侧两个轮子, 差速转向)
|
||||
*============================================================*/
|
||||
#define PWM_DUTY_MAX_MOTOR ((int16)9990)
|
||||
#define MOTOR_PWM_DEADBAND ((int16)3000)
|
||||
#define SPEED_PID_OUT_MAX ((float)(PWM_DUTY_MAX_MOTOR - MOTOR_PWM_DEADBAND))
|
||||
|
||||
void motor_set_left (int16 pwm); /* 左电机: 正=前进, 负=后退 */
|
||||
void motor_set_right(int16 pwm); /* 右电机: 正=前进, 负=后退 */
|
||||
|
||||
/*============================================================
|
||||
* PID 核心函数
|
||||
*============================================================*/
|
||||
void pid_init(pid_t *p, float kp, float ki, float kd,
|
||||
float out_max, float out_min, float integral_limit, uint8 mode);
|
||||
|
||||
float pid_compute(pid_t *p); /* 位置式 */
|
||||
|
||||
float pid_compute_inc(pid_t *p); /* 增量式 */
|
||||
|
||||
void pid_reset(pid_t *p);
|
||||
|
||||
/*============================================================
|
||||
* 控制环总控
|
||||
*============================================================*/
|
||||
|
||||
/* 速度环 (内环): 每100ms调用, 编码器速度 → PWM基础值
|
||||
* target_speed 由 Base Spd 菜单参数设定 (全局变量)
|
||||
* 输出: 驱动左右电机 PWM
|
||||
*/
|
||||
void cascade_speed_loop(int16 enc1_count, int16 enc2_count);
|
||||
|
||||
/* 转向环 (并行): 每帧调用, 摄像头中线偏差 → PWM差速
|
||||
* gyro_z 作为 D 项反馈, 抑制转向过冲和蛇形
|
||||
*/
|
||||
void cascade_steering_loop(float line_deviation, float gyro_z);
|
||||
|
||||
/* 控制使能 */
|
||||
extern bool g_control_enabled;
|
||||
|
||||
/* 目标速度 (Base Spd → 速度环输入), 单位: 编码器脉冲/100ms */
|
||||
extern float g_target_speed;
|
||||
|
||||
/* 最终下发到左右电机的 PWM 命令 (含死区补偿和转向差速) */
|
||||
extern float g_motor_pwm_left_cmd;
|
||||
extern float g_motor_pwm_right_cmd;
|
||||
|
||||
/* 陀螺仪 z 轴角速度 (°/s), 在 5ms ISR 中更新, 供转向环 D 项使用 */
|
||||
extern float g_gyro_z_dps;
|
||||
|
||||
#endif /* _PID_CONTROL_H_ */
|
||||
@@ -0,0 +1,75 @@
|
||||
/*********************************************************************************************************************
|
||||
* TC264 智能车 - PID调参桥接模块 (pid_tuning.c)
|
||||
*
|
||||
* 功能:
|
||||
* 1. 将菜单的 test_pid_value / test_user_value 同步到真实 PID 结构体
|
||||
* 2. 将关键控制变量推送到虚拟示波器, 用于上位机波形显示
|
||||
*
|
||||
* 菜单 PID 组映射:
|
||||
* 组0 "SpdL" → g_pid_speed_left (左轮速度环 P/I/D)
|
||||
* 组1 "SpdR" → g_pid_speed_right (右轮速度环 P/I/D)
|
||||
* 组2 "Steering" → g_pid_steering (转向环 P/I/D)
|
||||
*
|
||||
* 菜单浮点参数 (cleanup 后, PARAM_TOTAL_NUM=9):
|
||||
* [1] Base Spd — 手动速度模式时作为 g_target_speed
|
||||
* [2] Vac Duty — 真空泵占空比, user_menu.c 直接控制
|
||||
* [3] Max Spd — 保留(以后用)
|
||||
* [4] Crn Comp — 保留(以后用)
|
||||
*
|
||||
* 虚拟示波器通道:
|
||||
* CH1: 目标速度 (脉冲/100ms)
|
||||
* CH2: 左轮实际速度 (脉冲/100ms)
|
||||
* CH3: 右轮实际速度 (脉冲/100ms)
|
||||
* CH4: 左右平均电机 PWM
|
||||
*********************************************************************************************************************/
|
||||
|
||||
#include "zf_common_headfile.h"
|
||||
#include "pid_control.h"
|
||||
#include "pid_tuning.h"
|
||||
#include "zf_device_virtual_oscilloscope.h"
|
||||
|
||||
extern float test_user_value[];
|
||||
extern float test_pid_value[PID_TUNING_LOOP_COUNT][3];
|
||||
extern char test_pid_loop[PID_TUNING_LOOP_COUNT][12];
|
||||
extern int16 pid_switch_num;
|
||||
|
||||
static float g_last_speed_left = 0.0f;
|
||||
static float g_last_speed_right = 0.0f;
|
||||
float g_last_pwm = 0.0f;
|
||||
float g_last_pwm_left = 0.0f;
|
||||
float g_last_pwm_right = 0.0f;
|
||||
|
||||
void pid_tuning_sync(void)
|
||||
{
|
||||
g_pid_speed_left.kp = test_pid_value[0][0];
|
||||
g_pid_speed_left.ki = test_pid_value[0][1];
|
||||
g_pid_speed_left.kd = test_pid_value[0][2];
|
||||
|
||||
g_pid_speed_right.kp = test_pid_value[1][0];
|
||||
g_pid_speed_right.ki = test_pid_value[1][1];
|
||||
g_pid_speed_right.kd = test_pid_value[1][2];
|
||||
|
||||
g_pid_steering.kp = test_pid_value[2][0];
|
||||
g_pid_steering.ki = test_pid_value[2][1];
|
||||
g_pid_steering.kd = test_pid_value[2][2];
|
||||
|
||||
g_target_speed = test_user_value[1];
|
||||
|
||||
g_last_speed_left = g_pid_speed_left.feedback;
|
||||
g_last_speed_right = g_pid_speed_right.feedback;
|
||||
g_last_pwm_left = g_motor_pwm_left_cmd;
|
||||
g_last_pwm_right = g_motor_pwm_right_cmd;
|
||||
g_last_pwm = (g_last_pwm_left + g_last_pwm_right) * 0.5f;
|
||||
}
|
||||
|
||||
void pid_tuning_scope_output(void)
|
||||
{
|
||||
int16 ch1, ch2, ch3, ch4;
|
||||
|
||||
ch1 = (int16)g_target_speed;
|
||||
ch2 = (int16)g_last_speed_left;
|
||||
ch3 = (int16)g_last_speed_right;
|
||||
ch4 = (int16)g_last_pwm;
|
||||
|
||||
virtual_oscilloscope_data_conversion(ch1, ch2, ch3, ch4);
|
||||
}
|
||||
@@ -0,0 +1,30 @@
|
||||
#ifndef _PID_TUNING_H_
|
||||
#define _PID_TUNING_H_
|
||||
|
||||
#include "zf_common_typedef.h"
|
||||
|
||||
#define PID_TUNING_LOOP_COUNT 3
|
||||
|
||||
/*============================================================
|
||||
* 调参桥接模块
|
||||
*
|
||||
* 把菜单的 test_user_value 和 test_pid_value 同步到真实的
|
||||
* PID 结构体, 同时提供虚拟示波器数据输出。
|
||||
*
|
||||
* 调用方式:
|
||||
* pid_tuning_sync() — 每个控制周期调用, 同步参数
|
||||
* pid_tuning_scope_output() — 每 50ms 调用, 发送波形数据
|
||||
*============================================================*/
|
||||
|
||||
/* 将菜单参数同步到真实 PID */
|
||||
void pid_tuning_sync(void);
|
||||
|
||||
/* 速度环 PWM 输出值 (供调试/显示) */
|
||||
extern float g_last_pwm;
|
||||
extern float g_last_pwm_left;
|
||||
extern float g_last_pwm_right;
|
||||
|
||||
/* 将关键变量发送到虚拟示波器 (4通道 int16) */
|
||||
void pid_tuning_scope_output(void);
|
||||
|
||||
#endif /* _PID_TUNING_H_ */
|
||||
@@ -0,0 +1,110 @@
|
||||
#include "user_image_process.h"
|
||||
|
||||
IFX_ALIGN(4) uint8 image_binary_1[MT9V03X_1_H][MT9V03X_1_W];
|
||||
|
||||
int user_otsu_threshold(uint8 *image, uint16 col, uint16 row)
|
||||
{
|
||||
#define GRAYSCALE 256
|
||||
uint16 width = col;
|
||||
uint16 height = row;
|
||||
int pixelCount[GRAYSCALE];
|
||||
float pixelPro[GRAYSCALE];
|
||||
int i, j;
|
||||
int pixelSum = width * height / 4;
|
||||
int threshold = 0;
|
||||
uint8 *data = image;
|
||||
|
||||
for (i = 0; i < GRAYSCALE; i++)
|
||||
{
|
||||
pixelCount[i] = 0;
|
||||
pixelPro[i] = 0;
|
||||
}
|
||||
|
||||
uint32 gray_sum = 0;
|
||||
for (i = 0; i < height; i += 2)
|
||||
{
|
||||
for (j = 0; j < width; j += 2)
|
||||
{
|
||||
pixelCount[(int)data[i * width + j]]++;
|
||||
gray_sum += (int)data[i * width + j];
|
||||
}
|
||||
}
|
||||
|
||||
for (i = 0; i < GRAYSCALE; i++)
|
||||
{
|
||||
pixelPro[i] = (float)pixelCount[i] / pixelSum;
|
||||
}
|
||||
|
||||
float w0, w1, u0tmp, u1tmp, u0, u1, u, deltaTmp, deltaMax = 0;
|
||||
w0 = w1 = u0tmp = u1tmp = u0 = u1 = u = deltaTmp = 0;
|
||||
|
||||
for (j = 0; j < GRAYSCALE; j++)
|
||||
{
|
||||
w0 += pixelPro[j];
|
||||
u0tmp += j * pixelPro[j];
|
||||
w1 = 1 - w0;
|
||||
|
||||
/* 防止除零: 某一类为空时跳过该阈值 */
|
||||
if (w0 < 1e-6f || w1 < 1e-6f)
|
||||
continue;
|
||||
|
||||
u1tmp = (float)gray_sum / pixelSum - u0tmp;
|
||||
|
||||
u0 = u0tmp / w0;
|
||||
u1 = u1tmp / w1;
|
||||
u = u0tmp + u1tmp;
|
||||
|
||||
deltaTmp = w0 * (u0 - u) * (u0 - u) + w1 * (u1 - u) * (u1 - u);
|
||||
|
||||
if (deltaTmp > deltaMax)
|
||||
{
|
||||
deltaMax = deltaTmp;
|
||||
threshold = j;
|
||||
}
|
||||
}
|
||||
|
||||
return threshold;
|
||||
}
|
||||
|
||||
void user_image_binarize(uint8 *src_image, uint8 (*dst_image)[MT9V03X_1_W], uint16 col, uint16 row, int threshold)
|
||||
{
|
||||
uint16 i, j;
|
||||
|
||||
for (i = 0; i < row; i++)
|
||||
{
|
||||
for (j = 0; j < col; j++)
|
||||
{
|
||||
dst_image[i][j] = (src_image[i * col + j] >= (uint8)threshold) ? IMG_WHITE : IMG_BLACK;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void user_image_denoise(uint8 (*image)[MT9V03X_1_W], uint16 col, uint16 row)
|
||||
{
|
||||
uint16 i, j;
|
||||
static uint8 buf[MT9V03X_1_H][MT9V03X_1_W];
|
||||
|
||||
for (i = 0; i < row; i++) {
|
||||
for (j = 0; j < col; j++) {
|
||||
buf[i][j] = image[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
for (i = 1; i < row - 1; i++) {
|
||||
for (j = 1; j < col - 1; j++) {
|
||||
int cnt = 0;
|
||||
if (buf[i-1][j-1] == IMG_WHITE) cnt++;
|
||||
if (buf[i-1][j ] == IMG_WHITE) cnt++;
|
||||
if (buf[i-1][j+1] == IMG_WHITE) cnt++;
|
||||
if (buf[i ][j-1] == IMG_WHITE) cnt++;
|
||||
if (buf[i ][j ] == IMG_WHITE) cnt++;
|
||||
if (buf[i ][j+1] == IMG_WHITE) cnt++;
|
||||
if (buf[i+1][j-1] == IMG_WHITE) cnt++;
|
||||
if (buf[i+1][j ] == IMG_WHITE) cnt++;
|
||||
if (buf[i+1][j+1] == IMG_WHITE) cnt++;
|
||||
|
||||
image[i][j] = (cnt >= 5) ? IMG_WHITE : IMG_BLACK;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,16 @@
|
||||
#ifndef _USER_IMAGE_PROCESS_H_
|
||||
#define _USER_IMAGE_PROCESS_H_
|
||||
|
||||
#include "zf_common_typedef.h"
|
||||
#include "zf_device_mt9v03x_double.h"
|
||||
|
||||
#define IMG_BLACK 0x00
|
||||
#define IMG_WHITE 0xFF
|
||||
|
||||
extern uint8 image_binary_1[MT9V03X_1_H][MT9V03X_1_W];
|
||||
|
||||
int user_otsu_threshold(uint8 *image, uint16 col, uint16 row);
|
||||
void user_image_binarize(uint8 *src_image, uint8 (*dst_image)[MT9V03X_1_W], uint16 col, uint16 row, int threshold);
|
||||
void user_image_denoise(uint8 (*image)[MT9V03X_1_W], uint16 col, uint16 row);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,712 @@
|
||||
#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, ¢er_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;
|
||||
}
|
||||
@@ -0,0 +1,57 @@
|
||||
#ifndef _USER_LINE_PROCESS_H_
|
||||
#define _USER_LINE_PROCESS_H_
|
||||
|
||||
#include "zf_common_typedef.h"
|
||||
#include "zf_device_mt9v03x_double.h"
|
||||
|
||||
/*============================================================
|
||||
* 常量配置
|
||||
*============================================================*/
|
||||
#define MAX_EDGE_PTS 240 /* 最大边线点数 (图像高度) */
|
||||
|
||||
/*============================================================
|
||||
* 全局输出变量
|
||||
*============================================================*/
|
||||
extern float center_line[MAX_EDGE_PTS][2]; /* 赛道中线 (图像坐标) */
|
||||
extern uint16 center_line_num; /* 中线点数 */
|
||||
extern float left_edge[MAX_EDGE_PTS][2]; /* 左边线 (重采样后) */
|
||||
extern float right_edge[MAX_EDGE_PTS][2]; /* 右边线 (重采样后) */
|
||||
extern uint16 left_edge_num, right_edge_num; /* 边线点数 */
|
||||
extern float inflection_point[2]; /* 左拐点坐标 (图像) */
|
||||
extern uint8 inflection_found; /* 左拐点是否有效 */
|
||||
extern float inflection_point_r[2]; /* 右拐点坐标 (图像) */
|
||||
extern uint8 inflection_found_r; /* 右拐点是否有效 */
|
||||
|
||||
/*============================================================
|
||||
* 赛道边界检测 (迷宫法)
|
||||
*
|
||||
* 从 (start_x, start_y) 开始, 用左右手定则沿白线边缘向上追踪.
|
||||
* 内置底行全宽搜索兜底: 当中心点无赛道时自动扫描全行.
|
||||
*============================================================*/
|
||||
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);
|
||||
|
||||
/*============================================================
|
||||
* 中线计算 (插值补齐缺失边, 产生平滑稠密中线)
|
||||
*
|
||||
* 原始迷宫法左右追踪点 y 坐标不对齐, 同行配对极稀疏.
|
||||
* 本函数先对缺失边线做线性插值, 再对中线做空间平滑和鲁棒拟合.
|
||||
*============================================================*/
|
||||
void compute_center_line_interp(int16 left[][2], uint16 left_num,
|
||||
int16 right[][2], uint16 right_num,
|
||||
float center[][2], uint16 *center_num);
|
||||
|
||||
/*============================================================
|
||||
* 一帧图像处理主入口
|
||||
*
|
||||
* 输入: binary — 已二值化的图像 (140x80, 0x00=黑/背景, 0xFF=白/赛道)
|
||||
* 输出: center_line / left_edge / right_edge 全局变量
|
||||
*============================================================*/
|
||||
void line_process_run(uint8 *binary);
|
||||
|
||||
/* 中线加权偏差 (仅计入 y>=min_row 的下半部分, 底部权重更大) */
|
||||
float line_compute_weighted_deviation(uint16 image_center_x, uint16 min_row);
|
||||
|
||||
#endif /* _USER_LINE_PROCESS_H_ */
|
||||
@@ -0,0 +1,991 @@
|
||||
/*============================================================
|
||||
* 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();
|
||||
}
|
||||
@@ -0,0 +1,79 @@
|
||||
#ifndef _USER_MENU_H_
|
||||
#define _USER_MENU_H_
|
||||
|
||||
#include "zf_common_typedef.h"
|
||||
#include "zf_device_mt9v03x_double.h"
|
||||
#include "zf_device_ips200.h"
|
||||
|
||||
/*============================================================
|
||||
* 菜单页面定义
|
||||
*============================================================*/
|
||||
#define MENU_PAGE_REALTIME 0 /* 页面0: 实时状态显示页 */
|
||||
#define MENU_PAGE_PARAMETER 1 /* 页面1: 参数设置页 */
|
||||
|
||||
/*============================================================
|
||||
* 参数表定义
|
||||
*============================================================*/
|
||||
#define PARAM_TOTAL_NUM 9 /* 参数总数: WiFi+4浮点+PID选择+3PID值 */
|
||||
#define PARAM_PER_PAGE 10 /* 每页显示参数个数 */
|
||||
|
||||
/*============================================================
|
||||
* 屏幕布局宏定义 (IPS200 竖屏 240x320)
|
||||
*============================================================*/
|
||||
#define SCREEN_WIDTH 240 /* 屏幕宽度 */
|
||||
#define SCREEN_HEIGHT 320 /* 屏幕高度 */
|
||||
|
||||
/* 状态栏区域 */
|
||||
#define STATUSBAR_Y 0 /* 状态栏起始Y坐标 */
|
||||
#define STATUSBAR_H 32 /* 状态栏高度（两行：BAT行 + CPU行，每行16px） */
|
||||
#define STATUSBAR_FONT_SIZE IPS200_8X16_FONT /* 状态栏字体大小 */
|
||||
|
||||
/* 图像显示区域 */
|
||||
#define IMAGE_Y (STATUSBAR_Y + STATUSBAR_H + 2) /* 图像起始Y坐标 */
|
||||
#define IMAGE_CAM_W MT9V03X_1_W /* 摄像头图像宽度 */
|
||||
#define IMAGE_CAM_H MT9V03X_1_H /* 摄像头图像高度 */
|
||||
#define IMAGE_X ((SCREEN_WIDTH - IMAGE_CAM_W) / 2) /* 图像居中X */
|
||||
|
||||
/* 实时数据区域 */
|
||||
#define DATA_Y (IMAGE_Y + IMAGE_CAM_H + 2) /* 数据起始Y坐标 */
|
||||
#define DATA_LINE_H 16 /* 每行数据高度 */
|
||||
#define DATA_FONT_SIZE IPS200_8X16_FONT
|
||||
|
||||
/* 参数页面布局 */
|
||||
#define PARAM_PAGE_TITLE_Y 0 /* 参数页标题Y坐标 */
|
||||
#define PARAM_START_Y 20 /* 参数内容起始Y坐标 */
|
||||
#define PARAM_LINE_H 16 /* 参数行高 */
|
||||
|
||||
/*============================================================
|
||||
* 进度条颜色定义 (从低到高 红->黄->绿)
|
||||
*============================================================*/
|
||||
#define COLOR_BAR_LOW RGB565_RED
|
||||
#define COLOR_BAR_MID RGB565_YELLOW
|
||||
#define COLOR_BAR_HIGH RGB565_GREEN
|
||||
#define COLOR_BG RGB565_BLACK /* 背景色 */
|
||||
#define COLOR_TEXT RGB565_WHITE /* 文字色 */
|
||||
#define COLOR_HIGHLIGHT RGB565_CYAN /* 高亮色 */
|
||||
#define COLOR_BAR_BG RGB565_BLUE /* 进度条底色 */
|
||||
|
||||
extern uint8 camera_frame_buffer[MT9V03X_1_H * MT9V03X_1_W]; /* 帧缓冲区 */
|
||||
|
||||
/*============================================================
|
||||
* 函数声明
|
||||
*============================================================*/
|
||||
void user_menu_init (void); /* 菜单初始化 */
|
||||
void user_menu_loop (void); /* 菜单主循环 */
|
||||
|
||||
void user_menu_key_dispose (void); /* 按键处理 */
|
||||
void user_menu_switch_page (void); /* 页面切换 */
|
||||
|
||||
void user_menu_draw_status_bar (void); /* 绘制顶部状态栏 */
|
||||
void user_menu_draw_progress_bar (uint16 x, uint16 y, uint16 w, uint16 percent, uint16 color); /* 绘制进度条 */
|
||||
void user_menu_draw_camera (void); /* 绘制摄像头灰度图像 */
|
||||
void user_menu_draw_camera_binary(void); /* 绘制摄像头二值化图像 */
|
||||
void user_menu_draw_realtime_data(void); /* 绘制实时数据 */
|
||||
|
||||
void user_menu_draw_param_page (void); /* 绘制参数设置页面 */
|
||||
void user_menu_draw_param_table (void); /* 绘制参数表格 */
|
||||
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,195 @@
|
||||
/*============================================================
|
||||
* TC264 智能车 - 菜单设备层模块 (user_menu_device.c)
|
||||
*
|
||||
* 功能说明：
|
||||
* 提供菜单系统所需的设备数据采集与计算功能。
|
||||
* 包括电池电压监测、CPU 占用率统计等底层支持。
|
||||
*
|
||||
* 主要功能：
|
||||
* 1. battery_voltage_gather() - 采集电池电压并计算剩余电量百分比
|
||||
* 2. cpu_usage_check() - 统计 CPU 占用率
|
||||
*============================================================*/
|
||||
|
||||
#include "zf_common_function.h" /* 通用函数（数学运算等） */
|
||||
#include "zf_driver_timer.h" /* 定时器驱动（system_start等） */
|
||||
#include "zf_driver_delay.h" /* 延时函数 */
|
||||
#include "zf_driver_gpio.h" /* GPIO 驱动 (蜂鸣器) */
|
||||
#include "user_menu.h" /* 菜单头文件 */
|
||||
#include "user_menu_device.h" /* 设备层头文件 */
|
||||
|
||||
/*============================================================
|
||||
* 电池电压相关变量
|
||||
*============================================================*/
|
||||
float battery_voltage; /* 电池电压值 (V) */
|
||||
float battery_percentage; /* 电池剩余百分比 (0-100) */
|
||||
float battery_percentage_filter; /* 电池百分比（经低通滤波平滑后） */
|
||||
|
||||
/*============================================================
|
||||
* CPU占用率相关变量
|
||||
*============================================================*/
|
||||
float cpu_usage_percentage; /* CPU占用率 (0-100) */
|
||||
float cpu_usage_percentage_filter; /* CPU占用率（经低通滤波平滑后） */
|
||||
|
||||
/*============================================================
|
||||
* 电池电压采集与电量计算
|
||||
*
|
||||
* 计算原理：
|
||||
* 1. ADC 读取电压值 (0-4095)
|
||||
* 2. 根据分压电阻比例换算为实际电池电压
|
||||
* V_bat = adc_val / 4096 * 36.3 * 校正系数
|
||||
* 3. 根据锂电池特性估算剩余电量
|
||||
* 单节锂电池电压范围：3.7V (空) ~ 4.23V (满)
|
||||
* 根据总电压判断串联锂电池节数
|
||||
* 线性映射到 0-100%
|
||||
* 4. 低通滤波 (一阶惯性滤波) 平滑输出
|
||||
*
|
||||
* 输入参数：
|
||||
* adc_get_data: ADC 采集的原始值 (0-4095)
|
||||
*
|
||||
* 输出 (全局变量)：
|
||||
* battery_voltage - 当前电压值 (V)
|
||||
* battery_percentage - 当前电量百分比
|
||||
* battery_percentage_filter - 滤波后的电量百分比
|
||||
*============================================================*/
|
||||
void battery_voltage_gather(uint16 adc_get_data)
|
||||
{
|
||||
uint8 cell_num = 0; /* 锂电池串联节数 */
|
||||
float effective_voltage = 0; /* 有效电压范围 */
|
||||
|
||||
/* 计算实际电池电压
|
||||
* 36.3 = 参考电压分压比 (需根据实际硬件电路调整)
|
||||
* VOLTAGE_RECTIFY_COEFFICIENT = 电压校正系数 */
|
||||
battery_voltage = (float)adc_get_data / 4096.0f * 36.3f * VOLTAGE_RECTIFY_COEFFICIENT;
|
||||
|
||||
/* 判断锂电池串联节数 (区间阈值法，避免 ceil 在临界值误判)
|
||||
* 1S: < 5.5V, 2S: 5.5~10.5V, 3S: 10.5~15.0V, 4S: > 15.0V */
|
||||
if(battery_voltage < 5.5f)
|
||||
cell_num = 1;
|
||||
else if(battery_voltage < 10.5f)
|
||||
cell_num = 2;
|
||||
else if(battery_voltage < 15.0f)
|
||||
cell_num = 3;
|
||||
else
|
||||
cell_num = 4;
|
||||
|
||||
/* 有效电压范围 = 0.5V * 节数 (留出余量) */
|
||||
effective_voltage = 0.5f * (float)cell_num;
|
||||
|
||||
/* 将电压线性映射到 0-100%
|
||||
* 基准电压 = 3.7V * 串联节数 (空电截止电压) */
|
||||
battery_percentage = (func_limit_ab(
|
||||
(battery_voltage - 3.7f * (float)cell_num),
|
||||
0.0f, effective_voltage) / effective_voltage * 100.0f);
|
||||
|
||||
/* 一阶低通滤波：新值 = 旧值 * (49/50) + 当前值 * (1/50)
|
||||
* 用于平滑电量显示，避免显示值跳变 */
|
||||
battery_percentage_filter = (battery_percentage_filter * 49.0f + battery_percentage) / 50.0f;
|
||||
}
|
||||
|
||||
/*============================================================
|
||||
* CPU 占用率统计
|
||||
*
|
||||
* 实现原理：
|
||||
* 每帧处理完成后调用本函数，传入该帧的耗时(us)。
|
||||
* 累积所有帧的耗时，每 1000ms 计算占比：
|
||||
* CPU占用率 = (累积耗时 / 总时长) * 100%
|
||||
* 使用一阶低通滤波平滑显示。
|
||||
*
|
||||
* 示例：
|
||||
* 50fps 摄像头，每帧 20ms 周期，处理耗时 8ms
|
||||
* 在 1 秒内累积 50 * 8000us = 400000us
|
||||
* 占用率 = 400000 / (1000*1000) * 100 = 40%
|
||||
*============================================================*/
|
||||
void cpu_usage_record_frame(uint32 frame_busy_us)
|
||||
{
|
||||
static uint32 cpu_busy_us = 0; /* 累积忙碌时间 (us) */
|
||||
static uint32 cpu_last_ms = 0; /* 上次统计时刻 (ms) */
|
||||
static uint8 cpu_timer_ok = 0; /* 系统定时器是否已启动 */
|
||||
uint32 now;
|
||||
|
||||
if(!cpu_timer_ok)
|
||||
{
|
||||
system_start();
|
||||
cpu_timer_ok = 1;
|
||||
}
|
||||
|
||||
cpu_busy_us += frame_busy_us;
|
||||
now = system_getval_ms();
|
||||
|
||||
/* 每 1000ms 统计一次 */
|
||||
if(now - cpu_last_ms >= 1000)
|
||||
{
|
||||
uint32 elapsed_ms = now - cpu_last_ms;
|
||||
|
||||
/* 占用率 = 忙时间 / 总时间 * 100 */
|
||||
/* busy_us / (elapsed_ms * 1000us) * 100 = busy_us / (elapsed_ms * 10) */
|
||||
cpu_usage_percentage = (float)cpu_busy_us / (float)(elapsed_ms * 10);
|
||||
|
||||
if(cpu_usage_percentage > 100.0f)
|
||||
cpu_usage_percentage = 100.0f;
|
||||
|
||||
cpu_busy_us = 0;
|
||||
cpu_last_ms = now;
|
||||
}
|
||||
|
||||
/* 一阶低通滤波平滑显示 */
|
||||
cpu_usage_percentage_filter = (cpu_usage_percentage_filter * 49.0f + cpu_usage_percentage) / 50.0f;
|
||||
}
|
||||
|
||||
/*============================================================
|
||||
* 低电量蜂鸣报警
|
||||
*
|
||||
* 功能说明：
|
||||
* 当滤波后电量 <= 30% 时，蜂鸣器"滴滴滴"连响3声后停止。
|
||||
* 电量回升到 35% 以上后重置，可再次触发。
|
||||
* 使用 system_getval_ms() 非阻塞计时，不影响其他功能。
|
||||
*============================================================*/
|
||||
void low_battery_alarm_check(void)
|
||||
{
|
||||
static uint8 state = 0; /* 0=待机, 1=鸣响, 2=间歇, 3=冷却 */
|
||||
static uint8 cnt = 0; /* 已响次数 */
|
||||
static uint32 t0 = 0; /* 状态进入时刻 (ms) */
|
||||
uint32 now = system_getval_ms();
|
||||
|
||||
switch(state)
|
||||
{
|
||||
case 0: /* 待机：电量过低且电池真实存在时触发 */
|
||||
if(battery_voltage > 5.0f
|
||||
&& battery_percentage_filter > 0.1f
|
||||
&& battery_percentage_filter <= BATT_ALARM_THRESHOLD)
|
||||
{
|
||||
state = 1;
|
||||
cnt = 0;
|
||||
gpio_high(BEEP_PIN);
|
||||
t0 = now;
|
||||
}
|
||||
break;
|
||||
|
||||
case 1: /* 鸣响中：持续 200ms 后关闭 */
|
||||
if(now - t0 >= 200)
|
||||
{
|
||||
gpio_low(BEEP_PIN);
|
||||
cnt++;
|
||||
if(cnt >= 3)
|
||||
state = 3; /* 响完3声进入冷却 */
|
||||
else
|
||||
state = 2; /* 进入间歇 */
|
||||
t0 = now;
|
||||
}
|
||||
break;
|
||||
|
||||
case 2: /* 间歇中：静音 200ms 后再次鸣响 */
|
||||
if(now - t0 >= 200)
|
||||
{
|
||||
gpio_high(BEEP_PIN);
|
||||
state = 1;
|
||||
t0 = now;
|
||||
}
|
||||
break;
|
||||
|
||||
case 3: /* 冷却：等待电量回升到 35% 以上，避免重复报警 */
|
||||
if(battery_percentage_filter > BATT_ALARM_THRESHOLD + 5.0f)
|
||||
state = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,68 @@
|
||||
#ifndef _USER_MENU_DEVICE_H_
|
||||
#define _USER_MENU_DEVICE_H_
|
||||
|
||||
#include "zf_common_typedef.h" /* 常用类型定义 */
|
||||
|
||||
/*============================================================
|
||||
* 电池电压检测配置
|
||||
*============================================================*/
|
||||
#define VOLTAGE_PORT (ADC0_CH11_A11) /* 电池电压 ADC 通道 */
|
||||
#define VOLTAGE_RECTIFY_COEFFICIENT (1.042832f) /* 电压分压校正系数 */
|
||||
|
||||
/*============================================================
|
||||
* 蜂鸣器与报警配置
|
||||
*============================================================*/
|
||||
#define BEEP_PIN (P33_10) /* 蜂鸣器 GPIO */
|
||||
#define BATT_ALARM_THRESHOLD 30.0f /* 低电量报警阈值 (%) */
|
||||
|
||||
/*============================================================
|
||||
* 电机控制引脚配置
|
||||
*============================================================*/
|
||||
#define VACUUM_PWM (ATOM0_CH0_P21_2) /* 真空泵 PWM 引脚 */
|
||||
|
||||
/*============================================================
|
||||
* IPS200 显示屏配置
|
||||
*============================================================*/
|
||||
/* 根据实际硬件选择 SPI 或 并口 方式：
|
||||
* IPS200_TYPE_SPI - SPI 接口模式
|
||||
* IPS200_TYPE_PARALLEL8 - 8位并口模式 */
|
||||
#define IPS200_HARDWARE_TYPE (IPS200_TYPE_SPI)
|
||||
|
||||
/*============================================================
|
||||
* 全局变量声明 (定义在 cpu0_main.c 中)
|
||||
*============================================================*/
|
||||
|
||||
/* ----- 编码器数据 (在 isr.c 中更新) ----- */
|
||||
extern volatile int16 encoder_data_1; /* 编码器1计数值 */
|
||||
extern volatile int16 encoder_data_2; /* 编码器2计数值 */
|
||||
|
||||
/* ----- IMU 数据 (在 isr.c 中更新, 变量定义在 zf_device_imu660rb.c) ----- */
|
||||
extern int16 imu660rb_acc_x; /* X轴加速度 */
|
||||
extern int16 imu660rb_acc_y; /* Y轴加速度 */
|
||||
extern int16 imu660rb_acc_z; /* Z轴加速度 */
|
||||
extern int16 imu660rb_gyro_x; /* X轴角速度 */
|
||||
extern int16 imu660rb_gyro_y; /* Y轴角速度 */
|
||||
extern int16 imu660rb_gyro_z; /* Z轴角速度 */
|
||||
|
||||
/*============================================================
|
||||
* 全局变量声明 (由 user_menu_device.c 定义)
|
||||
*============================================================*/
|
||||
|
||||
/* ----- 电池电压变量 ----- */
|
||||
extern float battery_voltage; /* 电池电压值 (V) */
|
||||
extern float battery_percentage; /* 电池剩余百分比 (0-100) */
|
||||
extern float battery_percentage_filter; /* 电池百分比（低通滤波后） */
|
||||
|
||||
/* ----- CPU占用率变量 ----- */
|
||||
extern float cpu_usage_percentage; /* CPU占用率 (0-100) */
|
||||
extern float cpu_usage_percentage_filter; /* CPU占用率（低通滤波后） */
|
||||
|
||||
/*============================================================
|
||||
* 函数声明
|
||||
*============================================================*/
|
||||
void battery_voltage_gather(uint16 adc_get_data); /* 采集电池电压计算电量 */
|
||||
void cpu_usage_record_frame(uint32 frame_busy_us); /* 记录帧处理耗时并统计占用率 */
|
||||
void low_battery_alarm_check(void); /* 低电量蜂鸣报警 */
|
||||
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,3 @@
|
||||
建议用户需要添加自己的代码文件的时候，将文件存放在CODE文件夹下
|
||||
不要再创建子文件夹，直接将代码文件放在CODE文件夹里面就行
|
||||
然后工程里面添加文件即可编译
|
||||
Reference in new issue
Block a user