generated from hulk/gd32e23x_template_cmake_vscode
hulk
ef37553b18
- 新增 vl53l4cd_set_offset/get_offset/calibrate_offset - main 支持开机自动校准或启动时写回固定偏移 - 当前44mm哑光目标校准结果 offset=-9mm - 同步更新 README 与 CHANGES
393 lines
11 KiB
C
393 lines
11 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_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);
|
|
}
|