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
src/camera.cpp
+1 -72
View File
@@ -25,7 +25,6 @@
#include "camera.h"
#include "model_v10.hpp"
#include "vl53l0x.h"
#include "font_bitmap.h"
#include <fcntl.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 — 系统初始化
//
@@ -1102,7 +1031,7 @@ static void motor_update(bool zebra_block, bool tl_block)
prev_zstate = g_zstate;
prev_tlstate = g_tl_state;
double spd = target_speed * boost_factor();
double spd = (boost_factor() > 1.0) ? 26.0 : target_speed;
// 斑马线起步后 3.3~5s 降速走稳
if (g_zstate == Z_COOLDOWN) {
src/global.cpp
+66 -66
View File
@@ -1,66 +1,66 @@
#include "global.h"
#include <fstream>
CfgCache g_cfg;
double target_speed;
double readDoubleFromFile(const std::string &filename)
{
std::ifstream file(filename);
double value = 0.0;
if (file.is_open()) { file >> value; file.close(); }
return value;
}
bool readFlag(const std::string &filename)
{
std::ifstream file(filename);
int flag = 0;
if (file.is_open()) { file >> flag; file.close(); }
return flag;
}
void cfg_load_all()
{
g_cfg.speed = readDoubleFromFile(speed_file);
g_cfg.foresee = readDoubleFromFile(foresee_file);
g_cfg.foresee_lost_scale = readDoubleFromFile(foresee_lost_scale_file);
g_cfg.sharp_turn_scale = readDoubleFromFile(sharp_turn_scale_file);
g_cfg.zebrasee = readDoubleFromFile(zebrasee_file);
g_cfg.deadband = readDoubleFromFile(deadband_file);
g_cfg.steer_gain = readDoubleFromFile(steer_gain_file);
g_cfg.center_bias = readDoubleFromFile(center_bias_file);
g_cfg.start = readFlag(start_file);
g_cfg.showImg = readFlag(showImg_file);
g_cfg.debug = readFlag(debug_file);
g_cfg.cone_speed = readDoubleFromFile(cone_speed_file);
g_cfg.cone_min_frames = (int)readDoubleFromFile(cone_min_frames_file);
g_cfg.cone_margin = (int)readDoubleFromFile(cone_margin_file);
g_cfg.cone_thresh = readDoubleFromFile(cone_thresh_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_hold_frames = (int)readDoubleFromFile(cone_hold_frames_file);
g_cfg.cone_return_gain = readDoubleFromFile(cone_return_gain_file);
g_cfg.cone_return_frames = (int)readDoubleFromFile(cone_return_frames_file);
g_cfg.brake_scale = readDoubleFromFile(brake_scale_file);
g_cfg.brake_max = readDoubleFromFile(brake_max_file);
g_cfg.curve_slope = readDoubleFromFile(curve_slope_file);
g_cfg.curve_min = readDoubleFromFile(curve_min_file);
g_cfg.lidar_thresh = (int)readDoubleFromFile(lidar_thresh_file);
g_cfg.lidar_pre = (int)readDoubleFromFile(lidar_pre_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);
}
#include "global.h"
#include <fstream>
CfgCache g_cfg;
double target_speed;
double readDoubleFromFile(const std::string &filename)
{
std::ifstream file(filename);
double value = 0.0;
if (file.is_open()) { file >> value; file.close(); }
return value;
}
bool readFlag(const std::string &filename)
{
std::ifstream file(filename);
int flag = 0;
if (file.is_open()) { file >> flag; file.close(); }
return flag;
}
void cfg_load_all()
{
g_cfg.speed = readDoubleFromFile(speed_file);
g_cfg.foresee = readDoubleFromFile(foresee_file);
g_cfg.foresee_lost_scale = readDoubleFromFile(foresee_lost_scale_file);
g_cfg.sharp_turn_scale = readDoubleFromFile(sharp_turn_scale_file);
g_cfg.zebrasee = readDoubleFromFile(zebrasee_file);
g_cfg.deadband = readDoubleFromFile(deadband_file);
g_cfg.steer_gain = readDoubleFromFile(steer_gain_file);
g_cfg.center_bias = readDoubleFromFile(center_bias_file);
g_cfg.start = readFlag(start_file);
g_cfg.showImg = readFlag(showImg_file);
g_cfg.debug = readFlag(debug_file);
g_cfg.cone_speed = readDoubleFromFile(cone_speed_file);
g_cfg.cone_min_frames = (int)readDoubleFromFile(cone_min_frames_file);
g_cfg.cone_margin = (int)readDoubleFromFile(cone_margin_file);
g_cfg.cone_thresh = readDoubleFromFile(cone_thresh_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_hold_frames = (int)readDoubleFromFile(cone_hold_frames_file);
g_cfg.cone_return_gain = readDoubleFromFile(cone_return_gain_file);
g_cfg.cone_return_frames = (int)readDoubleFromFile(cone_return_frames_file);
g_cfg.brake_scale = readDoubleFromFile(brake_scale_file);
g_cfg.brake_max = readDoubleFromFile(brake_max_file);
g_cfg.curve_slope = readDoubleFromFile(curve_slope_file);
g_cfg.curve_min = readDoubleFromFile(curve_min_file);
g_cfg.lidar_thresh = (int)readDoubleFromFile(lidar_thresh_file);
g_cfg.lidar_pre = (int)readDoubleFromFile(lidar_pre_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);
}
src/vl53l0x.cpp
+174 -174
View File
@@ -1,174 +1,174 @@
#include "vl53l0x.h"
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <linux/i2c-dev.h>
#include <cstring>
#include <cstdio>
#include <cerrno>
extern "C" {
#include "vl53l0x_api.h"
}
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()
{
stop();
}
bool VL53L0X::init()
{
unbind_kernel_driver();
usleep(100000);
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, I2C_SLAVE, 0x29) < 0) {
fprintf(stderr, "[VL53L0X] I2C_SLAVE failed: %s\n", strerror(errno));
close(fd);
fd = -1;
return false;
}
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 (!initialized) return false;
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::startMeasure()
{
if (!initialized) return false;
VL53L0X_ClearInterruptMask(&dev, 0);
VL53L0X_Error Status = VL53L0X_StartMeasurement(&dev);
return (Status == VL53L0X_ERROR_NONE);
}
bool VL53L0X::readResult(VL53L0X_RangingMeasurementData_t &data)
{
if (!initialized) return false;
uint8_t ready = 0;
VL53L0X_Error Status;
int timeout = 25; // 25ms max, 读不到就跳过
while (!ready && --timeout > 0) {
Status = VL53L0X_GetMeasurementDataReady(&dev, &ready);
if (Status != VL53L0X_ERROR_NONE)
return false;
if (!ready)
usleep(1000);
}
if (!ready) return false;
Status = VL53L0X_GetRangingMeasurementData(&dev, &data);
VL53L0X_ClearInterruptMask(&dev, 0);
return (Status == VL53L0X_ERROR_NONE);
}
bool VL53L0X::stop()
{
if (fd >= 0) {
close(fd);
fd = -1;
}
initialized = false;
return true;
}
#include "vl53l0x.h"
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <linux/i2c-dev.h>
#include <cstring>
#include <cstdio>
#include <cerrno>
extern "C" {
#include "vl53l0x_api.h"
}
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()
{
stop();
}
bool VL53L0X::init()
{
unbind_kernel_driver();
usleep(100000);
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, I2C_SLAVE, 0x29) < 0) {
fprintf(stderr, "[VL53L0X] I2C_SLAVE failed: %s\n", strerror(errno));
close(fd);
fd = -1;
return false;
}
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 (!initialized) return false;
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::startMeasure()
{
if (!initialized) return false;
VL53L0X_ClearInterruptMask(&dev, 0);
VL53L0X_Error Status = VL53L0X_StartMeasurement(&dev);
return (Status == VL53L0X_ERROR_NONE);
}
bool VL53L0X::readResult(VL53L0X_RangingMeasurementData_t &data)
{
if (!initialized) return false;
uint8_t ready = 0;
VL53L0X_Error Status;
int timeout = 25; // 25ms max, 读不到就跳过
while (!ready && --timeout > 0) {
Status = VL53L0X_GetMeasurementDataReady(&dev, &ready);
if (Status != VL53L0X_ERROR_NONE)
return false;
if (!ready)
usleep(1000);
}
if (!ready) return false;
Status = VL53L0X_GetRangingMeasurementData(&dev, &data);
VL53L0X_ClearInterruptMask(&dev, 0);
return (Status == VL53L0X_ERROR_NONE);
}
bool VL53L0X::stop()
{
if (fd >= 0) {
close(fd);
fd = -1;
}
initialized = false;
return true;
}