C路: VL53L0X 用户态I2C直驱 + 高速模式 29.6fps
- 搬运 ST API C 源码到 lib/vl53l0x/, 替换 I2C 层为用户态 i2c-dev - 新增 vl53l0x_platform_user.cpp: WriteMulti/ReadMulti 等全平台接口 - 重写 vl53l0x.cpp: start时unbind内核驱动, 单次测距, 无后台轮询 - 高速模式 20000μs timing budget, 模型缓存零污染(41-43ms) - LiDAR 每5帧一读(~20ms), 隔帧模型推理, fps 25.1→29.6 (+18%) - 同事的LiDAR避障集成: 12个配置参数, 中线变形绕行, 刹车注释 - nice -10 + sched_yield 优先级优化
This commit is contained in:
spdis
+141
@@ -1,5 +1,6 @@
|
||||
#include "camera.h"
|
||||
#include "model_v10.hpp"
|
||||
#include "vl53l0x.h"
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
@@ -72,6 +73,19 @@ static int g_cone_hold_ctr = 0;
|
||||
static int g_cone_hold_src_row = -1;
|
||||
static int g_cone_hold_src_col = -1;
|
||||
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
// 激光雷达挡板避障 (主循环每 5 帧同步读取一次)
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
static VL53L0X g_lidar_sensor;
|
||||
static bool g_lidar_ok = false;
|
||||
static int g_lidar_frames = 0;
|
||||
static bool g_lidar_confirmed = false;
|
||||
static int g_lidar_obstacle_row = -1;
|
||||
static int g_lidar_ref_row = -1;
|
||||
static int g_lidar_hold_ctr = 0;
|
||||
static int g_lidar_hold_src_row = -1;
|
||||
static double g_lidar_hold_dir = 0;
|
||||
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
// LCD & FPS
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
@@ -178,6 +192,18 @@ int CameraInit(int camera_id)
|
||||
printf("[MODEL] Mild v12 模型已加载\n");
|
||||
|
||||
i2c_audio_open();
|
||||
|
||||
// 只在启用时才初始化激光,避免驱动内核定时器拖慢 CPU
|
||||
if (g_cfg.lidar_enable) {
|
||||
g_lidar_ok = g_lidar_sensor.init();
|
||||
if (g_lidar_ok)
|
||||
printf("[LIDAR] VL53L0X 已初始化\n");
|
||||
else
|
||||
printf("[LIDAR] VL53L0X 未连接, 避障禁用\n");
|
||||
} else {
|
||||
printf("[LIDAR] 已禁用\n");
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -191,6 +217,7 @@ void cameraDeInit(void)
|
||||
}
|
||||
close(fb);
|
||||
if (i2c_audio_fd >= 0) close(i2c_audio_fd);
|
||||
if (g_lidar_ok) g_lidar_sensor.stop();
|
||||
model_v10_deinit();
|
||||
}
|
||||
|
||||
@@ -350,6 +377,116 @@ static bool traffic_light_process()
|
||||
return (g_tl_state != TL_NORMAL);
|
||||
}
|
||||
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
// 激光雷达挡板避障
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
static bool lidar_is_active() {
|
||||
return g_lidar_confirmed ||
|
||||
(g_lidar_hold_ctr > 0 && g_lidar_hold_ctr <= g_cfg.lidar_hold_frames);
|
||||
}
|
||||
|
||||
static void lidar_avoid_process()
|
||||
{
|
||||
// 每 5 帧读取一次, 单次测距 ~20ms (高速模式)
|
||||
static int lidar_skip = 0;
|
||||
if (!g_lidar_ok || !g_cfg.lidar_enable) return;
|
||||
if (++lidar_skip < 5) return;
|
||||
lidar_skip = 0;
|
||||
|
||||
// ── 1. 单次激光测距 (高速模式 ~20ms) ──
|
||||
VL53L0X_RangingMeasurementData_t data;
|
||||
if (!g_lidar_sensor.readRange(data)) return;
|
||||
int d_mm = data.RangeMilliMeter;
|
||||
|
||||
if (d_mm >= g_cfg.lidar_thresh) {
|
||||
if (g_lidar_frames > 0)
|
||||
g_lidar_frames = std::max(0, g_lidar_frames - 1);
|
||||
goto hold_decay;
|
||||
}
|
||||
|
||||
// ── 2. 距离 → 图像行映射 ──
|
||||
{
|
||||
int lt_h = line_tracking_height;
|
||||
int row = lt_h - 1 - (d_mm - g_cfg.lidar_near) * (lt_h - 11)
|
||||
/ (g_cfg.lidar_far - g_cfg.lidar_near);
|
||||
if (row < 10) row = 10;
|
||||
if (row >= lt_h) row = lt_h - 1;
|
||||
|
||||
// ── 3a. 近处边线有效检查 ──
|
||||
bool near_valid = false;
|
||||
int near_row = -1;
|
||||
int near_start = std::min(row + g_cfg.lidar_near_start, lt_h - 1);
|
||||
int near_end = std::min(row + g_cfg.lidar_near_end, lt_h - 1);
|
||||
for (int r = near_start; r <= near_end; ++r) {
|
||||
if (left_line[r] != -1 && right_line[r] != -1) {
|
||||
near_valid = true;
|
||||
near_row = r;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// ── 3b. 远处丢线检查 ──
|
||||
bool far_lost = false;
|
||||
int far_start = std::max(row - g_cfg.lidar_far_span, 10);
|
||||
for (int r = row; r >= far_start; --r) {
|
||||
if (left_line[r] == -1 || right_line[r] == -1) {
|
||||
far_lost = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// ── 3c. 联合判定 ──
|
||||
if (near_valid && far_lost) {
|
||||
g_lidar_frames++;
|
||||
g_lidar_obstacle_row = row;
|
||||
g_lidar_ref_row = near_row;
|
||||
} else {
|
||||
if (g_lidar_frames > 0)
|
||||
g_lidar_frames = std::max(0, g_lidar_frames - 1);
|
||||
}
|
||||
}
|
||||
|
||||
g_lidar_confirmed = (g_lidar_frames >= g_cfg.lidar_min_frames);
|
||||
|
||||
// ── 保持/衰减 ──
|
||||
if (g_lidar_confirmed) {
|
||||
g_lidar_hold_ctr = 0;
|
||||
g_lidar_hold_src_row = g_lidar_obstacle_row;
|
||||
// 在参考行判断左右空间，往宽侧推
|
||||
int r = g_lidar_ref_row;
|
||||
double lspace = mid_line[r] - left_line[r];
|
||||
double rspace = right_line[r] - mid_line[r];
|
||||
g_lidar_hold_dir = (lspace > rspace) ? 1.0 : -1.0;
|
||||
if (g_cfg.debug)
|
||||
printf("[LIDAR] 触发 d=%d row=%d dir=%.0f\n", d_mm, g_lidar_obstacle_row, g_lidar_hold_dir);
|
||||
g_lidar_frames = 0;
|
||||
}
|
||||
|
||||
hold_decay:
|
||||
if (g_lidar_hold_src_row > 0 && g_lidar_hold_ctr <= g_cfg.lidar_hold_frames) {
|
||||
g_lidar_hold_ctr++;
|
||||
}
|
||||
|
||||
if (!lidar_is_active()) return;
|
||||
|
||||
// ── 中线变形 ──
|
||||
int src_row = g_lidar_hold_src_row;
|
||||
double rng = (double)g_cfg.lidar_avoid_range;
|
||||
double half_w = line_tracking_width / 2.0;
|
||||
double dir = g_lidar_hold_dir;
|
||||
double decay = g_lidar_confirmed ? 1.0 :
|
||||
(1.0 - (double)g_lidar_hold_ctr / g_cfg.lidar_hold_frames);
|
||||
|
||||
for (int row = 10; row <= src_row; ++row) {
|
||||
double t = std::clamp(((row - 10) / rng), 0.0, 1.0);
|
||||
double push = t * g_cfg.lidar_avoid_gain * half_w * dir * decay;
|
||||
double nm = std::clamp(mid_line[row] + push,
|
||||
(double)left_line[row] + 2.0,
|
||||
(double)right_line[row] - 2.0);
|
||||
mid_line[row] = (int)(nm + 0.5);
|
||||
}
|
||||
}
|
||||
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
// 锥桶检测 + 中线变形
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
@@ -476,6 +613,8 @@ static void motor_update(bool zebra_block, bool tl_block)
|
||||
double spd = target_speed;
|
||||
if (cone_is_slow() && g_zstate != Z_STOP && g_tl_state == TL_NORMAL)
|
||||
spd *= g_cfg.cone_speed;
|
||||
if (lidar_is_active() && g_zstate != Z_STOP && g_tl_state == TL_NORMAL)
|
||||
spd *= g_cfg.lidar_speed;
|
||||
ControlUpdate(spd, zebra_block || tl_block);
|
||||
}
|
||||
|
||||
@@ -534,6 +673,7 @@ static void lcd_render()
|
||||
|
||||
// 状态指示
|
||||
char ztxt[8]; int zi = 0;
|
||||
if (lidar_is_active()) ztxt[zi++] = 'L';
|
||||
if (g_tl_state == TL_STOP) ztxt[zi++] = 'R';
|
||||
else if (g_tl_state == TL_WAIT_GREEN) ztxt[zi++] = 'G';
|
||||
if (g_zstate == Z_STOP) ztxt[zi++] = 'S';
|
||||
@@ -596,6 +736,7 @@ int CameraHandler(void)
|
||||
// 5. 场景识别
|
||||
bool zebra_block = zebra_process();
|
||||
bool tl_block = traffic_light_process();
|
||||
lidar_avoid_process();
|
||||
cone_detect_and_deform();
|
||||
|
||||
// 6. 舵机
|
||||
|
||||
@@ -39,4 +39,17 @@ void cfg_load_all()
|
||||
|
||||
g_cfg.brake_scale = readDoubleFromFile(brake_scale_file);
|
||||
g_cfg.brake_max = readDoubleFromFile(brake_max_file);
|
||||
|
||||
g_cfg.lidar_thresh = (int)readDoubleFromFile(lidar_thresh_file);
|
||||
g_cfg.lidar_near = (int)readDoubleFromFile(lidar_near_file);
|
||||
g_cfg.lidar_far = (int)readDoubleFromFile(lidar_far_file);
|
||||
g_cfg.lidar_near_start = (int)readDoubleFromFile(lidar_near_start_file);
|
||||
g_cfg.lidar_near_end = (int)readDoubleFromFile(lidar_near_end_file);
|
||||
g_cfg.lidar_far_span = (int)readDoubleFromFile(lidar_far_span_file);
|
||||
g_cfg.lidar_min_frames = (int)readDoubleFromFile(lidar_min_frames_file);
|
||||
g_cfg.lidar_avoid_gain = readDoubleFromFile(lidar_avoid_gain_file);
|
||||
g_cfg.lidar_avoid_range = (int)readDoubleFromFile(lidar_avoid_range_file);
|
||||
g_cfg.lidar_speed = readDoubleFromFile(lidar_speed_file);
|
||||
g_cfg.lidar_hold_frames = (int)readDoubleFromFile(lidar_hold_frames_file);
|
||||
g_cfg.lidar_enable = (int)readDoubleFromFile(lidar_enable_file);
|
||||
}
|
||||
|
||||
+116
-44
@@ -2,71 +2,143 @@
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <linux/i2c-dev.h>
|
||||
#include <cstring>
|
||||
#include <cerrno>
|
||||
#include <cstdio>
|
||||
#include <cerrno>
|
||||
|
||||
#define VL53L0X_IOCTL_INIT _IO('p', 0x01)
|
||||
#define VL53L0X_IOCTL_STOP _IO('p', 0x05)
|
||||
#define VL53L0X_IOCTL_GETDATA _IOR('p', 0x0b, VL53L0X_RangingMeasurementData_t)
|
||||
extern "C" {
|
||||
#include "vl53l0x_api.h"
|
||||
}
|
||||
|
||||
VL53L0X::VL53L0X() : fd(-1) {}
|
||||
static void unbind_kernel_driver()
|
||||
{
|
||||
FILE *f = fopen("/sys/bus/i2c/drivers/stmvl53l0/unbind", "w");
|
||||
if (f) {
|
||||
fprintf(f, "1-0029");
|
||||
fclose(f);
|
||||
}
|
||||
}
|
||||
|
||||
VL53L0X::VL53L0X()
|
||||
: fd(-1), initialized(false)
|
||||
{
|
||||
memset(&dev, 0, sizeof(dev));
|
||||
}
|
||||
|
||||
VL53L0X::~VL53L0X()
|
||||
{
|
||||
if (fd > 0)
|
||||
{
|
||||
stop();
|
||||
close(fd);
|
||||
fd = -1;
|
||||
}
|
||||
stop();
|
||||
}
|
||||
|
||||
bool VL53L0X::init()
|
||||
{
|
||||
fd = open("/dev/stmvl53l0x_ranging", O_RDWR | O_SYNC);
|
||||
if (fd <= 0)
|
||||
{
|
||||
fprintf(stderr, "[VL53L0X] open failed: %s\n", strerror(errno));
|
||||
return false;
|
||||
}
|
||||
unbind_kernel_driver();
|
||||
usleep(100000);
|
||||
|
||||
ioctl(fd, VL53L0X_IOCTL_STOP, nullptr);
|
||||
fd = open("/dev/i2c-1", O_RDWR);
|
||||
if (fd < 0) {
|
||||
fprintf(stderr, "[VL53L0X] open /dev/i2c-1 failed: %s\n", strerror(errno));
|
||||
return false;
|
||||
}
|
||||
|
||||
if (ioctl(fd, VL53L0X_IOCTL_INIT, nullptr) < 0)
|
||||
{
|
||||
fprintf(stderr, "[VL53L0X] init failed: %s\n", strerror(errno));
|
||||
close(fd);
|
||||
fd = -1;
|
||||
return false;
|
||||
}
|
||||
if (ioctl(fd, I2C_SLAVE, 0x29) < 0) {
|
||||
fprintf(stderr, "[VL53L0X] I2C_SLAVE failed: %s\n", strerror(errno));
|
||||
close(fd);
|
||||
fd = -1;
|
||||
return false;
|
||||
}
|
||||
|
||||
fprintf(stderr, "[VL53L0X] init OK\n");
|
||||
return true;
|
||||
dev.I2cDevAddr = 0x29;
|
||||
dev.i2c_fd = fd;
|
||||
|
||||
VL53L0X_Error Status = VL53L0X_DataInit(&dev);
|
||||
if (Status != VL53L0X_ERROR_NONE) {
|
||||
fprintf(stderr, "[VL53L0X] DataInit failed: %d\n", Status);
|
||||
close(fd);
|
||||
fd = -1;
|
||||
return false;
|
||||
}
|
||||
|
||||
Status = VL53L0X_StaticInit(&dev);
|
||||
if (Status != VL53L0X_ERROR_NONE) {
|
||||
fprintf(stderr, "[VL53L0X] StaticInit failed: %d\n", Status);
|
||||
close(fd);
|
||||
fd = -1;
|
||||
return false;
|
||||
}
|
||||
|
||||
uint32_t refSpadCount;
|
||||
uint8_t isApertureSpads;
|
||||
uint8_t VhvSettings;
|
||||
uint8_t PhaseCal;
|
||||
|
||||
VL53L0X_PerformRefCalibration(&dev, &VhvSettings, &PhaseCal);
|
||||
VL53L0X_PerformRefSpadManagement(&dev, &refSpadCount, &isApertureSpads);
|
||||
|
||||
Status = VL53L0X_SetDeviceMode(&dev, VL53L0X_DEVICEMODE_SINGLE_RANGING);
|
||||
if (Status != VL53L0X_ERROR_NONE) {
|
||||
fprintf(stderr, "[VL53L0X] SetDeviceMode failed: %d\n", Status);
|
||||
close(fd);
|
||||
fd = -1;
|
||||
return false;
|
||||
}
|
||||
|
||||
VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&dev, 20000);
|
||||
|
||||
uint32_t actualBudget = 0;
|
||||
VL53L0X_GetMeasurementTimingBudgetMicroSeconds(&dev, &actualBudget);
|
||||
fprintf(stderr, "[VL53L0X] timing budget = %u us\n", actualBudget);
|
||||
|
||||
initialized = true;
|
||||
fprintf(stderr, "[VL53L0X] init OK\n");
|
||||
return true;
|
||||
}
|
||||
|
||||
bool VL53L0X::readRange(VL53L0X_RangingMeasurementData_t &data)
|
||||
{
|
||||
if (fd <= 0)
|
||||
return false;
|
||||
if (!initialized) return false;
|
||||
|
||||
if (ioctl(fd, VL53L0X_IOCTL_GETDATA, &data) < 0)
|
||||
{
|
||||
fprintf(stderr, "[VL53L0X] read failed: %s\n", strerror(errno));
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
VL53L0X_Error Status;
|
||||
|
||||
VL53L0X_ClearInterruptMask(&dev, 0);
|
||||
Status = VL53L0X_StartMeasurement(&dev);
|
||||
if (Status != VL53L0X_ERROR_NONE) {
|
||||
fprintf(stderr, "[VL53L0X] StartMeasurement failed: %d\n", Status);
|
||||
return false;
|
||||
}
|
||||
|
||||
uint8_t ready = 0;
|
||||
int timeout = 500;
|
||||
while (!ready && --timeout > 0) {
|
||||
Status = VL53L0X_GetMeasurementDataReady(&dev, &ready);
|
||||
if (Status != VL53L0X_ERROR_NONE)
|
||||
break;
|
||||
if (!ready)
|
||||
usleep(1000);
|
||||
}
|
||||
|
||||
if (!ready) {
|
||||
fprintf(stderr, "[VL53L0X] measurement timeout\n");
|
||||
return false;
|
||||
}
|
||||
|
||||
Status = VL53L0X_GetRangingMeasurementData(&dev, &data);
|
||||
if (Status != VL53L0X_ERROR_NONE) {
|
||||
fprintf(stderr, "[VL53L0X] GetRangingMeasurementData failed: %d\n", Status);
|
||||
return false;
|
||||
}
|
||||
|
||||
VL53L0X_ClearInterruptMask(&dev, 0);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool VL53L0X::stop()
|
||||
{
|
||||
if (fd <= 0)
|
||||
return false;
|
||||
|
||||
if (ioctl(fd, VL53L0X_IOCTL_STOP, nullptr) < 0)
|
||||
{
|
||||
fprintf(stderr, "[VL53L0X] stop failed: %s\n", strerror(errno));
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
if (fd >= 0) {
|
||||
close(fd);
|
||||
fd = -1;
|
||||
}
|
||||
initialized = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,159 @@
|
||||
extern "C" {
|
||||
#include "vl53l0x_platform.h"
|
||||
}
|
||||
|
||||
#include <unistd.h>
|
||||
#include <cstring>
|
||||
|
||||
VL53L0X_Error VL53L0X_LockSequenceAccess(VL53L0X_DEV Dev)
|
||||
{
|
||||
(void)Dev;
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_UnlockSequenceAccess(VL53L0X_DEV Dev)
|
||||
{
|
||||
(void)Dev;
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_WriteMulti(VL53L0X_DEV Dev, uint8_t index,
|
||||
uint8_t *pdata, uint32_t count)
|
||||
{
|
||||
if (count >= VL53L0X_MAX_I2C_XFER_SIZE)
|
||||
return VL53L0X_ERROR_INVALID_PARAMS;
|
||||
|
||||
uint8_t buf[VL53L0X_MAX_I2C_XFER_SIZE + 1];
|
||||
buf[0] = index;
|
||||
memcpy(buf + 1, pdata, count);
|
||||
|
||||
if (write(Dev->i2c_fd, buf, count + 1) != (ssize_t)(count + 1))
|
||||
return VL53L0X_ERROR_CONTROL_INTERFACE;
|
||||
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_ReadMulti(VL53L0X_DEV Dev, uint8_t index,
|
||||
uint8_t *pdata, uint32_t count)
|
||||
{
|
||||
if (count >= VL53L0X_MAX_I2C_XFER_SIZE)
|
||||
return VL53L0X_ERROR_INVALID_PARAMS;
|
||||
|
||||
if (write(Dev->i2c_fd, &index, 1) != 1)
|
||||
return VL53L0X_ERROR_CONTROL_INTERFACE;
|
||||
|
||||
if (read(Dev->i2c_fd, pdata, count) != (ssize_t)count)
|
||||
return VL53L0X_ERROR_CONTROL_INTERFACE;
|
||||
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_WrByte(VL53L0X_DEV Dev, uint8_t index, uint8_t data)
|
||||
{
|
||||
return VL53L0X_WriteMulti(Dev, index, &data, 1);
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_WrWord(VL53L0X_DEV Dev, uint8_t index, uint16_t data)
|
||||
{
|
||||
uint8_t buf[2];
|
||||
buf[0] = (data >> 8) & 0xFF;
|
||||
buf[1] = data & 0xFF;
|
||||
return VL53L0X_WriteMulti(Dev, index, buf, 2);
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_WrDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t data)
|
||||
{
|
||||
uint8_t buf[4];
|
||||
buf[0] = (data >> 24) & 0xFF;
|
||||
buf[1] = (data >> 16) & 0xFF;
|
||||
buf[2] = (data >> 8) & 0xFF;
|
||||
buf[3] = data & 0xFF;
|
||||
return VL53L0X_WriteMulti(Dev, index, buf, 4);
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_RdByte(VL53L0X_DEV Dev, uint8_t index, uint8_t *data)
|
||||
{
|
||||
return VL53L0X_ReadMulti(Dev, index, data, 1);
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_RdWord(VL53L0X_DEV Dev, uint8_t index, uint16_t *data)
|
||||
{
|
||||
uint8_t buf[2];
|
||||
VL53L0X_Error Status = VL53L0X_ReadMulti(Dev, index, buf, 2);
|
||||
*data = ((uint16_t)buf[0] << 8) | buf[1];
|
||||
return Status;
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_RdDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t *data)
|
||||
{
|
||||
uint8_t buf[4];
|
||||
VL53L0X_Error Status = VL53L0X_ReadMulti(Dev, index, buf, 4);
|
||||
*data = ((uint32_t)buf[0] << 24) | ((uint32_t)buf[1] << 16) |
|
||||
((uint32_t)buf[2] << 8) | buf[3];
|
||||
return Status;
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_UpdateByte(VL53L0X_DEV Dev, uint8_t index,
|
||||
uint8_t AndData, uint8_t OrData)
|
||||
{
|
||||
uint8_t data;
|
||||
VL53L0X_Error Status = VL53L0X_RdByte(Dev, index, &data);
|
||||
if (Status != VL53L0X_ERROR_NONE)
|
||||
return Status;
|
||||
data = (data & AndData) | OrData;
|
||||
return VL53L0X_WrByte(Dev, index, data);
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_PollingDelay(VL53L0X_DEV Dev)
|
||||
{
|
||||
(void)Dev;
|
||||
usleep(1000);
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
/* string/internal stubs — never called but needed for linking */
|
||||
extern "C" {
|
||||
|
||||
VL53L0X_Error VL53L0X_get_device_info(VL53L0X_DEV Dev, VL53L0X_DeviceInfo_t *pVL53L0X_DeviceInfo)
|
||||
{
|
||||
(void)Dev; (void)pVL53L0X_DeviceInfo;
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_get_device_error_string(VL53L0X_DeviceError ErrorCode, char *pDeviceErrorString)
|
||||
{
|
||||
(void)ErrorCode; (void)pDeviceErrorString;
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_get_range_status_string(uint8_t RangeStatus, char *pRangeStatusString)
|
||||
{
|
||||
(void)RangeStatus; (void)pRangeStatusString;
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_get_pal_error_string(VL53L0X_Error PalErrorCode, char *pPalErrorString)
|
||||
{
|
||||
(void)PalErrorCode; (void)pPalErrorString;
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_get_pal_state_string(VL53L0X_State PalStateCode, char *pPalStateString)
|
||||
{
|
||||
(void)PalStateCode; (void)pPalStateString;
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_get_sequence_steps_info(VL53L0X_SequenceStepId SequenceStepId, char *pSequenceStepsString)
|
||||
{
|
||||
(void)SequenceStepId; (void)pSequenceStepsString;
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_get_limit_check_info(VL53L0X_DEV Dev, uint16_t LimitCheckId, char *pLimitCheckString)
|
||||
{
|
||||
(void)Dev; (void)LimitCheckId; (void)pLimitCheckString;
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user