jiahong
374 lines
11 KiB
C
374 lines
11 KiB
C
/*
|
||
* 自然写智能点阵笔嵌入式固件软件 V1.0
|
||
* coordinate_task.c - 坐标计算任务
|
||
*
|
||
* 功能说明：
|
||
* 1. 从图像帧中解码Anoto点阵图案
|
||
* 2. 计算笔尖在纸面的物理坐标
|
||
* 3. 坐标滤波与去抖（卡尔曼滤波）
|
||
* 4. 坐标打包为BLE传输格式
|
||
*/
|
||
|
||
#include <stdint.h>
|
||
#include <stdbool.h>
|
||
#include <string.h>
|
||
#include <math.h>
|
||
|
||
#include "FreeRTOS.h"
|
||
#include "task.h"
|
||
#include "queue.h"
|
||
|
||
#include "dot_decoder.h"
|
||
|
||
/* ========== 坐标数据包结构 ========== */
|
||
|
||
typedef struct {
|
||
uint32_t raw_x; /* 原始X坐标（20位精度） */
|
||
uint32_t raw_y; /* 原始Y坐标 */
|
||
uint16_t pressure; /* 压力值（12位） */
|
||
uint32_t timestamp_ms; /* 时间戳 */
|
||
uint32_t page_id; /* 页面ID */
|
||
uint8_t pen_state; /* 笔状态：0=书写中, 1=笔落下, 2=笔抬起 */
|
||
} CoordinatePacket;
|
||
|
||
/* ========== 卡尔曼滤波器 ========== */
|
||
|
||
typedef struct {
|
||
float x_estimate; /* X状态估计值 */
|
||
float y_estimate; /* Y状态估计值 */
|
||
float x_error; /* X估计误差协方差 */
|
||
float y_error; /* Y估计误差协方差 */
|
||
float process_noise; /* 过程噪声 Q */
|
||
float measurement_noise; /* 测量噪声 R */
|
||
bool initialized; /* 是否已初始化 */
|
||
} KalmanFilter2D;
|
||
|
||
/* ========== 外部引用 ========== */
|
||
|
||
extern QueueHandle_t g_image_data_queue;
|
||
extern QueueHandle_t g_coordinate_queue;
|
||
|
||
/* ========== 图像帧元数据结构（与image_capture_task一致） ========== */
|
||
|
||
typedef struct {
|
||
uint8_t *pixel_buffer;
|
||
uint32_t frame_id;
|
||
uint32_t timestamp_ms;
|
||
uint8_t quality_score;
|
||
uint8_t exposure_value;
|
||
uint16_t pressure_raw;
|
||
} ImageFrameMetadata;
|
||
|
||
/* ========== 静态变量 ========== */
|
||
|
||
/* 卡尔曼滤波器实例 */
|
||
static KalmanFilter2D s_kalman;
|
||
|
||
/* 上一次有效坐标（用于抖动检测） */
|
||
static float s_last_valid_x = 0;
|
||
static float s_last_valid_y = 0;
|
||
|
||
/* 点阵码解码工作缓冲区 */
|
||
static uint8_t s_decode_buffer[128];
|
||
|
||
/* 统计信息 */
|
||
static uint32_t s_total_decoded = 0;
|
||
static uint32_t s_decode_failures = 0;
|
||
|
||
/* ========== 卡尔曼滤波实现 ========== */
|
||
|
||
/**
|
||
* 初始化卡尔曼滤波器
|
||
* @param kf 滤波器实例
|
||
* @param q 过程噪声（越大跟踪越快，噪声越多）
|
||
* @param r 测量噪声（越大滤波越强，延迟越大）
|
||
*/
|
||
static void kalman_init(KalmanFilter2D *kf, float q, float r) {
|
||
kf->x_estimate = 0;
|
||
kf->y_estimate = 0;
|
||
kf->x_error = 1.0f;
|
||
kf->y_error = 1.0f;
|
||
kf->process_noise = q;
|
||
kf->measurement_noise = r;
|
||
kf->initialized = false;
|
||
}
|
||
|
||
/**
|
||
* 卡尔曼滤波更新
|
||
* @param kf 滤波器实例
|
||
* @param measured_x 测量X值
|
||
* @param measured_y 测量Y值
|
||
* @param out_x 滤波后X输出
|
||
* @param out_y 滤波后Y输出
|
||
*/
|
||
static void kalman_update(KalmanFilter2D *kf, float measured_x, float measured_y,
|
||
float *out_x, float *out_y) {
|
||
if (!kf->initialized) {
|
||
/* 第一次测量，直接使用测量值 */
|
||
kf->x_estimate = measured_x;
|
||
kf->y_estimate = measured_y;
|
||
kf->initialized = true;
|
||
*out_x = measured_x;
|
||
*out_y = measured_y;
|
||
return;
|
||
}
|
||
|
||
/* 预测步骤：状态不变模型（笔的位置预测 = 上一次估计） */
|
||
float x_pred = kf->x_estimate;
|
||
float y_pred = kf->y_estimate;
|
||
float x_err_pred = kf->x_error + kf->process_noise;
|
||
float y_err_pred = kf->y_error + kf->process_noise;
|
||
|
||
/* 更新步骤：计算卡尔曼增益 */
|
||
float kx = x_err_pred / (x_err_pred + kf->measurement_noise);
|
||
float ky = y_err_pred / (y_err_pred + kf->measurement_noise);
|
||
|
||
/* 融合预测与测量 */
|
||
kf->x_estimate = x_pred + kx * (measured_x - x_pred);
|
||
kf->y_estimate = y_pred + ky * (measured_y - y_pred);
|
||
|
||
/* 更新误差协方差 */
|
||
kf->x_error = (1.0f - kx) * x_err_pred;
|
||
kf->y_error = (1.0f - ky) * y_err_pred;
|
||
|
||
*out_x = kf->x_estimate;
|
||
*out_y = kf->y_estimate;
|
||
}
|
||
|
||
/**
|
||
* 重置卡尔曼滤波器（新笔画开始时调用）
|
||
*/
|
||
static void kalman_reset(KalmanFilter2D *kf) {
|
||
kf->initialized = false;
|
||
kf->x_error = 1.0f;
|
||
kf->y_error = 1.0f;
|
||
}
|
||
|
||
/* ========== 抖动检测 ========== */
|
||
|
||
/**
|
||
* 检测坐标是否为抖动（笔静止时传感器的微小抖动）
|
||
* 如果两次坐标之间的距离小于阈值，视为抖动并丢弃
|
||
*
|
||
* @param x 当前X坐标
|
||
* @param y 当前Y坐标
|
||
* @param threshold 抖动阈值（坐标单位）
|
||
* @return true表示是抖动，应丢弃
|
||
*/
|
||
static bool is_jitter(float x, float y, float threshold) {
|
||
float dx = x - s_last_valid_x;
|
||
float dy = y - s_last_valid_y;
|
||
float distance_sq = dx * dx + dy * dy;
|
||
|
||
return (distance_sq < threshold * threshold);
|
||
}
|
||
|
||
/* ========== 点阵码图像解码 ========== */
|
||
|
||
/**
|
||
* 从32x32灰度图像中解码Anoto点阵图案
|
||
*
|
||
* 解码步骤：
|
||
* 1. 二值化：将灰度图转为黑白图
|
||
* 2. 点检测：定位图案中的各个墨点位置
|
||
* 3. 网格对齐：将检测到的点对齐到规则网格
|
||
* 4. 编码读取：根据点相对于网格中心的偏移方向读取编码值
|
||
* 5. 坐标计算：将编码序列映射为全局坐标
|
||
*
|
||
* @param pixels 32x32灰度图像数据
|
||
* @param quality 图像质量评分
|
||
* @param out_x 解码输出X坐标
|
||
* @param out_y 解码输出Y坐标
|
||
* @param out_page_id 解码输出页面ID
|
||
* @return 0成功, -1解码失败
|
||
*/
|
||
static int decode_dot_pattern(const uint8_t *pixels, uint8_t quality,
|
||
uint32_t *out_x, uint32_t *out_y,
|
||
uint32_t *out_page_id) {
|
||
/* 步骤1：自适应二值化 */
|
||
uint8_t threshold = 128;
|
||
|
||
/* 根据图像亮度动态调整阈值（Otsu方法简化版） */
|
||
uint32_t histogram[256] = {0};
|
||
uint16_t i;
|
||
for (i = 0; i < SENSOR_PIXELS; i++) {
|
||
histogram[pixels[i]]++;
|
||
}
|
||
|
||
/* 寻找双峰之间的谷值作为阈值 */
|
||
uint32_t total = SENSOR_PIXELS;
|
||
uint32_t sum = 0;
|
||
for (i = 0; i < 256; i++) {
|
||
sum += i * histogram[i];
|
||
}
|
||
|
||
uint32_t sum_bg = 0;
|
||
uint32_t weight_bg = 0;
|
||
float max_variance = 0;
|
||
|
||
for (i = 0; i < 256; i++) {
|
||
weight_bg += histogram[i];
|
||
if (weight_bg == 0) continue;
|
||
|
||
uint32_t weight_fg = total - weight_bg;
|
||
if (weight_fg == 0) break;
|
||
|
||
sum_bg += i * histogram[i];
|
||
float mean_bg = (float)sum_bg / weight_bg;
|
||
float mean_fg = (float)(sum - sum_bg) / weight_fg;
|
||
|
||
float diff = mean_bg - mean_fg;
|
||
float variance = (float)weight_bg * weight_fg * diff * diff;
|
||
|
||
if (variance > max_variance) {
|
||
max_variance = variance;
|
||
threshold = (uint8_t)i;
|
||
}
|
||
}
|
||
|
||
/* 步骤2：二值化并检测墨点中心 */
|
||
uint8_t dot_count = 0;
|
||
int16_t dot_x[64], dot_y[64]; /* 最多检测64个点 */
|
||
|
||
for (int row = 1; row < SENSOR_HEIGHT - 1; row++) {
|
||
for (int col = 1; col < SENSOR_WIDTH - 1; col++) {
|
||
uint8_t center = pixels[row * SENSOR_WIDTH + col];
|
||
if (center < threshold) {
|
||
/* 暗像素，检查是否为局部极小值（简单的点中心检测） */
|
||
uint8_t up = pixels[(row - 1) * SENSOR_WIDTH + col];
|
||
uint8_t down = pixels[(row + 1) * SENSOR_WIDTH + col];
|
||
uint8_t left = pixels[row * SENSOR_WIDTH + (col - 1)];
|
||
uint8_t right = pixels[row * SENSOR_WIDTH + (col + 1)];
|
||
|
||
if (center <= up && center <= down &&
|
||
center <= left && center <= right) {
|
||
if (dot_count < 64) {
|
||
dot_x[dot_count] = col;
|
||
dot_y[dot_count] = row;
|
||
dot_count++;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/* 至少需要检测到4个点才能解码 */
|
||
if (dot_count < 4) {
|
||
return -1;
|
||
}
|
||
|
||
/* 步骤3-5：调用点阵码解码器（核心算法在dot_decoder模块中） */
|
||
DotDecodeResult result;
|
||
int ret = dot_decoder_process(dot_x, dot_y, dot_count, &result);
|
||
|
||
if (ret == 0) {
|
||
*out_x = result.coordinate_x;
|
||
*out_y = result.coordinate_y;
|
||
*out_page_id = result.page_id;
|
||
return 0;
|
||
}
|
||
|
||
return -1;
|
||
}
|
||
|
||
/* ========== 压力值处理 ========== */
|
||
|
||
/**
|
||
* 处理原始ADC压力值
|
||
* 12位ADC → 归一化并应用非线性映射
|
||
*
|
||
* @param raw_adc 原始ADC值（0-4095）
|
||
* @return 处理后的压力值（0-4095，非线性映射后）
|
||
*/
|
||
static uint16_t process_pressure(uint16_t raw_adc) {
|
||
/* 去除静态偏移（笔尖自重产生的基础压力） */
|
||
const uint16_t offset = 200;
|
||
if (raw_adc < offset) {
|
||
return 0;
|
||
}
|
||
uint16_t adjusted = raw_adc - offset;
|
||
|
||
/* 非线性映射（平方根曲线，使轻触更灵敏） */
|
||
float normalized = (float)adjusted / (4095.0f - offset);
|
||
float mapped = sqrtf(normalized);
|
||
|
||
return (uint16_t)(mapped * 4095);
|
||
}
|
||
|
||
/* ========== 坐标计算主任务 ========== */
|
||
|
||
/**
|
||
* 坐标计算任务（FreeRTOS任务函数）
|
||
*
|
||
* 运行流程：
|
||
* 1. 从图像数据队列接收帧元数据
|
||
* 2. 解码点阵图案获得原始坐标
|
||
* 3. 卡尔曼滤波去噪
|
||
* 4. 抖动检测
|
||
* 5. 坐标打包并放入BLE发送队列
|
||
*/
|
||
void coordinate_task(void *pvParameters) {
|
||
(void)pvParameters;
|
||
|
||
ImageFrameMetadata frame;
|
||
CoordinatePacket packet;
|
||
|
||
/* 初始化卡尔曼滤波器 */
|
||
/* Q=0.1 跟踪速度适中, R=0.5 中等滤波强度 */
|
||
kalman_init(&s_kalman, 0.1f, 0.5f);
|
||
|
||
/* 抖动检测阈值（坐标单位，约0.1mm） */
|
||
const float jitter_threshold = 3.0f;
|
||
|
||
while (1) {
|
||
/* 阻塞等待图像帧数据 */
|
||
if (xQueueReceive(g_image_data_queue, &frame, portMAX_DELAY) != pdTRUE) {
|
||
continue;
|
||
}
|
||
|
||
/* 解码点阵图案 */
|
||
uint32_t raw_x, raw_y, page_id;
|
||
int decode_ret = decode_dot_pattern(frame.pixel_buffer, frame.quality_score,
|
||
&raw_x, &raw_y, &page_id);
|
||
|
||
if (decode_ret != 0) {
|
||
s_decode_failures++;
|
||
continue;
|
||
}
|
||
s_total_decoded++;
|
||
|
||
/* 卡尔曼滤波 */
|
||
float filtered_x, filtered_y;
|
||
kalman_update(&s_kalman, (float)raw_x, (float)raw_y,
|
||
&filtered_x, &filtered_y);
|
||
|
||
/* 抖动检测 */
|
||
if (is_jitter(filtered_x, filtered_y, jitter_threshold)) {
|
||
continue; /* 丢弃抖动数据 */
|
||
}
|
||
|
||
/* 更新最后有效坐标 */
|
||
s_last_valid_x = filtered_x;
|
||
s_last_valid_y = filtered_y;
|
||
|
||
/* 处理压力值 */
|
||
uint16_t pressure = process_pressure(frame.pressure_raw);
|
||
|
||
/* 构建坐标数据包 */
|
||
packet.raw_x = (uint32_t)filtered_x;
|
||
packet.raw_y = (uint32_t)filtered_y;
|
||
packet.pressure = pressure;
|
||
packet.timestamp_ms = frame.timestamp_ms;
|
||
packet.page_id = page_id;
|
||
packet.pen_state = 0; /* 书写中 */
|
||
|
||
/* 放入BLE发送队列（非阻塞，满则丢弃最老的） */
|
||
if (xQueueSend(g_coordinate_queue, &packet, 0) != pdTRUE) {
|
||
/* 队列满，读出一个旧数据再写入 */
|
||
CoordinatePacket dummy;
|
||
xQueueReceive(g_coordinate_queue, &dummy, 0);
|
||
xQueueSend(g_coordinate_queue, &packet, 0);
|
||
}
|
||
}
|
||
}
|