Initial commit: SmartCar Framework v0.1 — 龙芯2K0300智能车开发框架\n\n- HAL: GPIO/PWM/Encoder/Framebuffer 驱动\n- Control: PID/IMU/Motor/Servo 控制\n- Vision: HSV双Otsu→4点标定IPM→洪泛填充→逐行搜线\n- Strategy: 三区前瞻偏差+速度策略\n- Debug: 文件热调参+LCD预览+cv截帧\n- Scheduler: 5ms timerfd+epoll 中央调度器
This commit is contained in:
188
vision/element.cpp
Normal file
188
vision/element.cpp
Normal file
@@ -0,0 +1,188 @@
|
||||
#include "element.hpp"
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
|
||||
ElementMachine g_elm;
|
||||
|
||||
namespace {
|
||||
static constexpr uint8 k_cross_confirm_frames = 3;
|
||||
static constexpr uint8 k_cross_exit_frames = 5;
|
||||
static constexpr uint8 k_lock_min_frames = 5;
|
||||
static constexpr float k_width_widen_ratio = 1.6f;
|
||||
static constexpr float k_width_normal_ratio = 1.3f;
|
||||
|
||||
static int bottom_width(const SearchResult& sr)
|
||||
{
|
||||
int y = IMAGE_HEIGHT - 1;
|
||||
if (sr.lines.left[y] > 0 && sr.lines.right[y] > 0)
|
||||
return sr.lines.right[y] - sr.lines.left[y];
|
||||
return -1;
|
||||
}
|
||||
|
||||
static uint8 line_lost_state(const SearchResult& sr)
|
||||
{
|
||||
uint8 st = 0;
|
||||
if (!sr.lines.left_valid) st |= 1;
|
||||
if (!sr.lines.right_valid) st |= 2;
|
||||
return st;
|
||||
}
|
||||
|
||||
static void calibrate_width(const SearchResult& sr)
|
||||
{
|
||||
int w = bottom_width(sr);
|
||||
if (w > 10 && w < IMAGE_WIDTH - 10)
|
||||
{
|
||||
g_elm.normal_width = (g_elm.normal_width * g_elm.calib_cnt + w) / (g_elm.calib_cnt + 1);
|
||||
if (g_elm.calib_cnt < 50) ++g_elm.calib_cnt;
|
||||
}
|
||||
}
|
||||
|
||||
static bool is_cross_entry(const SearchResult& sr)
|
||||
{
|
||||
if (g_elm.calib_cnt < 20) return false;
|
||||
|
||||
int w = bottom_width(sr);
|
||||
if (w < 0) return false;
|
||||
|
||||
bool too_wide = (w > g_elm.normal_width * k_width_widen_ratio);
|
||||
uint8 lost = line_lost_state(sr);
|
||||
bool both_lost_upper = (lost == 3);
|
||||
|
||||
if (too_wide || both_lost_upper) g_elm.confirm_cnt++;
|
||||
else g_elm.confirm_cnt = 0;
|
||||
|
||||
return g_elm.confirm_cnt >= k_cross_confirm_frames;
|
||||
}
|
||||
|
||||
static bool is_cross_exit(const SearchResult& sr)
|
||||
{
|
||||
int w = bottom_width(sr);
|
||||
if (w < 0) { g_elm.exit_cnt = 0; return false; }
|
||||
|
||||
bool width_normal = (w < g_elm.normal_width * k_width_normal_ratio);
|
||||
bool bounds_ok = (sr.lines.left_valid && sr.lines.right_valid);
|
||||
|
||||
if (width_normal && bounds_ok) g_elm.exit_cnt++;
|
||||
else g_elm.exit_cnt = 0;
|
||||
|
||||
return g_elm.exit_cnt >= k_cross_exit_frames;
|
||||
}
|
||||
|
||||
static void cross_patch_midline(SearchResult& sr)
|
||||
{
|
||||
int bottom_y = IMAGE_HEIGHT - 1;
|
||||
int mid_x = sr.lines.mid[bottom_y];
|
||||
if (mid_x <= 0) return;
|
||||
|
||||
int lx = mid_x - static_cast<int>(g_elm.normal_width / 2);
|
||||
int rx = mid_x + static_cast<int>(g_elm.normal_width / 2);
|
||||
lx = std::max(0, lx);
|
||||
rx = std::min(IMAGE_WIDTH - 1, rx);
|
||||
|
||||
for (int y = 0; y < IMAGE_HEIGHT; ++y)
|
||||
{
|
||||
if (sr.lines.left[y] == 0 && sr.lines.right[y] == 0)
|
||||
{
|
||||
sr.lines.left[y] = static_cast<uint8>(lx);
|
||||
sr.lines.right[y] = static_cast<uint8>(rx);
|
||||
sr.lines.mid[y] = static_cast<uint8>(mid_x);
|
||||
}
|
||||
}
|
||||
sr.lines.left_valid = 1;
|
||||
sr.lines.right_valid = 1;
|
||||
}
|
||||
}
|
||||
|
||||
void element_init()
|
||||
{
|
||||
std::memset(&g_elm, 0, sizeof(g_elm));
|
||||
g_elm.state = TRACK_STRAIGHT;
|
||||
g_elm.last_state = TRACK_STRAIGHT;
|
||||
g_elm.normal_width = 50.0f;
|
||||
}
|
||||
|
||||
bool element_is_cross()
|
||||
{
|
||||
return g_elm.state == TRACK_CROSS;
|
||||
}
|
||||
|
||||
void element_recognize(SearchResult& sr, TrackInfo& info)
|
||||
{
|
||||
calibrate_width(sr);
|
||||
|
||||
g_elm.last_state = g_elm.state;
|
||||
++g_elm.in_state_frames;
|
||||
|
||||
if (g_elm.lock_cnt > 0)
|
||||
{
|
||||
--g_elm.lock_cnt;
|
||||
info.scene = g_elm.state;
|
||||
return;
|
||||
}
|
||||
|
||||
switch (g_elm.state)
|
||||
{
|
||||
case TRACK_STRAIGHT:
|
||||
case TRACK_GENTLE_CURVE:
|
||||
if (is_cross_entry(sr))
|
||||
{
|
||||
int bottom_y = IMAGE_HEIGHT - 1;
|
||||
g_elm.entry_deviation = (sr.lines.mid[bottom_y] > 0)
|
||||
? (sr.lines.mid[bottom_y] - IMAGE_WIDTH / 2) / static_cast<float>(IMAGE_WIDTH / 2)
|
||||
: 0.0f;
|
||||
|
||||
g_elm.state = TRACK_CROSS;
|
||||
g_elm.lock_cnt = k_lock_min_frames;
|
||||
g_elm.in_state_frames = 0;
|
||||
g_elm.confirm_cnt = 0;
|
||||
g_elm.exit_cnt = 0;
|
||||
info.scene = TRACK_CROSS;
|
||||
}
|
||||
else
|
||||
{
|
||||
info.scene = sr.lines.left_valid || sr.lines.right_valid
|
||||
? TRACK_STRAIGHT : TRACK_LOST_LINE;
|
||||
}
|
||||
info.line_valid = (info.scene != TRACK_LOST_LINE);
|
||||
break;
|
||||
|
||||
case TRACK_CROSS:
|
||||
cross_patch_midline(sr);
|
||||
|
||||
if (is_cross_exit(sr))
|
||||
{
|
||||
g_elm.state = TRACK_STRAIGHT;
|
||||
g_elm.lock_cnt = 0;
|
||||
g_elm.in_state_frames = 0;
|
||||
g_elm.confirm_cnt = 0;
|
||||
g_elm.exit_cnt = 0;
|
||||
info.scene = TRACK_STRAIGHT;
|
||||
}
|
||||
else
|
||||
{
|
||||
info.scene = TRACK_CROSS;
|
||||
}
|
||||
info.line_valid = true;
|
||||
break;
|
||||
|
||||
case TRACK_LOST_LINE:
|
||||
g_elm.confirm_cnt = 0;
|
||||
if (sr.lines.left_valid && sr.lines.right_valid)
|
||||
{
|
||||
g_elm.state = TRACK_STRAIGHT;
|
||||
info.scene = TRACK_STRAIGHT;
|
||||
}
|
||||
else
|
||||
{
|
||||
info.scene = TRACK_LOST_LINE;
|
||||
}
|
||||
info.line_valid = (info.scene != TRACK_LOST_LINE);
|
||||
break;
|
||||
|
||||
default:
|
||||
info.scene = TRACK_STRAIGHT;
|
||||
info.line_valid = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user