generated from hulk/gd32e23x_template_cmake_vscode
feat(VL53L4CD): 支持透过玻璃串扰补偿、串口校准命令与校准数据Flash存储
This commit is contained in:
@@ -300,6 +300,122 @@ bool vl53l4cd_calibrate_offset(uint16_t target_distance_mm, uint16_t num_samples
|
||||
return true;
|
||||
}
|
||||
|
||||
bool vl53l4cd_set_xtalk(uint16_t xtalk_kcps)
|
||||
{
|
||||
/* ST 固件上限：127 kcps（65536/512），寄存器值单位为 1/512 kcps */
|
||||
if (xtalk_kcps > 127U) {
|
||||
return false;
|
||||
}
|
||||
if (!vl53l4cd_write_reg16(VL53L4CD_REG_XTALK_X_PLANE_GRADIENT_KCPS, 0U) ||
|
||||
!vl53l4cd_write_reg16(VL53L4CD_REG_XTALK_Y_PLANE_GRADIENT_KCPS, 0U)) {
|
||||
return false;
|
||||
}
|
||||
return vl53l4cd_write_reg16(VL53L4CD_REG_XTALK_PLANE_OFFSET_KCPS,
|
||||
(uint16_t)(xtalk_kcps << 9));
|
||||
}
|
||||
|
||||
bool vl53l4cd_get_xtalk(uint16_t *xtalk_kcps)
|
||||
{
|
||||
uint16_t reg_value;
|
||||
|
||||
if (xtalk_kcps == NULL) {
|
||||
return false;
|
||||
}
|
||||
if (!vl53l4cd_read_reg16(VL53L4CD_REG_XTALK_PLANE_OFFSET_KCPS, ®_value)) {
|
||||
return false;
|
||||
}
|
||||
*xtalk_kcps = (uint16_t)(reg_value >> 9);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool vl53l4cd_calibrate_xtalk(uint16_t target_distance_mm, uint16_t num_samples,
|
||||
uint16_t *measured_xtalk_kcps)
|
||||
{
|
||||
vl53l4cd_result_t result;
|
||||
uint32_t distance_sum = 0U;
|
||||
uint32_t signal_sum = 0U;
|
||||
uint32_t spad_sum = 0U;
|
||||
uint32_t valid_samples = 0U;
|
||||
uint16_t avg_distance;
|
||||
uint32_t avg_signal;
|
||||
uint32_t avg_spad;
|
||||
uint32_t signal_ratio;
|
||||
uint16_t xtalk_kcps;
|
||||
uint16_t i;
|
||||
|
||||
if (measured_xtalk_kcps == NULL ||
|
||||
target_distance_mm < 10U || target_distance_mm > 5000U ||
|
||||
num_samples < 5U || num_samples > 255U) {
|
||||
return false;
|
||||
}
|
||||
|
||||
/* 先禁用串扰补偿再采样 */
|
||||
if (!vl53l4cd_set_xtalk(0U)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!vl53l4cd_stop_ranging() || !vl53l4cd_start_ranging()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
/* 热机 10 帧，丢弃 */
|
||||
for (i = 0U; i < 10U; i++) {
|
||||
if (!vl53l4cd_wait_for_data_ready(VL53L4CD_DATA_TIMEOUT_MS)) {
|
||||
(void)vl53l4cd_stop_ranging();
|
||||
return false;
|
||||
}
|
||||
(void)vl53l4cd_clear_interrupt();
|
||||
(void)vl53l4cd_get_result(&result);
|
||||
}
|
||||
|
||||
/* 统计有效测距样本（ST 参考实现：跳过首个采样帧） */
|
||||
for (i = 0U; i < num_samples; i++) {
|
||||
if (!vl53l4cd_wait_for_data_ready(VL53L4CD_DATA_TIMEOUT_MS)) {
|
||||
(void)vl53l4cd_stop_ranging();
|
||||
return false;
|
||||
}
|
||||
(void)vl53l4cd_clear_interrupt();
|
||||
if (i > 0U &&
|
||||
vl53l4cd_get_result(&result) &&
|
||||
result.range_status == VL53L4CD_STATUS_RANGE_VALID) {
|
||||
distance_sum += result.distance_mm;
|
||||
signal_sum += result.signal_rate_kcps;
|
||||
spad_sum += result.number_of_spad;
|
||||
valid_samples++;
|
||||
}
|
||||
}
|
||||
|
||||
(void)vl53l4cd_stop_ranging();
|
||||
|
||||
if (valid_samples == 0U) {
|
||||
return false;
|
||||
}
|
||||
|
||||
/* 定点数计算 xtalk = (1 - 平均距离/目标距离) * 平均信号/平均SPAD数 */
|
||||
avg_distance = (uint16_t)(distance_sum / valid_samples);
|
||||
avg_signal = signal_sum / valid_samples;
|
||||
avg_spad = spad_sum / valid_samples;
|
||||
if (avg_spad == 0U || avg_distance >= target_distance_mm) {
|
||||
return false;
|
||||
}
|
||||
|
||||
/* 乘法上界安全：avg_signal<=524280, (target-avg_distance)<=4990, 乘积 < 2^32 */
|
||||
signal_ratio = (uint32_t)((avg_signal * (uint32_t)(target_distance_mm - avg_distance))
|
||||
/ target_distance_mm);
|
||||
xtalk_kcps = (uint16_t)((signal_ratio + (avg_spad / 2U)) / avg_spad);
|
||||
if (xtalk_kcps > 127U) {
|
||||
/* 超出传感器固件支持的最大串扰值，判定为校准失败 */
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!vl53l4cd_set_xtalk(xtalk_kcps)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
*measured_xtalk_kcps = xtalk_kcps;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool vl53l4cd_init(void)
|
||||
{
|
||||
uint16_t model_id = 0U;
|
||||
|
||||
@@ -0,0 +1,137 @@
|
||||
/**
|
||||
* @file calib_store.c
|
||||
* @brief VL53L4CD 校准数据 Flash 持久化存储实现
|
||||
* @details 校准记录保存在主 Flash 的最后一页（1KB）。页地址根据上电检测到的
|
||||
* Flash 容量（16K/32K/64K）动态计算，同一份固件可适配不同容量器件：
|
||||
* 16K: 0x0800 3C00 32K: 0x0800 7C00 64K: 0x0800 FC00
|
||||
* 记录为 16 字节（2 个双字），带魔数、版本与累加和校验；
|
||||
* 空白 Flash 全 0xFF，不会误判为有效记录。
|
||||
* GD32E230 页擦除为 1KB，编程以 64 位双字为单位。
|
||||
*/
|
||||
#include "gd32e23x.h"
|
||||
#include "calib_store.h"
|
||||
#include "board_config.h"
|
||||
#include "gd32e23x_fmc.h"
|
||||
#include "uart.h"
|
||||
#include <string.h>
|
||||
|
||||
/** 校准记录魔数，"CVL4" 的 ASCII 小端编码 */
|
||||
#define CALIB_RECORD_MAGIC 0x344C5643U
|
||||
#define CALIB_RECORD_VERSION 1U
|
||||
#define CALIB_RECORD_SIZE 16U
|
||||
|
||||
typedef struct {
|
||||
uint32_t magic; /**< 魔数，用于识别有效记录 */
|
||||
uint16_t version; /**< 记录格式版本 */
|
||||
uint16_t flags; /**< 保留标志位 */
|
||||
int16_t offset_mm; /**< 偏移校准值，单位 mm */
|
||||
uint16_t xtalk_kcps; /**< 串扰校准值，单位 kcps */
|
||||
uint16_t checksum; /**< 前 12 字节累加和 */
|
||||
uint16_t pad; /**< 填充到 16 字节 */
|
||||
} calib_record_t;
|
||||
|
||||
/**
|
||||
* @brief 计算校准记录所在页的起始地址（Flash 最后一页）
|
||||
* @retval uint32_t 页起始地址
|
||||
*/
|
||||
static uint32_t calib_page_address(void)
|
||||
{
|
||||
uint32_t size_kb = get_flash_size();
|
||||
|
||||
if (size_kb == 0U) {
|
||||
size_kb = 16U; /* 读取异常时按最小容量处理 */
|
||||
}
|
||||
return 0x08000000U + (size_kb * 1024U) - 1024U;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 计算记录前 12 字节的 16 位累加和
|
||||
* @param[in] rec: 校准记录指针
|
||||
* @retval uint16_t 累加和
|
||||
*/
|
||||
static uint16_t calib_checksum(const calib_record_t *rec)
|
||||
{
|
||||
const uint8_t *p = (const uint8_t *)rec;
|
||||
uint16_t sum = 0U;
|
||||
uint8_t i;
|
||||
|
||||
for (i = 0U; i < 12U; i++) {
|
||||
sum += p[i];
|
||||
}
|
||||
return sum;
|
||||
}
|
||||
|
||||
bool calib_store_load(int16_t *offset_mm, uint16_t *xtalk_kcps)
|
||||
{
|
||||
const calib_record_t *rec = (const calib_record_t *)(uintptr_t)calib_page_address();
|
||||
|
||||
if (offset_mm == NULL || xtalk_kcps == NULL) {
|
||||
return false;
|
||||
}
|
||||
if (rec->magic != CALIB_RECORD_MAGIC || rec->version != CALIB_RECORD_VERSION) {
|
||||
return false;
|
||||
}
|
||||
if (rec->checksum != calib_checksum(rec)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
*offset_mm = rec->offset_mm;
|
||||
*xtalk_kcps = rec->xtalk_kcps;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool calib_store_save(int16_t offset_mm, uint16_t xtalk_kcps)
|
||||
{
|
||||
calib_record_t rec;
|
||||
uint64_t data[2];
|
||||
uint32_t page_addr;
|
||||
fmc_state_enum fmc_state;
|
||||
|
||||
rec.magic = CALIB_RECORD_MAGIC;
|
||||
rec.version = CALIB_RECORD_VERSION;
|
||||
rec.flags = 0U;
|
||||
rec.offset_mm = offset_mm;
|
||||
rec.xtalk_kcps = xtalk_kcps;
|
||||
rec.checksum = 0U;
|
||||
rec.pad = 0U;
|
||||
rec.checksum = calib_checksum(&rec);
|
||||
|
||||
/* GD32E230 按双字（64 位）编程，直接按内存布局拆成两个 64 位 */
|
||||
memcpy(&data[0], &rec, 8U);
|
||||
memcpy(&data[1], (const uint8_t *)&rec + 8U, 8U);
|
||||
|
||||
page_addr = calib_page_address();
|
||||
|
||||
/* Flash 操作期间总线停顿，暂停串口接收避免丢字节 */
|
||||
uart_rx_irq_pause();
|
||||
fmc_unlock();
|
||||
|
||||
fmc_state = fmc_page_erase(page_addr);
|
||||
if (fmc_state == FMC_READY) {
|
||||
fmc_state = fmc_doubleword_program(page_addr, data[0]);
|
||||
if (fmc_state == FMC_READY) {
|
||||
fmc_state = fmc_doubleword_program(page_addr + 8U, data[1]);
|
||||
}
|
||||
}
|
||||
|
||||
fmc_lock();
|
||||
uart_rx_irq_resume();
|
||||
|
||||
return (fmc_state == FMC_READY);
|
||||
}
|
||||
|
||||
bool calib_store_erase(void)
|
||||
{
|
||||
uint32_t page_addr = calib_page_address();
|
||||
fmc_state_enum fmc_state;
|
||||
|
||||
uart_rx_irq_pause();
|
||||
fmc_unlock();
|
||||
|
||||
fmc_state = fmc_page_erase(page_addr);
|
||||
|
||||
fmc_lock();
|
||||
uart_rx_irq_resume();
|
||||
|
||||
return (fmc_state == FMC_READY);
|
||||
}
|
||||
+313
-11
@@ -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 软件重启命令
|
||||
|
||||
+136
-28
@@ -1,35 +1,35 @@
|
||||
/*!
|
||||
\file main.c
|
||||
\brief running LED
|
||||
|
||||
|
||||
\version 2025-02-10, V2.4.0, demo for GD32E23x
|
||||
*/
|
||||
|
||||
/*
|
||||
Copyright (c) 2025, GigaDevice Semiconductor Inc.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without modification,
|
||||
Redistribution and use in source and binary forms, with or without modification,
|
||||
are permitted provided that the following conditions are met:
|
||||
|
||||
1. Redistributions of source code must retain the above copyright notice, this
|
||||
1. Redistributions of source code must retain the above copyright notice, this
|
||||
list of conditions and the following disclaimer.
|
||||
2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the documentation
|
||||
2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the documentation
|
||||
and/or other materials provided with the distribution.
|
||||
3. Neither the name of the copyright holder nor the names of its contributors
|
||||
may be used to endorse or promote products derived from this software without
|
||||
3. Neither the name of the copyright holder nor the names of its contributors
|
||||
may be used to endorse or promote products derived from this software without
|
||||
specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
|
||||
IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
|
||||
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
|
||||
NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
|
||||
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
|
||||
WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
|
||||
OF SUCH DAMAGE.
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "gd32e23x.h"
|
||||
@@ -38,10 +38,100 @@ OF SUCH DAMAGE.
|
||||
#include "uart_ring_buffer.h"
|
||||
#include "led.h"
|
||||
#include "command.h"
|
||||
#include <stdio.h>
|
||||
#include "i2c.h"
|
||||
#include "board_config.h"
|
||||
#include "VL53L4CD_Driver.h"
|
||||
#include "calib_store.h"
|
||||
|
||||
/*!
|
||||
\brief 向测距串口发送一个字节
|
||||
\param[in] byte: 待发送字节
|
||||
\param[out] none
|
||||
\retval none
|
||||
*/
|
||||
static void uart_put_byte(uint8_t byte)
|
||||
{
|
||||
while (usart_flag_get(UART_PHY, USART_FLAG_TBE) == RESET) {}
|
||||
usart_data_transmit(UART_PHY, byte);
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief 向测距串口发送以 NUL 结尾的字符串
|
||||
\param[in] s: 字符串指针
|
||||
\param[out] none
|
||||
\retval none
|
||||
*/
|
||||
static void uart_write_str(const char *s)
|
||||
{
|
||||
while (*s != '\0') {
|
||||
uart_put_byte((uint8_t)*s++);
|
||||
}
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief 以十进制输出无符号整数（替代 printf，节省 libc 体积）
|
||||
\param[in] value: 待输出的数值
|
||||
\param[out] none
|
||||
\retval none
|
||||
*/
|
||||
static void uart_write_u32(uint32_t value)
|
||||
{
|
||||
char temp[10];
|
||||
uint8_t i = 0U;
|
||||
|
||||
if (value == 0U) {
|
||||
uart_put_byte('0');
|
||||
return;
|
||||
}
|
||||
while (value > 0U) {
|
||||
temp[i++] = (char)('0' + (value % 10U));
|
||||
value /= 10U;
|
||||
}
|
||||
while (i > 0U) {
|
||||
uart_put_byte((uint8_t)temp[--i]);
|
||||
}
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief 以十进制输出有符号整数
|
||||
\param[in] value: 待输出的数值
|
||||
\param[out] none
|
||||
\retval none
|
||||
*/
|
||||
static void uart_write_i32(int32_t value) __attribute__((unused));
|
||||
static void uart_write_i32(int32_t value)
|
||||
{
|
||||
uint32_t magnitude;
|
||||
|
||||
if (value < 0) {
|
||||
uart_put_byte('-');
|
||||
magnitude = (uint32_t)(-value);
|
||||
} else {
|
||||
magnitude = (uint32_t)value;
|
||||
}
|
||||
uart_write_u32(magnitude);
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief 输出一帧测距结果，格式与原 printf 版本逐字节一致
|
||||
\param[in] r: 测距结果指针
|
||||
\param[out] none
|
||||
\retval none
|
||||
*/
|
||||
static void report_result(const vl53l4cd_result_t *r)
|
||||
{
|
||||
uart_write_str("Distance: ");
|
||||
uart_write_u32(r->distance_mm);
|
||||
uart_write_str(" mm | status: ");
|
||||
uart_write_u32(r->range_status);
|
||||
uart_write_str(" | signal: ");
|
||||
uart_write_u32(r->signal_rate_kcps);
|
||||
uart_write_str(" kcps | ambient: ");
|
||||
uart_write_u32(r->ambient_rate_kcps);
|
||||
uart_write_str(" kcps | sigma: ");
|
||||
uart_write_u32(r->sigma_mm);
|
||||
uart_write_str(" mm\r\n");
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief main function
|
||||
@@ -84,21 +174,41 @@ int main(void)
|
||||
if (vl53l4cd_calibrate_offset(VL53L4CD_CALIB_TARGET_MM,
|
||||
VL53L4CD_CALIB_SAMPLES,
|
||||
&cal_offset)) {
|
||||
printf("VL53L4CD offset calibrated: %d mm\r\n", cal_offset);
|
||||
uart_write_str("VL53L4CD offset calibrated: ");
|
||||
uart_write_i32(cal_offset);
|
||||
uart_write_str(" mm\r\n");
|
||||
} else {
|
||||
printf("VL53L4CD offset calibration failed\r\n");
|
||||
uart_write_str("VL53L4CD offset calibration failed\r\n");
|
||||
}
|
||||
#else
|
||||
(void)vl53l4cd_set_offset(VL53L4CD_CALIB_OFFSET_MM);
|
||||
int16_t stored_offset = 0;
|
||||
uint16_t stored_xtalk = 0;
|
||||
bool applied = false;
|
||||
|
||||
#if VL53L4CD_PERSIST_CALIB == CFG_ENABLE
|
||||
/* Flash 末页有有效校准数据时优先生效（M105 保存 / M107 擦除） */
|
||||
if (calib_store_load(&stored_offset, &stored_xtalk)) {
|
||||
vl53l4cd_set_offset(stored_offset);
|
||||
vl53l4cd_set_xtalk(stored_xtalk);
|
||||
applied = true;
|
||||
}
|
||||
#endif
|
||||
if (!applied) {
|
||||
vl53l4cd_set_offset(VL53L4CD_CALIB_OFFSET_MM);
|
||||
#if VL53L4CD_GLASS_XTALK == CFG_ENABLE
|
||||
/* 透过玻璃安装：写入编译期固定串扰补偿 */
|
||||
vl53l4cd_set_xtalk(VL53L4CD_GLASS_XTALK_KCPS);
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
vl53l4cd_start_ranging();
|
||||
printf("VL53L4CD ready\r\n");
|
||||
uart_write_str("VL53L4CD ready\r\n");
|
||||
} else {
|
||||
printf("VL53L4CD init failed\r\n");
|
||||
uart_write_str("VL53L4CD init failed\r\n");
|
||||
}
|
||||
|
||||
/* ========== Command Testing ========== */
|
||||
|
||||
|
||||
/* ========== */
|
||||
|
||||
while(1){
|
||||
@@ -109,12 +219,10 @@ int main(void)
|
||||
|
||||
vl53l4cd_clear_interrupt();
|
||||
if (vl53l4cd_get_result(&result)) {
|
||||
printf("Distance: %u mm | status: %u | signal: %lu kcps | ambient: %lu kcps | sigma: %u mm\r\n",
|
||||
result.distance_mm,
|
||||
result.range_status,
|
||||
(unsigned long)result.signal_rate_kcps,
|
||||
(unsigned long)result.ambient_rate_kcps,
|
||||
result.sigma_mm);
|
||||
command_set_last_result(&result); /* 供 M3 命令查询 */
|
||||
if (get_sensor_report_enabled()) {
|
||||
report_result(&result);
|
||||
}
|
||||
}
|
||||
delay_ms(500);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user