spdis/Loongson_2k0300_SmartCar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: boost改为固定26 + 删除死代码

Browse Source 
- motor_update: boost从乘法改为固定值26
- 删除废弃的font_bitmap.h中文渲染代码(已改用PNG)
- 移除无源码的lcd_test编译目标
- 设备speed=20
This commit is contained in:
spdis
2026-07-06 17:11:15 +08:00
parent 8ee66fe444
commit e720009794
9 changed files with 1317 additions and 1391 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/global.h
+107 -107
View File
@@ -1,107 +1,107 @@
#ifndef GLOBAL_H #ifndef GLOBAL_H
#define GLOBAL_H #define GLOBAL_H
#include <string> #include <string>
// ── 参数文件名 ── // ── 参数文件名 ──
const std::string kp_file = "./kp"; const std::string kp_file = "./kp";
const std::string ki_file = "./ki"; const std::string ki_file = "./ki";
const std::string kd_file = "./kd"; const std::string kd_file = "./kd";
const std::string start_file = "./start"; const std::string start_file = "./start";
const std::string showImg_file = "./showImg"; const std::string showImg_file = "./showImg";
const std::string destfps_file = "./destfps"; const std::string destfps_file = "./destfps";
const std::string foresee_file = "./foresee"; const std::string foresee_file = "./foresee";
const std::string foresee_lost_scale_file = "./foresee_lost_scale"; const std::string foresee_lost_scale_file = "./foresee_lost_scale";
const std::string sharp_turn_scale_file = "./sharp_turn_scale"; const std::string sharp_turn_scale_file = "./sharp_turn_scale";
const std::string zebrasee_file = "./zebrasee"; const std::string zebrasee_file = "./zebrasee";
const std::string saveImg_file = "./saveImg"; const std::string saveImg_file = "./saveImg";
const std::string speed_file = "./speed"; const std::string speed_file = "./speed";
const std::string deadband_file = "./deadband"; const std::string deadband_file = "./deadband";
const std::string steer_gain_file = "./steer_gain"; const std::string steer_gain_file = "./steer_gain";
const std::string center_bias_file = "./center_bias"; const std::string center_bias_file = "./center_bias";
const std::string debug_file = "./debug"; const std::string debug_file = "./debug";
const std::string cone_avoid_gain_file = "./cone_avoid_gain"; const std::string cone_avoid_gain_file = "./cone_avoid_gain";
const std::string cone_avoid_range_file = "./cone_avoid_range"; const std::string cone_avoid_range_file = "./cone_avoid_range";
const std::string cone_speed_file = "./cone_speed"; const std::string cone_speed_file = "./cone_speed";
const std::string cone_min_frames_file = "./cone_min_frames"; const std::string cone_min_frames_file = "./cone_min_frames";
const std::string cone_margin_file = "./cone_margin"; const std::string cone_margin_file = "./cone_margin";
const std::string cone_thresh_file = "./cone_thresh"; const std::string cone_thresh_file = "./cone_thresh";
const std::string cone_hold_frames_file = "./cone_hold_frames"; const std::string cone_hold_frames_file = "./cone_hold_frames";
const std::string cone_return_gain_file = "./cone_return_gain"; const std::string cone_return_gain_file = "./cone_return_gain";
const std::string cone_return_frames_file = "./cone_return_frames"; const std::string cone_return_frames_file = "./cone_return_frames";
const std::string brake_scale_file = "./brake_scale"; const std::string brake_scale_file = "./brake_scale";
const std::string brake_max_file = "./brake_max"; const std::string brake_max_file = "./brake_max";
const std::string curve_slope_file = "./curve_slope"; const std::string curve_slope_file = "./curve_slope";
const std::string curve_min_file = "./curve_min"; const std::string curve_min_file = "./curve_min";
const std::string lidar_thresh_file = "./lidar_thresh"; const std::string lidar_thresh_file = "./lidar_thresh";
const std::string lidar_pre_file = "./lidar_pre"; const std::string lidar_pre_file = "./lidar_pre";
const std::string lidar_near_file = "./lidar_near"; const std::string lidar_near_file = "./lidar_near";
const std::string lidar_far_file = "./lidar_far"; const std::string lidar_far_file = "./lidar_far";
const std::string lidar_near_start_file = "./lidar_near_start"; const std::string lidar_near_start_file = "./lidar_near_start";
const std::string lidar_near_end_file = "./lidar_near_end"; const std::string lidar_near_end_file = "./lidar_near_end";
const std::string lidar_far_span_file = "./lidar_far_span"; const std::string lidar_far_span_file = "./lidar_far_span";
const std::string lidar_min_frames_file = "./lidar_min_frames"; const std::string lidar_min_frames_file = "./lidar_min_frames";
const std::string lidar_avoid_gain_file = "./lidar_avoid_gain"; const std::string lidar_avoid_gain_file = "./lidar_avoid_gain";
const std::string lidar_avoid_range_file = "./lidar_avoid_range"; const std::string lidar_avoid_range_file = "./lidar_avoid_range";
const std::string lidar_speed_file = "./lidar_speed"; const std::string lidar_speed_file = "./lidar_speed";
const std::string lidar_hold_frames_file = "./lidar_hold_frames"; const std::string lidar_hold_frames_file = "./lidar_hold_frames";
const std::string lidar_enable_file = "./lidar_enable"; const std::string lidar_enable_file = "./lidar_enable";
double readDoubleFromFile(const std::string &filename); double readDoubleFromFile(const std::string &filename);
bool readFlag(const std::string &filename); bool readFlag(const std::string &filename);
void cfg_load_all(); void cfg_load_all();
// ── 启动时缓存的运行参数 ── // ── 启动时缓存的运行参数 ──
struct CfgCache { struct CfgCache {
double speed = 60; // 目标速度(占空比 %) double speed = 60; // 目标速度(占空比 %)
double foresee = 80; // 前瞻行 double foresee = 80; // 前瞻行
double foresee_lost_scale = 0.7; // 丢线时前瞻缩放 (<1=看更远) double foresee_lost_scale = 0.7; // 丢线时前瞻缩放 (<1=看更远)
double sharp_turn_scale = 0.5; // 急弯时前瞻缩放 (<1=看更远) double sharp_turn_scale = 0.5; // 急弯时前瞻缩放 (<1=看更远)
double zebrasee = 60; // 斑马线触发距离阈值 double zebrasee = 60; // 斑马线触发距离阈值
double deadband = 5; // 舵机死区 double deadband = 5; // 舵机死区
double steer_gain = 1.0; // 舵机增益 double steer_gain = 1.0; // 舵机增益
double center_bias = 0; // 中线偏移修正 double center_bias = 0; // 中线偏移修正
int start = 0; // 1=启动 0=停止 int start = 0; // 1=启动 0=停止
int showImg = 0; // LCD 预览开关 int showImg = 0; // LCD 预览开关
int debug = 0; // debug 模式 int debug = 0; // debug 模式
double cone_avoid_gain = 0.3; // 锥桶中线变形推离量 (归一化) double cone_avoid_gain = 0.3; // 锥桶中线变形推离量 (归一化)
int cone_avoid_range = 30; // 变形斜坡陡峭度 int cone_avoid_range = 30; // 变形斜坡陡峭度
double cone_speed = 0.5; // 锥桶触发时速度倍率 double cone_speed = 0.5; // 锥桶触发时速度倍率
int cone_min_frames = 1; // 确认帧数 (1=单帧即确认, 视觉巡线响应快导致检测窗口极短) int cone_min_frames = 1; // 确认帧数 (1=单帧即确认, 视觉巡线响应快导致检测窗口极短)
int cone_margin = 0; // 0=中心点 1=框边缘 int cone_margin = 0; // 0=中心点 1=框边缘
double cone_thresh = 0.80; // 锥桶置信度阈值 double cone_thresh = 0.80; // 锥桶置信度阈值
int cone_hold_frames = 15; // 锥桶消失后保持变形的帧数 int cone_hold_frames = 15; // 锥桶消失后保持变形的帧数
double cone_return_gain = 1.0; // 回弹力度 (倍乘, 1.0=等同于避让力度) double cone_return_gain = 1.0; // 回弹力度 (倍乘, 1.0=等同于避让力度)
int cone_return_frames = 30; // 回弹持续帧数 int cone_return_frames = 30; // 回弹持续帧数
double brake_scale = 10; // 速度(pps) → 刹车占空比(ns) 缩放系数 double brake_scale = 10; // 速度(pps) → 刹车占空比(ns) 缩放系数
int brake_max = 10000; // 最大刹车占空比 (ns) int brake_max = 10000; // 最大刹车占空比 (ns)
double curve_slope = 0.4; // 弯道减速斜率 (越大越猛) double curve_slope = 0.4; // 弯道减速斜率 (越大越猛)
double curve_min = 0.8; // 弯道最低速度倍率 double curve_min = 0.8; // 弯道最低速度倍率
int lidar_thresh = 300; // 激光障碍判定距离阈值 (mm), 必须<此值才触发 int lidar_thresh = 300; // 激光障碍判定距离阈值 (mm), 必须<此值才触发
int lidar_pre = 800; // 预触发去抖窗口, <此值开始攒帧 int lidar_pre = 800; // 预触发去抖窗口, <此值开始攒帧
int lidar_near = 50; // row=lt_h-1 对应的物理距离 (mm) int lidar_near = 50; // row=lt_h-1 对应的物理距离 (mm)
int lidar_far = 1200; // row=10 对应的物理距离 (mm) int lidar_far = 1200; // row=10 对应的物理距离 (mm)
int lidar_near_start = 1; // near_valid 检测起始偏移 int lidar_near_start = 1; // near_valid 检测起始偏移
int lidar_near_end = 4; // near_valid 检测终止偏移 int lidar_near_end = 4; // near_valid 检测终止偏移
int lidar_far_span = 6; // far_lost 检测跨度 int lidar_far_span = 6; // far_lost 检测跨度
int lidar_min_frames = 3; // 连续确认帧数 int lidar_min_frames = 3; // 连续确认帧数
double lidar_avoid_gain = 0.4; // 绕行中线推离力度 (归一化) double lidar_avoid_gain = 0.4; // 绕行中线推离力度 (归一化)
int lidar_avoid_range = 30; // 绕行斜坡陡峭度 (行数) int lidar_avoid_range = 30; // 绕行斜坡陡峭度 (行数)
double lidar_speed = 0.4; // 绕行时速度倍率 double lidar_speed = 0.4; // 绕行时速度倍率
int lidar_hold_frames = 30; // 消失后变形保持帧数 int lidar_hold_frames = 30; // 消失后变形保持帧数
int lidar_enable = 1; // 激光避障总开关 int lidar_enable = 1; // 激光避障总开关
}; };
extern CfgCache g_cfg; extern CfgCache g_cfg;
extern double target_speed; extern double target_speed;
#endif #endif
lib/vl53l0x.h
+28 -28
View File
@@ -1,28 +1,28 @@
#ifndef VL53L0X_H #ifndef VL53L0X_H
#define VL53L0X_H #define VL53L0X_H
#include <cstdint> #include <cstdint>
extern "C" { extern "C" {
#include "vl53l0x_platform.h" #include "vl53l0x_platform.h"
} }
class VL53L0X class VL53L0X
{ {
public: public:
VL53L0X(); VL53L0X();
~VL53L0X(); ~VL53L0X();
bool init(); bool init();
bool startMeasure(); bool startMeasure();
bool readResult(VL53L0X_RangingMeasurementData_t &data); bool readResult(VL53L0X_RangingMeasurementData_t &data);
bool readRange(VL53L0X_RangingMeasurementData_t &data); bool readRange(VL53L0X_RangingMeasurementData_t &data);
bool stop(); bool stop();
private: private:
int fd; int fd;
bool initialized; bool initialized;
vl53l0x_dev_t dev; vl53l0x_dev_t dev;
}; };
#endif #endif
lib/vl53l0x_def.h
+640 -640
View File
File diff suppressed because it is too large Load Diff
lib/vl53l0x_device.h
+257 -257
View File
@@ -1,257 +1,257 @@
/******************************************************************************* /*******************************************************************************
Copyright © 2016, STMicroelectronics International N.V. Copyright © 2016, STMicroelectronics International N.V.
All rights reserved. All rights reserved.
Redistribution and use in source and binary forms, with or without Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met: modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright * Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer. notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright * Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution. documentation and/or other materials provided with the distribution.
* Neither the name of STMicroelectronics nor the * Neither the name of STMicroelectronics nor the
names of its contributors may be used to endorse or promote products names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission. derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED. NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/ *******************************************************************************/
/** /**
* Device specific defines. To be adapted by implementer for the targeted * Device specific defines. To be adapted by implementer for the targeted
* device. * device.
*/ */
#ifndef _VL53L0X_DEVICE_H_ #ifndef _VL53L0X_DEVICE_H_
#define _VL53L0X_DEVICE_H_ #define _VL53L0X_DEVICE_H_
#include "vl53l0x_types.h" #include "vl53l0x_types.h"
/** @defgroup VL53L0X_DevSpecDefines_group VL53L0X cut1.1 Device Specific Defines /** @defgroup VL53L0X_DevSpecDefines_group VL53L0X cut1.1 Device Specific Defines
* @brief VL53L0X cut1.1 Device Specific Defines * @brief VL53L0X cut1.1 Device Specific Defines
* @{ * @{
*/ */
/** @defgroup VL53L0X_DeviceError_group Device Error /** @defgroup VL53L0X_DeviceError_group Device Error
* @brief Device Error code * @brief Device Error code
* *
* This enum is Device specific it should be updated in the implementation * This enum is Device specific it should be updated in the implementation
* Use @a VL53L0X_GetStatusErrorString() to get the string. * Use @a VL53L0X_GetStatusErrorString() to get the string.
* It is related to Status Register of the Device. * It is related to Status Register of the Device.
* @{ * @{
*/ */
typedef uint8_t VL53L0X_DeviceError; typedef uint8_t VL53L0X_DeviceError;
#define VL53L0X_DEVICEERROR_NONE ((VL53L0X_DeviceError) 0) #define VL53L0X_DEVICEERROR_NONE ((VL53L0X_DeviceError) 0)
/*!< 0 NoError */ /*!< 0 NoError */
#define VL53L0X_DEVICEERROR_VCSELCONTINUITYTESTFAILURE ((VL53L0X_DeviceError) 1) #define VL53L0X_DEVICEERROR_VCSELCONTINUITYTESTFAILURE ((VL53L0X_DeviceError) 1)
#define VL53L0X_DEVICEERROR_VCSELWATCHDOGTESTFAILURE ((VL53L0X_DeviceError) 2) #define VL53L0X_DEVICEERROR_VCSELWATCHDOGTESTFAILURE ((VL53L0X_DeviceError) 2)
#define VL53L0X_DEVICEERROR_NOVHVVALUEFOUND ((VL53L0X_DeviceError) 3) #define VL53L0X_DEVICEERROR_NOVHVVALUEFOUND ((VL53L0X_DeviceError) 3)
#define VL53L0X_DEVICEERROR_MSRCNOTARGET ((VL53L0X_DeviceError) 4) #define VL53L0X_DEVICEERROR_MSRCNOTARGET ((VL53L0X_DeviceError) 4)
#define VL53L0X_DEVICEERROR_SNRCHECK ((VL53L0X_DeviceError) 5) #define VL53L0X_DEVICEERROR_SNRCHECK ((VL53L0X_DeviceError) 5)
#define VL53L0X_DEVICEERROR_RANGEPHASECHECK ((VL53L0X_DeviceError) 6) #define VL53L0X_DEVICEERROR_RANGEPHASECHECK ((VL53L0X_DeviceError) 6)
#define VL53L0X_DEVICEERROR_SIGMATHRESHOLDCHECK ((VL53L0X_DeviceError) 7) #define VL53L0X_DEVICEERROR_SIGMATHRESHOLDCHECK ((VL53L0X_DeviceError) 7)
#define VL53L0X_DEVICEERROR_TCC ((VL53L0X_DeviceError) 8) #define VL53L0X_DEVICEERROR_TCC ((VL53L0X_DeviceError) 8)
#define VL53L0X_DEVICEERROR_PHASECONSISTENCY ((VL53L0X_DeviceError) 9) #define VL53L0X_DEVICEERROR_PHASECONSISTENCY ((VL53L0X_DeviceError) 9)
#define VL53L0X_DEVICEERROR_MINCLIP ((VL53L0X_DeviceError) 10) #define VL53L0X_DEVICEERROR_MINCLIP ((VL53L0X_DeviceError) 10)
#define VL53L0X_DEVICEERROR_RANGECOMPLETE ((VL53L0X_DeviceError) 11) #define VL53L0X_DEVICEERROR_RANGECOMPLETE ((VL53L0X_DeviceError) 11)
#define VL53L0X_DEVICEERROR_ALGOUNDERFLOW ((VL53L0X_DeviceError) 12) #define VL53L0X_DEVICEERROR_ALGOUNDERFLOW ((VL53L0X_DeviceError) 12)
#define VL53L0X_DEVICEERROR_ALGOOVERFLOW ((VL53L0X_DeviceError) 13) #define VL53L0X_DEVICEERROR_ALGOOVERFLOW ((VL53L0X_DeviceError) 13)
#define VL53L0X_DEVICEERROR_RANGEIGNORETHRESHOLD ((VL53L0X_DeviceError) 14) #define VL53L0X_DEVICEERROR_RANGEIGNORETHRESHOLD ((VL53L0X_DeviceError) 14)
/** @} end of VL53L0X_DeviceError_group */ /** @} end of VL53L0X_DeviceError_group */
/** @defgroup VL53L0X_CheckEnable_group Check Enable list /** @defgroup VL53L0X_CheckEnable_group Check Enable list
* @brief Check Enable code * @brief Check Enable code
* *
* Define used to specify the LimitCheckId. * Define used to specify the LimitCheckId.
* Use @a VL53L0X_GetLimitCheckInfo() to get the string. * Use @a VL53L0X_GetLimitCheckInfo() to get the string.
* @{ * @{
*/ */
#define VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE 0 #define VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE 0
#define VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE 1 #define VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE 1
#define VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP 2 #define VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP 2
#define VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD 3 #define VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD 3
#define VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC 4 #define VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC 4
#define VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE 5 #define VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE 5
#define VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS 6 #define VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS 6
/** @} end of VL53L0X_CheckEnable_group */ /** @} end of VL53L0X_CheckEnable_group */
/** @defgroup VL53L0X_GpioFunctionality_group Gpio Functionality /** @defgroup VL53L0X_GpioFunctionality_group Gpio Functionality
* @brief Defines the different functionalities for the device GPIO(s) * @brief Defines the different functionalities for the device GPIO(s)
* @{ * @{
*/ */
typedef uint8_t VL53L0X_GpioFunctionality; typedef uint8_t VL53L0X_GpioFunctionality;
#define VL53L0X_GPIOFUNCTIONALITY_OFF \ #define VL53L0X_GPIOFUNCTIONALITY_OFF \
((VL53L0X_GpioFunctionality) 0) /*!< NO Interrupt */ ((VL53L0X_GpioFunctionality) 0) /*!< NO Interrupt */
#define VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW \ #define VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW \
((VL53L0X_GpioFunctionality) 1) /*!< Level Low (value < thresh_low) */ ((VL53L0X_GpioFunctionality) 1) /*!< Level Low (value < thresh_low) */
#define VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH \ #define VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH \
((VL53L0X_GpioFunctionality) 2) /*!< Level High (value > thresh_high) */ ((VL53L0X_GpioFunctionality) 2) /*!< Level High (value > thresh_high) */
#define VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT \ #define VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT \
((VL53L0X_GpioFunctionality) 3) ((VL53L0X_GpioFunctionality) 3)
/*!< Out Of Window (value < thresh_low OR value > thresh_high) */ /*!< Out Of Window (value < thresh_low OR value > thresh_high) */
#define VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY \ #define VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY \
((VL53L0X_GpioFunctionality) 4) /*!< New Sample Ready */ ((VL53L0X_GpioFunctionality) 4) /*!< New Sample Ready */
/** @} end of VL53L0X_GpioFunctionality_group */ /** @} end of VL53L0X_GpioFunctionality_group */
/* Device register map */ /* Device register map */
/** @defgroup VL53L0X_DefineRegisters_group Define Registers /** @defgroup VL53L0X_DefineRegisters_group Define Registers
* @brief List of all the defined registers * @brief List of all the defined registers
* @{ * @{
*/ */
#define VL53L0X_REG_SYSRANGE_START 0x000 #define VL53L0X_REG_SYSRANGE_START 0x000
/** mask existing bit in #VL53L0X_REG_SYSRANGE_START*/ /** mask existing bit in #VL53L0X_REG_SYSRANGE_START*/
#define VL53L0X_REG_SYSRANGE_MODE_MASK 0x0F #define VL53L0X_REG_SYSRANGE_MODE_MASK 0x0F
/** bit 0 in #VL53L0X_REG_SYSRANGE_START write 1 toggle state in /** bit 0 in #VL53L0X_REG_SYSRANGE_START write 1 toggle state in
* continuous mode and arm next shot in single shot mode */ * continuous mode and arm next shot in single shot mode */
#define VL53L0X_REG_SYSRANGE_MODE_START_STOP 0x01 #define VL53L0X_REG_SYSRANGE_MODE_START_STOP 0x01
/** bit 1 write 0 in #VL53L0X_REG_SYSRANGE_START set single shot mode */ /** bit 1 write 0 in #VL53L0X_REG_SYSRANGE_START set single shot mode */
#define VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT 0x00 #define VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT 0x00
/** bit 1 write 1 in #VL53L0X_REG_SYSRANGE_START set back-to-back /** bit 1 write 1 in #VL53L0X_REG_SYSRANGE_START set back-to-back
* operation mode */ * operation mode */
#define VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK 0x02 #define VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK 0x02
/** bit 2 write 1 in #VL53L0X_REG_SYSRANGE_START set timed operation /** bit 2 write 1 in #VL53L0X_REG_SYSRANGE_START set timed operation
* mode */ * mode */
#define VL53L0X_REG_SYSRANGE_MODE_TIMED 0x04 #define VL53L0X_REG_SYSRANGE_MODE_TIMED 0x04
/** bit 3 write 1 in #VL53L0X_REG_SYSRANGE_START set histogram operation /** bit 3 write 1 in #VL53L0X_REG_SYSRANGE_START set histogram operation
* mode */ * mode */
#define VL53L0X_REG_SYSRANGE_MODE_HISTOGRAM 0x08 #define VL53L0X_REG_SYSRANGE_MODE_HISTOGRAM 0x08
#define VL53L0X_REG_SYSTEM_THRESH_HIGH 0x000C #define VL53L0X_REG_SYSTEM_THRESH_HIGH 0x000C
#define VL53L0X_REG_SYSTEM_THRESH_LOW 0x000E #define VL53L0X_REG_SYSTEM_THRESH_LOW 0x000E
#define VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG 0x0001 #define VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG 0x0001
#define VL53L0X_REG_SYSTEM_RANGE_CONFIG 0x0009 #define VL53L0X_REG_SYSTEM_RANGE_CONFIG 0x0009
#define VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD 0x0004 #define VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD 0x0004
#define VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO 0x000A #define VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO 0x000A
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_DISABLED 0x00 #define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_DISABLED 0x00
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_LEVEL_LOW 0x01 #define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_LEVEL_LOW 0x01
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_LEVEL_HIGH 0x02 #define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_LEVEL_HIGH 0x02
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_OUT_OF_WINDOW 0x03 #define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_OUT_OF_WINDOW 0x03
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY 0x04 #define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY 0x04
#define VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH 0x0084 #define VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH 0x0084
#define VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR 0x000B #define VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR 0x000B
/* Result registers */ /* Result registers */
#define VL53L0X_REG_RESULT_INTERRUPT_STATUS 0x0013 #define VL53L0X_REG_RESULT_INTERRUPT_STATUS 0x0013
#define VL53L0X_REG_RESULT_RANGE_STATUS 0x0014 #define VL53L0X_REG_RESULT_RANGE_STATUS 0x0014
#define VL53L0X_REG_RESULT_CORE_PAGE 1 #define VL53L0X_REG_RESULT_CORE_PAGE 1
#define VL53L0X_REG_RESULT_CORE_AMBIENT_WINDOW_EVENTS_RTN 0x00BC #define VL53L0X_REG_RESULT_CORE_AMBIENT_WINDOW_EVENTS_RTN 0x00BC
#define VL53L0X_REG_RESULT_CORE_RANGING_TOTAL_EVENTS_RTN 0x00C0 #define VL53L0X_REG_RESULT_CORE_RANGING_TOTAL_EVENTS_RTN 0x00C0
#define VL53L0X_REG_RESULT_CORE_AMBIENT_WINDOW_EVENTS_REF 0x00D0 #define VL53L0X_REG_RESULT_CORE_AMBIENT_WINDOW_EVENTS_REF 0x00D0
#define VL53L0X_REG_RESULT_CORE_RANGING_TOTAL_EVENTS_REF 0x00D4 #define VL53L0X_REG_RESULT_CORE_RANGING_TOTAL_EVENTS_REF 0x00D4
#define VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF 0x00B6 #define VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF 0x00B6
/* Algo register */ /* Algo register */
#define VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM 0x0028 #define VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM 0x0028
#define VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS 0x008a #define VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS 0x008a
/* Check Limit registers */ /* Check Limit registers */
#define VL53L0X_REG_MSRC_CONFIG_CONTROL 0x0060 #define VL53L0X_REG_MSRC_CONFIG_CONTROL 0x0060
#define VL53L0X_REG_PRE_RANGE_CONFIG_MIN_SNR 0X0027 #define VL53L0X_REG_PRE_RANGE_CONFIG_MIN_SNR 0X0027
#define VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW 0x0056 #define VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW 0x0056
#define VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH 0x0057 #define VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH 0x0057
#define VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT 0x0064 #define VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT 0x0064
#define VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_SNR 0X0067 #define VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_SNR 0X0067
#define VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW 0x0047 #define VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW 0x0047
#define VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH 0x0048 #define VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH 0x0048
#define VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT 0x0044 #define VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT 0x0044
#define VL53L0X_REG_PRE_RANGE_CONFIG_SIGMA_THRESH_HI 0X0061 #define VL53L0X_REG_PRE_RANGE_CONFIG_SIGMA_THRESH_HI 0X0061
#define VL53L0X_REG_PRE_RANGE_CONFIG_SIGMA_THRESH_LO 0X0062 #define VL53L0X_REG_PRE_RANGE_CONFIG_SIGMA_THRESH_LO 0X0062
/* PRE RANGE registers */ /* PRE RANGE registers */
#define VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD 0x0050 #define VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD 0x0050
#define VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI 0x0051 #define VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI 0x0051
#define VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_LO 0x0052 #define VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_LO 0x0052
#define VL53L0X_REG_SYSTEM_HISTOGRAM_BIN 0x0081 #define VL53L0X_REG_SYSTEM_HISTOGRAM_BIN 0x0081
#define VL53L0X_REG_HISTOGRAM_CONFIG_INITIAL_PHASE_SELECT 0x0033 #define VL53L0X_REG_HISTOGRAM_CONFIG_INITIAL_PHASE_SELECT 0x0033
#define VL53L0X_REG_HISTOGRAM_CONFIG_READOUT_CTRL 0x0055 #define VL53L0X_REG_HISTOGRAM_CONFIG_READOUT_CTRL 0x0055
#define VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD 0x0070 #define VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD 0x0070
#define VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI 0x0071 #define VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI 0x0071
#define VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO 0x0072 #define VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO 0x0072
#define VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS 0x0020 #define VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS 0x0020
#define VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP 0x0046 #define VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP 0x0046
#define VL53L0X_REG_SOFT_RESET_GO2_SOFT_RESET_N 0x00bf #define VL53L0X_REG_SOFT_RESET_GO2_SOFT_RESET_N 0x00bf
#define VL53L0X_REG_IDENTIFICATION_MODEL_ID 0x00c0 #define VL53L0X_REG_IDENTIFICATION_MODEL_ID 0x00c0
#define VL53L0X_REG_IDENTIFICATION_REVISION_ID 0x00c2 #define VL53L0X_REG_IDENTIFICATION_REVISION_ID 0x00c2
#define VL53L0X_REG_OSC_CALIBRATE_VAL 0x00f8 #define VL53L0X_REG_OSC_CALIBRATE_VAL 0x00f8
#define VL53L0X_SIGMA_ESTIMATE_MAX_VALUE 65535 #define VL53L0X_SIGMA_ESTIMATE_MAX_VALUE 65535
/* equivalent to a range sigma of 655.35mm */ /* equivalent to a range sigma of 655.35mm */
#define VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH 0x032 #define VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH 0x032
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0 0x0B0 #define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0 0x0B0
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_1 0x0B1 #define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_1 0x0B1
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_2 0x0B2 #define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_2 0x0B2
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_3 0x0B3 #define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_3 0x0B3
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_4 0x0B4 #define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_4 0x0B4
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_5 0x0B5 #define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_5 0x0B5
#define VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT 0xB6 #define VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT 0xB6
#define VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD 0x4E /* 0x14E */ #define VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD 0x4E /* 0x14E */
#define VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET 0x4F /* 0x14F */ #define VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET 0x4F /* 0x14F */
#define VL53L0X_REG_POWER_MANAGEMENT_GO1_POWER_FORCE 0x80 #define VL53L0X_REG_POWER_MANAGEMENT_GO1_POWER_FORCE 0x80
/* /*
* Speed of light in um per 1E-10 Seconds * Speed of light in um per 1E-10 Seconds
*/ */
#define VL53L0X_SPEED_OF_LIGHT_IN_AIR 2997 #define VL53L0X_SPEED_OF_LIGHT_IN_AIR 2997
#define VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV 0x0089 #define VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV 0x0089
#define VL53L0X_REG_ALGO_PHASECAL_LIM 0x0030 /* 0x130 */ #define VL53L0X_REG_ALGO_PHASECAL_LIM 0x0030 /* 0x130 */
#define VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT 0x0030 #define VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT 0x0030
/** @} VL53L0X_DefineRegisters_group */ /** @} VL53L0X_DefineRegisters_group */
/** @} VL53L0X_DevSpecDefines_group */ /** @} VL53L0X_DevSpecDefines_group */
#endif #endif
/* _VL53L0X_DEVICE_H_ */ /* _VL53L0X_DEVICE_H_ */
main/CMakeLists.txt
-3
View File
@@ -7,6 +7,3 @@ target_link_libraries(lidar_test common_lib)
add_executable(remote_control remote_control.cpp) add_executable(remote_control remote_control.cpp)
target_link_libraries(remote_control common_lib) target_link_libraries(remote_control common_lib)
add_executable(lcd_test lcd_test.cpp)
target_link_libraries(lcd_test common_lib ${OpenCV_LIBS})
main/main.cpp
+44 -44
View File
@@ -1,44 +1,44 @@
#include <iostream> #include <iostream>
#include <opencv2/opencv.hpp> #include <opencv2/opencv.hpp>
#include <csignal> #include <csignal>
#include <atomic> #include <atomic>
#include <thread> #include <thread>
#include <chrono> #include <chrono>
#include <sched.h> #include <sched.h>
#include <sys/resource.h> #include <sys/resource.h>
#include "global.h" #include "global.h"
#include "camera.h" #include "camera.h"
#include "control.h" #include "control.h"
std::atomic<bool> running(true); std::atomic<bool> running(true);
void signalHandler(int) { running.store(false); } void signalHandler(int) { running.store(false); }
int main(void) int main(void)
{ {
std::signal(SIGINT, signalHandler); std::signal(SIGINT, signalHandler);
cfg_load_all(); cfg_load_all();
if (CameraInit(0) < 0) { if (CameraInit(0) < 0) {
std::cerr << "CameraInit failed" << std::endl; std::cerr << "CameraInit failed" << std::endl;
return -1; return -1;
} }
ControlInit(); ControlInit();
std::cout << "All services started" << std::endl; std::cout << "All services started" << std::endl;
setpriority(PRIO_PROCESS, 0, -10); setpriority(PRIO_PROCESS, 0, -10);
while (running.load()) { while (running.load()) {
CameraHandler(); CameraHandler();
target_speed = g_cfg.speed; target_speed = g_cfg.speed;
sched_yield(); sched_yield();
} }
std::cout << "Stopping..." << std::endl; std::cout << "Stopping..." << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(500)); std::this_thread::sleep_for(std::chrono::milliseconds(500));
ControlExit(); ControlExit();
cameraDeInit(); cameraDeInit();
std::cout << "Stopped." << std::endl; std::cout << "Stopped." << std::endl;
return 0; return 0;
} }
src/camera.cpp
+1 -72
View File
@@ -25,7 +25,6 @@
#include "camera.h" #include "camera.h"
#include "model_v10.hpp" #include "model_v10.hpp"
#include "vl53l0x.h" #include "vl53l0x.h"
#include "font_bitmap.h"
#include <fcntl.h> #include <fcntl.h>
#include <unistd.h> #include <unistd.h>
@@ -218,76 +217,6 @@ static void play_zebra_audio()
} }
// ═══════════════════════════════════════════════════════════
// LCD 中文文字渲染
//
// 使用 src/font_bitmap.h 中嵌入的 16x16 字模
// ═══════════════════════════════════════════════════════════
static const uint8_t* find_glyph(uint32_t codepoint)
{
for (int i = 0; i < FONT_GLYPH_COUNT; ++i)
if (FONT_GLYPHS[i].unicode == codepoint) return FONT_GLYPHS[i].bitmap;
return nullptr;
}
static uint32_t utf8_decode(const char*& p)
{
unsigned char c = static_cast<unsigned char>(*p);
if (c == 0) return 0;
if ((c & 0x80) == 0) { ++p; return c; }
if ((c & 0xE0) == 0xC0) {
uint32_t v = c & 0x1F; ++p;
v = (v << 6) | (static_cast<unsigned char>(*p++) & 0x3F);
return v;
}
if ((c & 0xF0) == 0xE0) {
uint32_t v = c & 0x0F; ++p;
v = (v << 6) | (static_cast<unsigned char>(*p++) & 0x3F);
v = (v << 6) | (static_cast<unsigned char>(*p++) & 0x3F);
return v;
}
if ((c & 0xF8) == 0xF0) {
uint32_t v = c & 0x07; ++p;
v = (v << 6) | (static_cast<unsigned char>(*p++) & 0x3F);
v = (v << 6) | (static_cast<unsigned char>(*p++) & 0x3F);
v = (v << 6) | (static_cast<unsigned char>(*p++) & 0x3F);
return v;
}
++p;
return '?';
}
static void lcd_draw_glyph(cv::Mat& img, int x0, int y0, const uint8_t* glyph, const cv::Scalar& color)
{
for (int row = 0; row < FONT_SIZE; ++row) {
for (int col = 0; col < FONT_SIZE; ++col) {
if (glyph[row * FONT_SIZE + col] > 128) {
int px = x0 + col;
int py = y0 + row;
if (px >= 0 && px < img.cols && py >= 0 && py < img.rows)
img.at<cv::Vec3b>(py, px) = cv::Vec3b(static_cast<uchar>(color[0]),
static_cast<uchar>(color[1]),
static_cast<uchar>(color[2]));
}
}
}
}
static void lcd_draw_chinese_text(cv::Mat& img, const char* text, int x, int y, const cv::Scalar& color)
{
const char* p = text;
int cx = x;
while (*p) {
uint32_t cp = utf8_decode(p);
const uint8_t* glyph = find_glyph(cp);
if (glyph) {
lcd_draw_glyph(img, cx, y, glyph, color);
cx += FONT_SIZE;
}
}
}
// ═══════════════════════════════════════════════════════════ // ═══════════════════════════════════════════════════════════
// CameraInit — 系统初始化 // CameraInit — 系统初始化
// //
@@ -1102,7 +1031,7 @@ static void motor_update(bool zebra_block, bool tl_block)
prev_zstate = g_zstate; prev_zstate = g_zstate;
prev_tlstate = g_tl_state; prev_tlstate = g_tl_state;
double spd = target_speed * boost_factor(); double spd = (boost_factor() > 1.0) ? 26.0 : target_speed;
// 斑马线起步后 3.3~5s 降速走稳 // 斑马线起步后 3.3~5s 降速走稳
if (g_zstate == Z_COOLDOWN) { if (g_zstate == Z_COOLDOWN) {
src/global.cpp
+66 -66
View File
@@ -1,66 +1,66 @@
#include "global.h" #include "global.h"
#include <fstream> #include <fstream>
CfgCache g_cfg; CfgCache g_cfg;
double target_speed; double target_speed;
double readDoubleFromFile(const std::string &filename) double readDoubleFromFile(const std::string &filename)
{ {
std::ifstream file(filename); std::ifstream file(filename);
double value = 0.0; double value = 0.0;
if (file.is_open()) { file >> value; file.close(); } if (file.is_open()) { file >> value; file.close(); }
return value; return value;
} }
bool readFlag(const std::string &filename) bool readFlag(const std::string &filename)
{ {
std::ifstream file(filename); std::ifstream file(filename);
int flag = 0; int flag = 0;
if (file.is_open()) { file >> flag; file.close(); } if (file.is_open()) { file >> flag; file.close(); }
return flag; return flag;
} }
void cfg_load_all() void cfg_load_all()
{ {
g_cfg.speed = readDoubleFromFile(speed_file); g_cfg.speed = readDoubleFromFile(speed_file);
g_cfg.foresee = readDoubleFromFile(foresee_file); g_cfg.foresee = readDoubleFromFile(foresee_file);
g_cfg.foresee_lost_scale = readDoubleFromFile(foresee_lost_scale_file); g_cfg.foresee_lost_scale = readDoubleFromFile(foresee_lost_scale_file);
g_cfg.sharp_turn_scale = readDoubleFromFile(sharp_turn_scale_file); g_cfg.sharp_turn_scale = readDoubleFromFile(sharp_turn_scale_file);
g_cfg.zebrasee = readDoubleFromFile(zebrasee_file); g_cfg.zebrasee = readDoubleFromFile(zebrasee_file);
g_cfg.deadband = readDoubleFromFile(deadband_file); g_cfg.deadband = readDoubleFromFile(deadband_file);
g_cfg.steer_gain = readDoubleFromFile(steer_gain_file); g_cfg.steer_gain = readDoubleFromFile(steer_gain_file);
g_cfg.center_bias = readDoubleFromFile(center_bias_file); g_cfg.center_bias = readDoubleFromFile(center_bias_file);
g_cfg.start = readFlag(start_file); g_cfg.start = readFlag(start_file);
g_cfg.showImg = readFlag(showImg_file); g_cfg.showImg = readFlag(showImg_file);
g_cfg.debug = readFlag(debug_file); g_cfg.debug = readFlag(debug_file);
g_cfg.cone_speed = readDoubleFromFile(cone_speed_file); g_cfg.cone_speed = readDoubleFromFile(cone_speed_file);
g_cfg.cone_min_frames = (int)readDoubleFromFile(cone_min_frames_file); g_cfg.cone_min_frames = (int)readDoubleFromFile(cone_min_frames_file);
g_cfg.cone_margin = (int)readDoubleFromFile(cone_margin_file); g_cfg.cone_margin = (int)readDoubleFromFile(cone_margin_file);
g_cfg.cone_thresh = readDoubleFromFile(cone_thresh_file); g_cfg.cone_thresh = readDoubleFromFile(cone_thresh_file);
g_cfg.cone_avoid_gain = readDoubleFromFile(cone_avoid_gain_file); g_cfg.cone_avoid_gain = readDoubleFromFile(cone_avoid_gain_file);
g_cfg.cone_avoid_range = (int)readDoubleFromFile(cone_avoid_range_file); g_cfg.cone_avoid_range = (int)readDoubleFromFile(cone_avoid_range_file);
g_cfg.cone_hold_frames = (int)readDoubleFromFile(cone_hold_frames_file); g_cfg.cone_hold_frames = (int)readDoubleFromFile(cone_hold_frames_file);
g_cfg.cone_return_gain = readDoubleFromFile(cone_return_gain_file); g_cfg.cone_return_gain = readDoubleFromFile(cone_return_gain_file);
g_cfg.cone_return_frames = (int)readDoubleFromFile(cone_return_frames_file); g_cfg.cone_return_frames = (int)readDoubleFromFile(cone_return_frames_file);
g_cfg.brake_scale = readDoubleFromFile(brake_scale_file); g_cfg.brake_scale = readDoubleFromFile(brake_scale_file);
g_cfg.brake_max = readDoubleFromFile(brake_max_file); g_cfg.brake_max = readDoubleFromFile(brake_max_file);
g_cfg.curve_slope = readDoubleFromFile(curve_slope_file); g_cfg.curve_slope = readDoubleFromFile(curve_slope_file);
g_cfg.curve_min = readDoubleFromFile(curve_min_file); g_cfg.curve_min = readDoubleFromFile(curve_min_file);
g_cfg.lidar_thresh = (int)readDoubleFromFile(lidar_thresh_file); g_cfg.lidar_thresh = (int)readDoubleFromFile(lidar_thresh_file);
g_cfg.lidar_pre = (int)readDoubleFromFile(lidar_pre_file); g_cfg.lidar_pre = (int)readDoubleFromFile(lidar_pre_file);
g_cfg.lidar_near = (int)readDoubleFromFile(lidar_near_file); g_cfg.lidar_near = (int)readDoubleFromFile(lidar_near_file);
g_cfg.lidar_far = (int)readDoubleFromFile(lidar_far_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_start = (int)readDoubleFromFile(lidar_near_start_file);
g_cfg.lidar_near_end = (int)readDoubleFromFile(lidar_near_end_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_far_span = (int)readDoubleFromFile(lidar_far_span_file);
g_cfg.lidar_min_frames = (int)readDoubleFromFile(lidar_min_frames_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_gain = readDoubleFromFile(lidar_avoid_gain_file);
g_cfg.lidar_avoid_range = (int)readDoubleFromFile(lidar_avoid_range_file); g_cfg.lidar_avoid_range = (int)readDoubleFromFile(lidar_avoid_range_file);
g_cfg.lidar_speed = readDoubleFromFile(lidar_speed_file); g_cfg.lidar_speed = readDoubleFromFile(lidar_speed_file);
g_cfg.lidar_hold_frames = (int)readDoubleFromFile(lidar_hold_frames_file); g_cfg.lidar_hold_frames = (int)readDoubleFromFile(lidar_hold_frames_file);
g_cfg.lidar_enable = (int)readDoubleFromFile(lidar_enable_file); g_cfg.lidar_enable = (int)readDoubleFromFile(lidar_enable_file);
} }
src/vl53l0x.cpp
+174 -174
View File
@@ -1,174 +1,174 @@
#include "vl53l0x.h" #include "vl53l0x.h"
#include <fcntl.h> #include <fcntl.h>
#include <unistd.h> #include <unistd.h>
#include <sys/ioctl.h> #include <sys/ioctl.h>
#include <linux/i2c-dev.h> #include <linux/i2c-dev.h>
#include <cstring> #include <cstring>
#include <cstdio> #include <cstdio>
#include <cerrno> #include <cerrno>
extern "C" { extern "C" {
#include "vl53l0x_api.h" #include "vl53l0x_api.h"
} }
static void unbind_kernel_driver() static void unbind_kernel_driver()
{ {
FILE *f = fopen("/sys/bus/i2c/drivers/stmvl53l0/unbind", "w"); FILE *f = fopen("/sys/bus/i2c/drivers/stmvl53l0/unbind", "w");
if (f) { if (f) {
fprintf(f, "1-0029"); fprintf(f, "1-0029");
fclose(f); fclose(f);
} }
} }
VL53L0X::VL53L0X() VL53L0X::VL53L0X()
: fd(-1), initialized(false) : fd(-1), initialized(false)
{ {
memset(&dev, 0, sizeof(dev)); memset(&dev, 0, sizeof(dev));
} }
VL53L0X::~VL53L0X() VL53L0X::~VL53L0X()
{ {
stop(); stop();
} }
bool VL53L0X::init() bool VL53L0X::init()
{ {
unbind_kernel_driver(); unbind_kernel_driver();
usleep(100000); usleep(100000);
fd = open("/dev/i2c-1", O_RDWR); fd = open("/dev/i2c-1", O_RDWR);
if (fd < 0) { if (fd < 0) {
fprintf(stderr, "[VL53L0X] open /dev/i2c-1 failed: %s\n", strerror(errno)); fprintf(stderr, "[VL53L0X] open /dev/i2c-1 failed: %s\n", strerror(errno));
return false; return false;
} }
if (ioctl(fd, I2C_SLAVE, 0x29) < 0) { if (ioctl(fd, I2C_SLAVE, 0x29) < 0) {
fprintf(stderr, "[VL53L0X] I2C_SLAVE failed: %s\n", strerror(errno)); fprintf(stderr, "[VL53L0X] I2C_SLAVE failed: %s\n", strerror(errno));
close(fd); close(fd);
fd = -1; fd = -1;
return false; return false;
} }
dev.I2cDevAddr = 0x29; dev.I2cDevAddr = 0x29;
dev.i2c_fd = fd; dev.i2c_fd = fd;
VL53L0X_Error Status = VL53L0X_DataInit(&dev); VL53L0X_Error Status = VL53L0X_DataInit(&dev);
if (Status != VL53L0X_ERROR_NONE) { if (Status != VL53L0X_ERROR_NONE) {
fprintf(stderr, "[VL53L0X] DataInit failed: %d\n", Status); fprintf(stderr, "[VL53L0X] DataInit failed: %d\n", Status);
close(fd); close(fd);
fd = -1; fd = -1;
return false; return false;
} }
Status = VL53L0X_StaticInit(&dev); Status = VL53L0X_StaticInit(&dev);
if (Status != VL53L0X_ERROR_NONE) { if (Status != VL53L0X_ERROR_NONE) {
fprintf(stderr, "[VL53L0X] StaticInit failed: %d\n", Status); fprintf(stderr, "[VL53L0X] StaticInit failed: %d\n", Status);
close(fd); close(fd);
fd = -1; fd = -1;
return false; return false;
} }
uint32_t refSpadCount; uint32_t refSpadCount;
uint8_t isApertureSpads; uint8_t isApertureSpads;
uint8_t VhvSettings; uint8_t VhvSettings;
uint8_t PhaseCal; uint8_t PhaseCal;
VL53L0X_PerformRefCalibration(&dev, &VhvSettings, &PhaseCal); VL53L0X_PerformRefCalibration(&dev, &VhvSettings, &PhaseCal);
VL53L0X_PerformRefSpadManagement(&dev, &refSpadCount, &isApertureSpads); VL53L0X_PerformRefSpadManagement(&dev, &refSpadCount, &isApertureSpads);
Status = VL53L0X_SetDeviceMode(&dev, VL53L0X_DEVICEMODE_SINGLE_RANGING); Status = VL53L0X_SetDeviceMode(&dev, VL53L0X_DEVICEMODE_SINGLE_RANGING);
if (Status != VL53L0X_ERROR_NONE) { if (Status != VL53L0X_ERROR_NONE) {
fprintf(stderr, "[VL53L0X] SetDeviceMode failed: %d\n", Status); fprintf(stderr, "[VL53L0X] SetDeviceMode failed: %d\n", Status);
close(fd); close(fd);
fd = -1; fd = -1;
return false; return false;
} }
VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&dev, 20000); VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&dev, 20000);
uint32_t actualBudget = 0; uint32_t actualBudget = 0;
VL53L0X_GetMeasurementTimingBudgetMicroSeconds(&dev, &actualBudget); VL53L0X_GetMeasurementTimingBudgetMicroSeconds(&dev, &actualBudget);
fprintf(stderr, "[VL53L0X] timing budget = %u us\n", actualBudget); fprintf(stderr, "[VL53L0X] timing budget = %u us\n", actualBudget);
initialized = true; initialized = true;
fprintf(stderr, "[VL53L0X] init OK\n"); fprintf(stderr, "[VL53L0X] init OK\n");
return true; return true;
} }
bool VL53L0X::readRange(VL53L0X_RangingMeasurementData_t &data) bool VL53L0X::readRange(VL53L0X_RangingMeasurementData_t &data)
{ {
if (!initialized) return false; if (!initialized) return false;
VL53L0X_Error Status; VL53L0X_Error Status;
VL53L0X_ClearInterruptMask(&dev, 0); VL53L0X_ClearInterruptMask(&dev, 0);
Status = VL53L0X_StartMeasurement(&dev); Status = VL53L0X_StartMeasurement(&dev);
if (Status != VL53L0X_ERROR_NONE) { if (Status != VL53L0X_ERROR_NONE) {
fprintf(stderr, "[VL53L0X] StartMeasurement failed: %d\n", Status); fprintf(stderr, "[VL53L0X] StartMeasurement failed: %d\n", Status);
return false; return false;
} }
uint8_t ready = 0; uint8_t ready = 0;
int timeout = 500; int timeout = 500;
while (!ready && --timeout > 0) { while (!ready && --timeout > 0) {
Status = VL53L0X_GetMeasurementDataReady(&dev, &ready); Status = VL53L0X_GetMeasurementDataReady(&dev, &ready);
if (Status != VL53L0X_ERROR_NONE) if (Status != VL53L0X_ERROR_NONE)
break; break;
if (!ready) if (!ready)
usleep(1000); usleep(1000);
} }
if (!ready) { if (!ready) {
fprintf(stderr, "[VL53L0X] measurement timeout\n"); fprintf(stderr, "[VL53L0X] measurement timeout\n");
return false; return false;
} }
Status = VL53L0X_GetRangingMeasurementData(&dev, &data); Status = VL53L0X_GetRangingMeasurementData(&dev, &data);
if (Status != VL53L0X_ERROR_NONE) { if (Status != VL53L0X_ERROR_NONE) {
fprintf(stderr, "[VL53L0X] GetRangingMeasurementData failed: %d\n", Status); fprintf(stderr, "[VL53L0X] GetRangingMeasurementData failed: %d\n", Status);
return false; return false;
} }
VL53L0X_ClearInterruptMask(&dev, 0); VL53L0X_ClearInterruptMask(&dev, 0);
return true; return true;
} }
bool VL53L0X::startMeasure() bool VL53L0X::startMeasure()
{ {
if (!initialized) return false; if (!initialized) return false;
VL53L0X_ClearInterruptMask(&dev, 0); VL53L0X_ClearInterruptMask(&dev, 0);
VL53L0X_Error Status = VL53L0X_StartMeasurement(&dev); VL53L0X_Error Status = VL53L0X_StartMeasurement(&dev);
return (Status == VL53L0X_ERROR_NONE); return (Status == VL53L0X_ERROR_NONE);
} }
bool VL53L0X::readResult(VL53L0X_RangingMeasurementData_t &data) bool VL53L0X::readResult(VL53L0X_RangingMeasurementData_t &data)
{ {
if (!initialized) return false; if (!initialized) return false;
uint8_t ready = 0; uint8_t ready = 0;
VL53L0X_Error Status; VL53L0X_Error Status;
int timeout = 25; // 25ms max, 读不到就跳过 int timeout = 25; // 25ms max, 读不到就跳过
while (!ready && --timeout > 0) { while (!ready && --timeout > 0) {
Status = VL53L0X_GetMeasurementDataReady(&dev, &ready); Status = VL53L0X_GetMeasurementDataReady(&dev, &ready);
if (Status != VL53L0X_ERROR_NONE) if (Status != VL53L0X_ERROR_NONE)
return false; return false;
if (!ready) if (!ready)
usleep(1000); usleep(1000);
} }
if (!ready) return false; if (!ready) return false;
Status = VL53L0X_GetRangingMeasurementData(&dev, &data); Status = VL53L0X_GetRangingMeasurementData(&dev, &data);
VL53L0X_ClearInterruptMask(&dev, 0); VL53L0X_ClearInterruptMask(&dev, 0);
return (Status == VL53L0X_ERROR_NONE); return (Status == VL53L0X_ERROR_NONE);
} }
bool VL53L0X::stop() bool VL53L0X::stop()
{ {
if (fd >= 0) { if (fd >= 0) {
close(fd); close(fd);
fd = -1; fd = -1;
} }
initialized = false; initialized = false;
return true; return true;
} }