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

Fix vision pipeline: search-first IPM, IMU non-blocking, LCD inverse IPM, servo direction, model WIP

Browse Source
This commit is contained in:
spdis
2026-05-25 13:57:47 +08:00
parent 610f0a7549
commit 235ec4c41e
15 changed files with 260 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
control/imu.cpp
View File

@@ -3,14 +3,19 @@
#include <unistd.h>
#include <cmath>
#include <cstring>
#include <cstdio>
#include <termios.h>
YawTracker::YawTracker() : _fd(-1), _yaw(0), _unbounded_yaw(0), _gyro_z(0), _gyro_bias(0), _ready(false) {}
bool YawTracker::begin(const char* device, int baud)
{
_fd = open(device, O_RDWR | O_NOCTTY);
if (_fd < 0) return false;
_fd = open(device, O_RDWR | O_NOCTTY | O_NONBLOCK);
if (_fd < 0) {
printf("[IMU] 无法打开 %s, 继续无IMU运行\n", device);
_ready = false;
return false;
}
termios opt;
tcgetattr(_fd, &opt);
@@ -30,16 +35,18 @@ bool YawTracker::begin(const char* device, int baud)
void YawTracker::calibrate(int samples)
{
float sum = 0;
for (int i = 0; i < samples; ++i)
int valid = 0, fail = 0;
for (int i = 0; i < samples && fail < 10; ++i)
{
int ret = _parseJY62();
if (ret > 0) sum += _gyro_z;
usleep(5000);
if (ret > 0) { sum += _gyro_z; ++valid; fail = 0; }
else { ++fail; }
usleep(2000);
}
_gyro_bias = sum / samples;
_yaw = 0;
_unbounded_yaw = 0;
_ready = true;
_gyro_bias = valid > 0 ? sum / valid : 0;
_yaw = 0; _unbounded_yaw = 0;
_ready = (valid > 10);
if (!_ready) printf("[IMU] 校准失败 (仅 %d/%d 样本), 继续无IMU运行\n", valid, samples);
}
void YawTracker::update(float dt)
@@ -60,18 +67,20 @@ int YawTracker::_parseJY62()
{
uint8 buf[11];
int total = 0;
while (total < 11)
for (int tries = 0; total < 11 && tries < 20; ++tries)
{
int n = read(_fd, buf + total, 11 - total);
if (n <= 0) return -1;
if (n < 0) { usleep(2000); continue; } // EAGAIN, 等 2ms
if (n == 0) break;
total += n;
}
if (total < 11) return -1;
while (buf[0] != 0x55 && total > 10)
for (int tries = 0; buf[0] != 0x55 && total > 10 && tries < 10; ++tries)
{
memmove(buf, buf + 1, 10);
int n = read(_fd, buf + 10, 1);
if (n <= 0) return -1;
if (n <= 0) { usleep(1000); continue; }
}
if (buf[0] != 0x55) return -1;