generated from hulk/gd32e23x_template_cmake_vscode
509 lines
15 KiB
C
509 lines
15 KiB
C
/**
|
||
* @file VL53L4CD_Driver.c
|
||
* @brief VL53L4CD 测距传感器驱动实现
|
||
*
|
||
* 初始化配置参考 ST VL53L4CD ULD / Pololu Arduino 库，
|
||
* 使用 16 位寄存器地址，底层走项目现有硬件 I2C0。
|
||
*/
|
||
#include "VL53L4CD_Driver.h"
|
||
#include "i2c.h"
|
||
#include "systick.h"
|
||
|
||
/* 当前传感器 I2C 地址，默认 0x29 */
|
||
static uint8_t g_vl53l4cd_addr = VL53L4CD_I2C_ADDR;
|
||
|
||
/* VL53L4CD 默认配置，0x30..0x87 共 88 字节，来自 ST ULD */
|
||
static const uint8_t g_vl53l4cd_default_config[88] = {
|
||
0x11, 0x02, 0x00, 0x02, 0x08, 0x00, 0x08, 0x10,
|
||
0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0xFF, 0x00,
|
||
0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x20, 0x0B,
|
||
0x00, 0x00, 0x02, 0x14, 0x21, 0x00, 0x00, 0x05,
|
||
0x00, 0x00, 0x00, 0x00, 0xC8, 0x00, 0x00, 0x38,
|
||
0xFF, 0x01, 0x00, 0x08, 0x00, 0x00, 0x01, 0xCC,
|
||
0x07, 0x01, 0xF1, 0x05, 0x00, 0xA0, 0x00, 0x80,
|
||
0x08, 0x38, 0x00, 0x00, 0x00, 0x00, 0x0F, 0x89,
|
||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
|
||
0x07, 0x05, 0x06, 0x06, 0x00, 0x00, 0x02, 0xC7,
|
||
0xFF, 0x9B, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00
|
||
};
|
||
|
||
/**
|
||
* @brief 连续写多个寄存器字节
|
||
* @param[in] reg: 起始 16 位寄存器地址
|
||
* @param[in] data: 数据指针
|
||
* @param[in] length: 字节数
|
||
* @retval true: 成功; false: 失败
|
||
*/
|
||
static bool vl53l4cd_write_regs(uint16_t reg, const uint8_t *data, uint8_t length)
|
||
{
|
||
if (data == NULL || length == 0U) {
|
||
return false;
|
||
}
|
||
return (i2c_write(g_vl53l4cd_addr, reg, (uint8_t *)data, length) == I2C_RESULT_SUCCESS);
|
||
}
|
||
|
||
bool vl53l4cd_read_reg8(uint16_t reg, uint8_t *value)
|
||
{
|
||
if (value == NULL) {
|
||
return false;
|
||
}
|
||
return (i2c_read(g_vl53l4cd_addr, reg, value, 1U) == I2C_RESULT_SUCCESS);
|
||
}
|
||
|
||
bool vl53l4cd_read_reg16(uint16_t reg, uint16_t *value)
|
||
{
|
||
uint8_t buf[2];
|
||
|
||
if (value == NULL) {
|
||
return false;
|
||
}
|
||
if (i2c_read(g_vl53l4cd_addr, reg, buf, 2U) != I2C_RESULT_SUCCESS) {
|
||
return false;
|
||
}
|
||
*value = (uint16_t)(((uint16_t)buf[0] << 8) | buf[1]);
|
||
return true;
|
||
}
|
||
|
||
bool vl53l4cd_write_reg8(uint16_t reg, uint8_t value)
|
||
{
|
||
return vl53l4cd_write_regs(reg, &value, 1U);
|
||
}
|
||
|
||
bool vl53l4cd_write_reg16(uint16_t reg, uint16_t value)
|
||
{
|
||
uint8_t buf[2];
|
||
|
||
buf[0] = (uint8_t)(value >> 8);
|
||
buf[1] = (uint8_t)(value & 0xFF);
|
||
return vl53l4cd_write_regs(reg, buf, 2U);
|
||
}
|
||
|
||
bool vl53l4cd_data_ready(void)
|
||
{
|
||
uint8_t status = 0U;
|
||
|
||
if (!vl53l4cd_read_reg8(VL53L4CD_REG_GPIO_TIO_HV_STATUS, &status)) {
|
||
return false;
|
||
}
|
||
/* 默认中断极性配置下，bit0=0 表示新结果就绪 */
|
||
return ((status & 0x01U) == 0U);
|
||
}
|
||
|
||
bool vl53l4cd_clear_interrupt(void)
|
||
{
|
||
return vl53l4cd_write_reg8(VL53L4CD_REG_SYSTEM_INTERRUPT_CLEAR, 0x01U);
|
||
}
|
||
|
||
/**
|
||
* @brief 等待新的测距结果就绪
|
||
* @param[in] timeout_ms: 超时时间，单位 ms
|
||
* @retval true: 结果就绪; false: 超时或读取失败
|
||
*/
|
||
static bool vl53l4cd_wait_for_data_ready(uint32_t timeout_ms)
|
||
{
|
||
uint32_t elapsed_ms = 0U;
|
||
|
||
while (elapsed_ms < timeout_ms) {
|
||
if (vl53l4cd_data_ready()) {
|
||
return true;
|
||
}
|
||
delay_ms(1);
|
||
elapsed_ms++;
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
bool vl53l4cd_get_result(vl53l4cd_result_t *result)
|
||
{
|
||
static const uint8_t status_rtn[24] = {
|
||
255, 255, 255, 5, 2, 4, 1, 7, 3, 0,
|
||
255, 255, 9, 13, 255, 255, 255, 255, 10, 6,
|
||
255, 255, 11, 12
|
||
};
|
||
uint8_t buf[VL53L4CD_RESULT_BLOCK_SIZE];
|
||
uint8_t raw_status;
|
||
|
||
if (result == NULL) {
|
||
return false;
|
||
}
|
||
if (i2c_read(g_vl53l4cd_addr, VL53L4CD_REG_RESULT_RANGE_STATUS,
|
||
buf, VL53L4CD_RESULT_BLOCK_SIZE) != I2C_RESULT_SUCCESS) {
|
||
return false;
|
||
}
|
||
|
||
raw_status = buf[0] & 0x1FU;
|
||
result->range_status = (raw_status < 24U) ? status_rtn[raw_status]
|
||
: (uint8_t)VL53L4CD_STATUS_UNKNOWN;
|
||
result->number_of_spad = buf[3];
|
||
result->signal_rate_kcps = ((uint32_t)buf[5] << 8 | buf[6]) * 8U;
|
||
result->ambient_rate_kcps = ((uint32_t)buf[7] << 8 | buf[8]) * 8U;
|
||
result->sigma_mm = ((uint16_t)buf[9] << 8 | buf[10]) / 4U;
|
||
result->distance_mm = (uint16_t)(((uint16_t)buf[13] << 8) | buf[14]);
|
||
|
||
if (result->number_of_spad != 0U) {
|
||
result->signal_per_spad_kcps = result->signal_rate_kcps / result->number_of_spad;
|
||
result->ambient_per_spad_kcps = result->ambient_rate_kcps / result->number_of_spad;
|
||
} else {
|
||
result->signal_per_spad_kcps = 0U;
|
||
result->ambient_per_spad_kcps = 0U;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
bool vl53l4cd_set_range_timing(uint8_t timing_budget_ms, uint32_t inter_measurement_ms)
|
||
{
|
||
uint16_t osc_frequency = 0U;
|
||
uint32_t timing_budget_us;
|
||
uint32_t macro_period_us;
|
||
uint32_t tmp;
|
||
uint16_t ls_byte;
|
||
uint8_t ms_byte;
|
||
|
||
if (timing_budget_ms < 10U || timing_budget_ms > 200U) {
|
||
return false;
|
||
}
|
||
if (inter_measurement_ms != 0U) {
|
||
/* 为节省 flash，本版本只支持连续模式 */
|
||
return false;
|
||
}
|
||
if (!vl53l4cd_read_reg16(0x0006U, &osc_frequency) || osc_frequency == 0U) {
|
||
return false;
|
||
}
|
||
|
||
timing_budget_us = (uint32_t)timing_budget_ms * 1000U;
|
||
macro_period_us = ((uint32_t)2304U * (0x40000000U / osc_frequency)) >> 6U;
|
||
|
||
/* 连续模式：测量周期写 0 */
|
||
{
|
||
uint8_t zero[4] = {0U, 0U, 0U, 0U};
|
||
if (!vl53l4cd_write_regs(VL53L4CD_REG_INTERMEASUREMENT_MS, zero, 4U)) {
|
||
return false;
|
||
}
|
||
}
|
||
timing_budget_us -= 2500U;
|
||
|
||
timing_budget_us <<= 12U;
|
||
|
||
/* RANGE_CONFIG_A: 基于 16 的宏周期 */
|
||
tmp = (macro_period_us * 16U) >> 6U;
|
||
ls_byte = (uint16_t)(((timing_budget_us + (tmp >> 1U)) / tmp) - 1U);
|
||
ms_byte = 0U;
|
||
while (ls_byte > 0xFFU) {
|
||
ls_byte >>= 1U;
|
||
ms_byte++;
|
||
}
|
||
if (!vl53l4cd_write_reg16(VL53L4CD_REG_RANGE_CONFIG_A,
|
||
(uint16_t)((uint16_t)ms_byte << 8 | ls_byte))) {
|
||
return false;
|
||
}
|
||
|
||
/* RANGE_CONFIG_B: 基于 12 的宏周期 */
|
||
tmp = (macro_period_us * 12U) >> 6U;
|
||
ls_byte = (uint16_t)(((timing_budget_us + (tmp >> 1U)) / tmp) - 1U);
|
||
ms_byte = 0U;
|
||
while (ls_byte > 0xFFU) {
|
||
ls_byte >>= 1U;
|
||
ms_byte++;
|
||
}
|
||
return vl53l4cd_write_reg16(VL53L4CD_REG_RANGE_CONFIG_B,
|
||
(uint16_t)((uint16_t)ms_byte << 8 | ls_byte));
|
||
}
|
||
|
||
bool vl53l4cd_set_offset(int16_t offset_mm)
|
||
{
|
||
uint16_t reg_value;
|
||
|
||
reg_value = (uint16_t)((int16_t)offset_mm * 4);
|
||
|
||
return vl53l4cd_write_reg16(VL53L4CD_REG_RANGE_OFFSET_MM, reg_value) &&
|
||
vl53l4cd_write_reg16(VL53L4CD_REG_INNER_OFFSET_MM, 0U) &&
|
||
vl53l4cd_write_reg16(VL53L4CD_REG_OUTER_OFFSET_MM, 0U);
|
||
}
|
||
|
||
bool vl53l4cd_get_offset(int16_t *offset_mm)
|
||
{
|
||
uint16_t reg_value;
|
||
int16_t raw;
|
||
|
||
if (offset_mm == NULL) {
|
||
return false;
|
||
}
|
||
if (!vl53l4cd_read_reg16(VL53L4CD_REG_RANGE_OFFSET_MM, ®_value)) {
|
||
return false;
|
||
}
|
||
|
||
raw = (int16_t)reg_value;
|
||
*offset_mm = (int16_t)(raw / 4);
|
||
return true;
|
||
}
|
||
|
||
bool vl53l4cd_calibrate_offset(uint16_t target_distance_mm, uint16_t num_samples,
|
||
int16_t *measured_offset_mm)
|
||
{
|
||
vl53l4cd_result_t result;
|
||
int32_t distance_sum = 0;
|
||
uint16_t valid_samples = 0;
|
||
uint16_t i;
|
||
int16_t average_distance;
|
||
int16_t offset;
|
||
|
||
if (measured_offset_mm == NULL ||
|
||
target_distance_mm < 10U || target_distance_mm > 1000U ||
|
||
num_samples < 5U || num_samples > 255U) {
|
||
return false;
|
||
}
|
||
|
||
if (!vl53l4cd_stop_ranging() ||
|
||
!vl53l4cd_set_offset(0) ||
|
||
!vl53l4cd_start_ranging()) {
|
||
return false;
|
||
}
|
||
|
||
/* 热机 10 帧，丢弃 */
|
||
for (i = 0U; i < 10U; i++) {
|
||
if (!vl53l4cd_wait_for_data_ready(VL53L4CD_DATA_TIMEOUT_MS)) {
|
||
return false;
|
||
}
|
||
(void)vl53l4cd_clear_interrupt();
|
||
(void)vl53l4cd_get_result(&result);
|
||
}
|
||
|
||
/* 统计有效测距样本 */
|
||
for (i = 0U; i < num_samples; i++) {
|
||
if (!vl53l4cd_wait_for_data_ready(VL53L4CD_DATA_TIMEOUT_MS)) {
|
||
return false;
|
||
}
|
||
(void)vl53l4cd_clear_interrupt();
|
||
if (vl53l4cd_get_result(&result) &&
|
||
result.range_status == VL53L4CD_STATUS_RANGE_VALID) {
|
||
distance_sum += (int32_t)result.distance_mm;
|
||
valid_samples++;
|
||
}
|
||
}
|
||
|
||
(void)vl53l4cd_stop_ranging();
|
||
|
||
if (valid_samples == 0U) {
|
||
return false;
|
||
}
|
||
|
||
average_distance = (int16_t)(distance_sum / (int32_t)valid_samples);
|
||
offset = (int16_t)((int16_t)target_distance_mm - average_distance);
|
||
|
||
if (!vl53l4cd_set_offset(offset)) {
|
||
return false;
|
||
}
|
||
|
||
*measured_offset_mm = offset;
|
||
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;
|
||
uint8_t firmware_status = 0U;
|
||
uint32_t elapsed_ms = 0U;
|
||
uint16_t reg;
|
||
uint8_t count;
|
||
|
||
/* 上电后固件需要一点启动时间，先重试读取型号 */
|
||
while (elapsed_ms < VL53L4CD_BOOT_TIMEOUT_MS) {
|
||
if (vl53l4cd_read_reg16(VL53L4CD_REG_IDENTIFICATION_MODEL_ID, &model_id) &&
|
||
model_id == 0xEBAAU) {
|
||
break;
|
||
}
|
||
delay_ms(2);
|
||
elapsed_ms += 2U;
|
||
}
|
||
if (model_id != 0xEBAAU) {
|
||
return false;
|
||
}
|
||
|
||
/* 等待固件启动完成，FIRMWARE__SYSTEM_STATUS == 0x03 */
|
||
elapsed_ms = 0U;
|
||
while (elapsed_ms < VL53L4CD_BOOT_TIMEOUT_MS) {
|
||
if (vl53l4cd_read_reg8(VL53L4CD_REG_FIRMWARE_SYSTEM_STATUS, &firmware_status) &&
|
||
firmware_status == 0x03U) {
|
||
break;
|
||
}
|
||
delay_ms(2);
|
||
elapsed_ms += 2U;
|
||
}
|
||
if (firmware_status != 0x03U) {
|
||
return false;
|
||
}
|
||
|
||
/* I2C/GPIO 上拉配置：本板按 3.3V AVDD 上拉处理 */
|
||
if (!vl53l4cd_write_reg8(0x002DU, 0x00U) ||
|
||
!vl53l4cd_write_reg8(0x002EU, 0x00U) ||
|
||
!vl53l4cd_write_reg8(0x002FU, 0x00U)) {
|
||
return false;
|
||
}
|
||
|
||
/* 写入 0x30..0x87 默认配置，分 3 次块写 */
|
||
for (reg = 0x0030U; reg <= 0x0087U; reg += 30U) {
|
||
uint8_t block[30];
|
||
count = 0U;
|
||
while (count < 30U && (reg + count) <= 0x0087U) {
|
||
block[count] = g_vl53l4cd_default_config[reg + count - 0x0030U];
|
||
count++;
|
||
}
|
||
if (!vl53l4cd_write_regs(reg, block, count)) {
|
||
return false;
|
||
}
|
||
}
|
||
|
||
/* 启动 VHV 校准并等待首个结果 */
|
||
if (!vl53l4cd_write_reg8(VL53L4CD_REG_SYSTEM_START, 0x40U)) {
|
||
return false;
|
||
}
|
||
elapsed_ms = 0U;
|
||
while (!vl53l4cd_data_ready()) {
|
||
delay_ms(5);
|
||
elapsed_ms += 5U;
|
||
if (elapsed_ms >= VL53L4CD_DATA_TIMEOUT_MS) {
|
||
return false;
|
||
}
|
||
}
|
||
|
||
if (!vl53l4cd_clear_interrupt() ||
|
||
!vl53l4cd_stop_ranging() ||
|
||
!vl53l4cd_write_reg8(VL53L4CD_REG_VHV_TIMEOUT_MACROP_LOOP_BOUND, 0x09U) ||
|
||
!vl53l4cd_write_reg8(0x000BU, 0x00U) ||
|
||
!vl53l4cd_write_reg16(0x0024U, 0x0500U)) {
|
||
return false;
|
||
}
|
||
|
||
/* 默认 50ms 时序预算，连续模式 */
|
||
return vl53l4cd_set_range_timing(50U, 0U);
|
||
}
|
||
|
||
bool vl53l4cd_start_ranging(void)
|
||
{
|
||
/* 本驱动固定使用连续模式 */
|
||
return vl53l4cd_write_reg8(VL53L4CD_REG_SYSTEM_START, 0x21U);
|
||
}
|
||
|
||
bool vl53l4cd_stop_ranging(void)
|
||
{
|
||
return vl53l4cd_write_reg8(VL53L4CD_REG_SYSTEM_START, 0x80U);
|
||
}
|