217 lines
8.0 KiB
C
217 lines
8.0 KiB
C
#include "vl53_board.h"
|
||
#include "FreeRTOS.h"
|
||
#include "task.h"
|
||
#include "robot_params.h"
|
||
#include "vl53_calibration_config.h"
|
||
|
||
/* ================= 卡尔曼滤波底层实现 ================= */
|
||
static void vl53_kalman_init(Vl53Kalman_t *kf, float q, float r) {
|
||
kf->x = 0.0f;
|
||
kf->p = 1.0f;
|
||
kf->q = q;
|
||
kf->r = r;
|
||
kf->initialized = 0;
|
||
}
|
||
|
||
static float vl53_kalman_update(Vl53Kalman_t *kf, float measurement) {
|
||
if (kf->initialized == 0) {
|
||
kf->x = measurement;
|
||
kf->initialized = 1;
|
||
return kf->x;
|
||
}
|
||
kf->p = kf->p + kf->q;
|
||
float k = kf->p / (kf->p + kf->r);
|
||
kf->x = kf->x + k * (measurement - kf->x);
|
||
kf->p = (1.0f - k) * kf->p;
|
||
return kf->x;
|
||
}
|
||
|
||
static const Vl53RuntimeCalibration_t *vl53_get_runtime_calibration(uint8_t id)
|
||
{
|
||
switch (id) {
|
||
case 0: return &k_vl53_left_calibration[0];
|
||
case 1: return &k_vl53_left_calibration[1];
|
||
case 2: return &k_vl53_right_calibration[0];
|
||
case 3: return &k_vl53_right_calibration[1];
|
||
default: return NULL;
|
||
}
|
||
}
|
||
|
||
static VL53L0X_Error vl53_apply_runtime_calibration(VL53L0X_DEV dev, uint8_t id)
|
||
{
|
||
const Vl53RuntimeCalibration_t *cal = vl53_get_runtime_calibration(id);
|
||
VL53L0X_Error status;
|
||
|
||
if (cal == NULL) return VL53L0X_ERROR_NONE;
|
||
|
||
if (cal->offset_calibrated != 0U) {
|
||
status = VL53L0X_SetOffsetCalibrationDataMicroMeter(dev, cal->offset_micro_meters);
|
||
if (status != VL53L0X_ERROR_NONE) return status;
|
||
}
|
||
|
||
if (cal->xtalk_calibrated != 0U) {
|
||
status = VL53L0X_SetXTalkCompensationRateMegaCps(dev, cal->xtalk_compensation_rate_mcps);
|
||
if (status != VL53L0X_ERROR_NONE) return status;
|
||
|
||
status = VL53L0X_SetXTalkCompensationEnable(dev, 1u);
|
||
if (status != VL53L0X_ERROR_NONE) return status;
|
||
} else {
|
||
status = VL53L0X_SetXTalkCompensationEnable(dev, 0u);
|
||
if (status != VL53L0X_ERROR_NONE) return status;
|
||
}
|
||
|
||
return VL53L0X_ERROR_NONE;
|
||
}
|
||
|
||
/* ================= ST 官方配置序列 ================= */
|
||
static VL53L0X_Error vl53_do_static_init(VL53L0X_DEV dev, uint32_t timing_budget_us)
|
||
{
|
||
VL53L0X_Error status;
|
||
uint32_t ref_spad_count = 0u;
|
||
uint8_t is_aperture_spads = 0u;
|
||
uint8_t vhv_settings = 0u;
|
||
uint8_t phase_cal = 0u;
|
||
|
||
status = VL53L0X_StaticInit(dev);
|
||
if (status != VL53L0X_ERROR_NONE) return status;
|
||
|
||
status = VL53L0X_PerformRefSpadManagement(dev, &ref_spad_count, &is_aperture_spads);
|
||
if (status != VL53L0X_ERROR_NONE) return status;
|
||
|
||
status = VL53L0X_PerformRefCalibration(dev, &vhv_settings, &phase_cal);
|
||
if (status != VL53L0X_ERROR_NONE) return status;
|
||
|
||
status = VL53L0X_SetDeviceMode(dev, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING);
|
||
if (status != VL53L0X_ERROR_NONE) return status;
|
||
|
||
status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(dev, timing_budget_us);
|
||
if (status != VL53L0X_ERROR_NONE) return status;
|
||
|
||
/* 连续测量采用 back-to-back 模式，让下一次测量在上一帧完成后立即开始。
|
||
* 这样任务轮询周期变快时，能尽可能拿到最新帧，而不是被固定间隔再次拖慢。 */
|
||
status = VL53L0X_SetInterMeasurementPeriodMilliSeconds(dev, 0u);
|
||
if (status != VL53L0X_ERROR_NONE) return status;
|
||
|
||
status = VL53L0X_SetLimitCheckEnable(dev, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1u);
|
||
if (status != VL53L0X_ERROR_NONE) return status;
|
||
|
||
status = VL53L0X_SetLimitCheckEnable(dev, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1u);
|
||
if (status != VL53L0X_ERROR_NONE) return status;
|
||
|
||
return VL53L0X_ERROR_NONE;
|
||
}
|
||
|
||
/* ================= 核心管理器 API ================= */
|
||
VL53L0X_Error Vl53Board_Init(Vl53Board_t *board, const Vl53BoardHwCfg_t *hw_cfgs, uint8_t count, uint32_t timing_budget_us)
|
||
{
|
||
if ((board == NULL) || (hw_cfgs == NULL) || (count == 0) || (count > VL53_MAX_DEVS_PER_BOARD)) {
|
||
return VL53L0X_ERROR_INVALID_PARAMS;
|
||
}
|
||
|
||
memset(board, 0, sizeof(Vl53Board_t));
|
||
board->dev_count = count;
|
||
board->timing_budget_us = (timing_budget_us == 0u) ? 33000u : timing_budget_us;
|
||
|
||
for (uint8_t i = 0; i < count; i++) {
|
||
board->dev[i].name = hw_cfgs[i].name;
|
||
board->dev[i].id = hw_cfgs[i].id;
|
||
board->dev[i].comms_type = 1u;
|
||
board->dev[i].comms_speed_khz = 400u;
|
||
board->dev[i].is_present = 0u;
|
||
|
||
VL53L0X_PlatformAttachBus(&board->dev[i], hw_cfgs[i].hi2c, VL53L0X_DEFAULT_ADDR_8BIT, 100u, NULL);
|
||
VL53L0X_PlatformAttachPins(&board->dev[i], hw_cfgs[i].xshut_port, hw_cfgs[i].xshut_pin, NULL, 0);
|
||
|
||
VL53L0X_PlatformSetXShut(&board->dev[i], GPIO_PIN_RESET);
|
||
|
||
/* 初始化卡尔曼滤波器：Q/R 从 robot_params.h 读取 */
|
||
vl53_kalman_init(&board->kf[i], PARAM_VL53_KALMAN_Q, PARAM_VL53_KALMAN_R);
|
||
}
|
||
|
||
vTaskDelay(pdMS_TO_TICKS(10));
|
||
|
||
for (uint8_t i = 0; i < count; i++) {
|
||
if (VL53L0X_PlatformSetXShut(&board->dev[i], GPIO_PIN_SET) != VL53L0X_ERROR_NONE) continue;
|
||
vTaskDelay(pdMS_TO_TICKS(20));
|
||
|
||
board->dev[i].I2cDevAddr = VL53L0X_DEFAULT_ADDR_8BIT;
|
||
if (VL53L0X_PlatformChangeAddress(&board->dev[i], hw_cfgs[i].runtime_addr_8bit) != VL53L0X_ERROR_NONE) continue;
|
||
if (VL53L0X_DataInit(&board->dev[i]) != VL53L0X_ERROR_NONE) continue;
|
||
if (vl53_do_static_init(&board->dev[i], board->timing_budget_us) != VL53L0X_ERROR_NONE) continue;
|
||
if (vl53_apply_runtime_calibration(&board->dev[i], hw_cfgs[i].id) != VL53L0X_ERROR_NONE) continue;
|
||
|
||
board->init_mask |= (uint8_t)(1u << i);
|
||
board->dev[i].is_present = 1u;
|
||
}
|
||
|
||
return VL53L0X_ERROR_NONE;
|
||
}
|
||
|
||
VL53L0X_Error Vl53Board_StartContinuous(Vl53Board_t *board)
|
||
{
|
||
if (board == NULL) return VL53L0X_ERROR_INVALID_PARAMS;
|
||
for (uint8_t i = 0; i < board->dev_count; i++) {
|
||
if (board->init_mask & (1u << i)) VL53L0X_StartMeasurement(&board->dev[i]);
|
||
}
|
||
return VL53L0X_ERROR_NONE;
|
||
}
|
||
|
||
VL53L0X_Error Vl53Board_StopContinuous(Vl53Board_t *board)
|
||
{
|
||
if (board == NULL) return VL53L0X_ERROR_INVALID_PARAMS;
|
||
for (uint8_t i = 0; i < board->dev_count; i++) {
|
||
if (board->init_mask & (1u << i)) VL53L0X_StopMeasurement(&board->dev[i]);
|
||
}
|
||
return VL53L0X_ERROR_NONE;
|
||
}
|
||
|
||
VL53L0X_Error Vl53Board_ReadAll(Vl53Board_t *board, Vl53BoardSnapshot_t *snapshot)
|
||
{
|
||
if (board == NULL || snapshot == NULL) return VL53L0X_ERROR_INVALID_PARAMS;
|
||
|
||
memset(snapshot, 0, sizeof(Vl53BoardSnapshot_t));
|
||
snapshot->tick_ms = xTaskGetTickCount() * portTICK_PERIOD_MS;
|
||
|
||
for (uint8_t i = 0; i < board->dev_count; i++) {
|
||
if ((board->init_mask & (1u << i)) == 0u) {
|
||
snapshot->range_status[i] = 255u;
|
||
continue;
|
||
}
|
||
|
||
uint8_t ready = 0u;
|
||
if (VL53L0X_GetMeasurementDataReady(&board->dev[i], &ready) != VL53L0X_ERROR_NONE) continue;
|
||
|
||
if (ready) {
|
||
VL53L0X_RangingMeasurementData_t data;
|
||
memset(&data, 0, sizeof(data));
|
||
|
||
if (VL53L0X_GetRangingMeasurementData(&board->dev[i], &data) == VL53L0X_ERROR_NONE) {
|
||
/* 1. 写入原始数据 */
|
||
snapshot->range_mm[i] = data.RangeMilliMeter;
|
||
snapshot->range_status[i] = data.RangeStatus;
|
||
|
||
if (data.RangeStatus == 0u) {
|
||
/* 2. 标记有效并按开关决定是否应用卡尔曼滤波 */
|
||
snapshot->valid_mask |= (1u << i);
|
||
#if PARAM_VL53_USE_KALMAN_FILTER
|
||
snapshot->range_mm_filtered[i] = vl53_kalman_update(&board->kf[i], (float)data.RangeMilliMeter);
|
||
#else
|
||
snapshot->range_mm_filtered[i] = (float)data.RangeMilliMeter;
|
||
board->kf[i].x = (float)data.RangeMilliMeter;
|
||
board->kf[i].initialized = 1u;
|
||
#endif
|
||
} else {
|
||
/* 测距失败时，滤波值维持上一次的历史最佳估计不变 */
|
||
snapshot->range_mm_filtered[i] = board->kf[i].x;
|
||
}
|
||
|
||
VL53L0X_ClearInterruptMask(&board->dev[i], 0u);
|
||
}
|
||
} else {
|
||
/* 如果没准备好，把上一帧的历史值顺延下来，防止读到 0 */
|
||
snapshot->range_mm_filtered[i] = board->kf[i].x;
|
||
}
|
||
}
|
||
return VL53L0X_ERROR_NONE;
|
||
}
|