hulk/GD32E230_VL53L4CD
1
0
Fork 0
generated from hulk/gd32e23x_template_cmake_vscode
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

feat(VL53L4CD): 支持透过玻璃串扰补偿、串口校准命令与校准数据Flash存储

Browse Source
This commit is contained in:
hulk
2026-09-07 00:29:41 +08:00
parent ef37553b18
commit ec412b1ffd
13 changed files with 966 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Src/command.c
+313 -11
View File
@@ -21,6 +21,7 @@
#include "gd32e23x_usart.h"
#include "i2c.h"
#include "core_cm23.h"
#include "calib_store.h"
/* ============================================================================
* 协议格式说明
@@ -88,6 +89,13 @@ static const uint8_t s_report_status_ok[] __attribute__((unused)) = { 'o', 'k' }
static const uint8_t s_report_status_err[] = { 'e','r','r' }; /**< 错误响应数据 */
/** @} */
/** @name 运行状态共享数据
* @{ */
static bool s_report_enabled = true; /**< 测距上报开关，M1/M2 控制 */
static vl53l4cd_result_t s_last_result; /**< 最近一帧测距结果，主循环推送 */
static bool s_last_result_valid = false; /**< 最近一帧结果是否有效 */
/** @} */
void system_software_reset(void)
{
// 确保所有待处理的内存访问完成
@@ -111,6 +119,42 @@ void system_software_reset(void)
* 公共接口函数
* ============================================================================ */
bool get_sensor_report_enabled(void)
{
return s_report_enabled;
}
void set_sensor_report_enabled(bool enabled)
{
s_report_enabled = enabled;
if (enabled) {
led_on();
} else {
led_off();
}
}
void command_set_last_result(const vl53l4cd_result_t *result)
{
if (result != NULL) {
s_last_result = *result;
s_last_result_valid = true;
}
}
/**
* @brief 发送 "前缀=数值" 格式的成功响应，如 "off=-9"、"xt=12"。
* @param prefix 数值前缀字符。
* @param value 有符号数值。
*/
static void send_resp_prefixed(char prefix, int32_t value);
/**
* @brief 发送 "d=<距离>;s=<状态>" 格式的测距结果响应。
* @param result 测距结果指针。
*/
static void send_resp_result(const vl53l4cd_result_t *result);
/**
* @brief 计算协议包的 8 位累加校验值（Checksum）。
* @details 对输入缓冲区逐字节累加并取低 8 位，累加范围为 data[1] 至 data[len-2]，
@@ -150,7 +194,7 @@ static uint8_t command_sum_crc_calc(const uint8_t *data, uint8_t len)
static void send_response(uint8_t type, const uint8_t *payload, uint8_t len)
{
uint8_t buf_len = (uint8_t)(3 + len + 1);
uint8_t buf[16]; // 简单场景足够，必要时可增大
uint8_t buf[20]; // 简单场景足够，必要时可增大
if (buf_len > sizeof(buf)) return; // 防御
buf[0] = RESP_HEADER;
@@ -322,6 +366,62 @@ static uint8_t parse_uint_dec(const uint8_t *s, uint8_t n, uint32_t *out)
return i;
}
/**
* @brief 从缓冲区解析十进制有符号整数。
* @details 支持可选的正负号前缀（'-' 或 '+'），数字部分同 parse_uint_dec()。
* @param s 指向待解析字符缓冲区的起始位置。
* @param n 允许解析的最大字符数。
* @param out 输出参数，存储解析结果，可为NULL。
* @return uint8_t 实际消耗的字符数（含符号位），首字符不是数字时返回 0。
* @ingroup Command
*/
static uint8_t parse_int_dec(const uint8_t *s, uint8_t n, int32_t *out)
{
uint8_t used = 0;
bool negative = false;
uint32_t magnitude = 0;
uint8_t digits;
if (n > 0 && (s[0] == '-' || s[0] == '+')) {
negative = (s[0] == '-');
used = 1U;
}
digits = parse_uint_dec(&s[used], (uint8_t)(n - used), &magnitude);
if (digits == 0U) {
return 0;
}
used = (uint8_t)(used + digits);
if (out != NULL) {
*out = negative ? -(int32_t)magnitude : (int32_t)magnitude;
}
return used;
}
/**
* @brief 在命令字符串中查找带符号参数的值
* @param cmd 指向命令起始位置（Mxxx 之后）
* @param cmd_len 命令剩余长度
* @param key 要查找的参数字符（如 'S'）
* @param value 输出参数，存储解析到的值
* @return bool 是否找到该参数
*/
static bool find_int_parameter_value(const uint8_t *cmd, uint8_t cmd_len, char key, int32_t *value)
{
for (uint8_t i = 0; i < cmd_len; i++) {
if (cmd[i] == (uint8_t)key) {
uint8_t param_idx = (uint8_t)(i + 1);
if (param_idx >= cmd_len) return false;
if (parse_int_dec(&cmd[param_idx], (uint8_t)(cmd_len - param_idx), value) > 0) {
return true;
}
return false;
}
}
return false;
}
/**
* @brief 通用T参数解析函数
* @details 解析命令中的T参数（定时参数），格式为T<数字>
@@ -390,6 +490,41 @@ static bool __attribute__((unused)) find_parameter_value(const uint8_t *cmd, uin
return false;
}
/**
* @brief 发送 "前缀=数值" 格式的成功响应，如 "off=-9"、"xt=12"。
* @param prefix 数值前缀字符。
* @param value 有符号数值。
*/
static void send_resp_prefixed(char prefix, int32_t value)
{
char payload[13];
uint8_t n = 0;
payload[n++] = prefix;
payload[n++] = '=';
n += int_to_str(value, &payload[n]);
send_response(RESP_TYPE_OK, (const uint8_t *)payload, n);
}
/**
* @brief 发送 "d=<距离>;s=<状态>" 格式的测距结果响应。
* @param result 测距结果指针。
*/
static void send_resp_result(const vl53l4cd_result_t *result)
{
char payload[13];
uint8_t n = 0;
payload[n++] = 'd';
payload[n++] = '=';
n += uint_to_str(result->distance_mm, &payload[n]);
payload[n++] = ';';
payload[n++] = 's';
payload[n++] = '=';
n += uint_to_str(result->range_status, &payload[n]);
send_response(RESP_TYPE_OK, (const uint8_t *)payload, n);
}
/* ============================================================================
* 命令处理函数
* ============================================================================ */
@@ -397,18 +532,30 @@ static bool __attribute__((unused)) find_parameter_value(const uint8_t *cmd, uin
/**
* @brief 解析并处理完整的命令帧。
* @details 处理经过协议校验的完整命令帧，支持以下命令格式：
* - 无参数命令：M<数字>（如 M1、M2、M10、M201）
* - 带参数命令：M<数字>S<参数>（如 M100S123，参数为十进制）
*
* - 无参数命令：M<数字>（如 M1、M2、M999）
* - 带参数命令：M<数字>S<参数>（如 M101S-9、M102S44，参数为十进制）
*
* 支持的命令：
* - M1: 开启LED，启用传感器上报
* - M2: 关闭LED，禁用传感器上报
* - M100S<value>: 设置PWM值（示例）
*
* - M1 开启测距上报（LED 亮）
* - M2 关闭测距上报（LED 灭）
* - M3 立即返回最近一帧测距结果 "d=830;s=0"
* - M101 读取当前偏移量 "off=-9"
* - M101S 写入偏移量（mm，立即生效）
* - M102S 执行偏移校准（S=目标实际距离 mm，默认 44）
* - M103 读取当前串扰补偿 "xt=12"
* - M103S 写入串扰补偿（kcps，0..127，立即生效）
* - M104S 执行串扰校准（S=玻璃后方目标距离 mm，默认 500）
* - M105 保存当前偏移+串扰到 Flash
* - M106 从 Flash 读取并应用校准数据
* - M107 擦除 Flash 校准数据并清零偏移/串扰
* - M888 软件重启
* - M999 输出固件版本号
* - M9999 进入 OTA/Bootloader 模式
*
* @param frame 指向完整命令帧的缓冲区（从包头0xD5开始）。
* @param len 命令帧总长度（字节）。
* @note 函数内部进行帧格式校验，格式错误时自动发送错误响应。
* @warning 假设输入帧已通过基本协议校验（包头、类型、CRC等）。
* @warning M102/M104 校准命令为阻塞执行，耗时约 2 秒，期间主循环上报暂停。
* @ingroup Command
*/
@@ -440,9 +587,164 @@ void handle_command(const uint8_t *frame, uint8_t len) {
cmd_index = (uint8_t)(cmd_index + used_base_cmd); // 更新索引到命令后
switch (base_cmd) {
case 1u: // M1
/* ==========================================
* M1 开启测距上报（LED 亮）
* M2 关闭测距上报（LED 灭）
* ========================================== */
case 1u:
set_sensor_report_enabled(true);
send_response(RESP_TYPE_OK, s_report_status_ok, sizeof(s_report_status_ok));
return;
case 2u:
set_sensor_report_enabled(false);
send_response(RESP_TYPE_OK, s_report_status_ok, sizeof(s_report_status_ok));
return;
/* ==========================================
* M3 立即返回最近一帧测距结果 "d=830;s=0"
* （上报关闭时也可用于查询）
* ========================================== */
case 3u:
if (s_last_result_valid) {
send_resp_result(&s_last_result);
} else {
send_response(RESP_TYPE_ERR, s_report_status_err, sizeof(s_report_status_err));
}
return;
/* ==========================================
* M101 读取当前偏移量 "off=-9"
* M101S<mm> 写入偏移量（立即生效，如 M101S-9）
* ========================================== */
case 101u: {
int32_t offset_param;
int16_t offset_mm;
if (find_int_parameter_value(&cmd[cmd_index], (uint8_t)(cmd_len - cmd_index), 'S', &offset_param)) {
if (offset_param < -4095 || offset_param > 4095) {
send_response(RESP_TYPE_PARAM_ERR, s_report_status_err, sizeof(s_report_status_err));
return;
}
if (!vl53l4cd_set_offset((int16_t)offset_param)) {
send_response(RESP_TYPE_ERR, s_report_status_err, sizeof(s_report_status_err));
return;
}
}
if (vl53l4cd_get_offset(&offset_mm)) {
send_resp_prefixed('o', offset_mm);
COMMAND_DEBUG("offset %d mm", offset_mm);
} else {
send_response(RESP_TYPE_ERR, s_report_status_err, sizeof(s_report_status_err));
}
return;
}
/* ==========================================
* M102[S<mm>] 执行偏移校准
* 目标放在已知距离，可选 S 指定距离（默认 VL53L4CD_CALIB_TARGET_MM），
* 校准需数秒，期间上报暂停，成功返回 "off=-9"
* ========================================== */
case 102u: {
uint32_t target_param;
int16_t measured_offset;
if (!find_parameter_value(&cmd[cmd_index], (uint8_t)(cmd_len - cmd_index), 'S', &target_param)) {
target_param = VL53L4CD_CALIB_TARGET_MM;
}
if (vl53l4cd_calibrate_offset((uint16_t)target_param, VL53L4CD_CALIB_SAMPLES, &measured_offset)) {
send_resp_prefixed('o', measured_offset);
} else {
send_response(RESP_TYPE_ERR, s_report_status_err, sizeof(s_report_status_err));
}
return;
}
/* ==========================================
* M103 读取当前串扰补偿值 "xt=12"
* M103S<kcps> 写入串扰补偿值 0..127（立即生效）
* ========================================== */
case 103u: {
uint32_t xtalk_param;
uint16_t xtalk_kcps;
if (find_parameter_value(&cmd[cmd_index], (uint8_t)(cmd_len - cmd_index), 'S', &xtalk_param)) {
if (xtalk_param > 127U) {
send_response(RESP_TYPE_PARAM_ERR, s_report_status_err, sizeof(s_report_status_err));
return;
}
if (!vl53l4cd_set_xtalk((uint16_t)xtalk_param)) {
send_response(RESP_TYPE_ERR, s_report_status_err, sizeof(s_report_status_err));
return;
}
}
if (vl53l4cd_get_xtalk(&xtalk_kcps)) {
send_resp_prefixed('x', (int32_t)xtalk_kcps);
COMMAND_DEBUG("xtalk %u kcps", xtalk_kcps);
} else {
send_response(RESP_TYPE_ERR, s_report_status_err, sizeof(s_report_status_err));
}
return;
}
/* ==========================================
* M104[S<mm>] 执行串扰校准（透过玻璃）
* 玻璃后方目标放在已知距离（默认 VL53L4CD_XTALK_CALIB_TARGET_MM），
* 建议先完成 M102 偏移校准；成功返回 "xt=12"
* ========================================== */
case 104u: {
uint32_t target_param;
uint16_t measured_xtalk;
if (!find_parameter_value(&cmd[cmd_index], (uint8_t)(cmd_len - cmd_index), 'S', &target_param)) {
target_param = VL53L4CD_XTALK_CALIB_TARGET_MM;
}
if (vl53l4cd_calibrate_xtalk((uint16_t)target_param, VL53L4CD_XTALK_CALIB_SAMPLES, &measured_xtalk)) {
send_resp_prefixed('x', (int32_t)measured_xtalk);
} else {
send_response(RESP_TYPE_ERR, s_report_status_err, sizeof(s_report_status_err));
}
return;
}
/* ==========================================
* M105 保存当前偏移+串扰到 Flash（掉电保持）
* M106 从 Flash 读取并应用校准数据
* M107 擦除 Flash 校准数据并清零当前偏移/串扰
* ========================================== */
case 105u: {
int16_t offset_mm;
uint16_t xtalk_kcps;
if (vl53l4cd_get_offset(&offset_mm) && vl53l4cd_get_xtalk(&xtalk_kcps) &&
calib_store_save(offset_mm, xtalk_kcps)) {
send_response(RESP_TYPE_OK, s_report_status_ok, sizeof(s_report_status_ok));
return;
} else {
send_response(RESP_TYPE_ERR, s_report_status_err, sizeof(s_report_status_err));
}
return;
}
case 106u: {
int16_t offset_mm;
uint16_t xtalk_kcps;
if (calib_store_load(&offset_mm, &xtalk_kcps) &&
vl53l4cd_set_offset(offset_mm) && vl53l4cd_set_xtalk(xtalk_kcps)) {
send_resp_prefixed('o', offset_mm);
} else {
send_response(RESP_TYPE_ERR, s_report_status_err, sizeof(s_report_status_err));
}
return;
}
case 107u:
if (calib_store_erase() && vl53l4cd_set_offset(0) && vl53l4cd_set_xtalk(0)) {
send_response(RESP_TYPE_OK, s_report_status_ok, sizeof(s_report_status_ok));
} else {
send_response(RESP_TYPE_ERR, s_report_status_err, sizeof(s_report_status_err));
}
return;
/* ==========================================
* M888 软件重启命令