Falling_jasmine/ASER
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

1.0

Browse Source
This commit is contained in:
nitiantuhao
2026-04-04 17:09:19 +08:00
parent ee63bee6e5
commit 1ecdf6c187
9 changed files with 201 additions and 320 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
App/VL53L0X_API/platform/vl53_board.c
View File

@@ -2,6 +2,7 @@
#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) {
@@ -25,6 +26,43 @@ static float vl53_kalman_update(Vl53Kalman_t *kf, float measurement) {
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)
{
@@ -95,6 +133,7 @@ VL53L0X_Error Vl53Board_Init(Vl53Board_t *board, const Vl53BoardHwCfg_t *hw_cfgs
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;
@@ -163,4 +202,4 @@ VL53L0X_Error Vl53Board_ReadAll(Vl53Board_t *board, Vl53BoardSnapshot_t *snapsho
}
}
return VL53L0X_ERROR_NONE;
}
}