btl143/QDAC-firmware
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'master' of https://git.brisky.space/btl143/QDAC-firmware

Browse Source
This commit is contained in:
Glz
2024-07-03 16:37:57 +08:00
parent 770247f9b6 61a92c8620
commit 7facae7a1a
4 changed files with 97 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files

90
app/gl_barrier.c
View File

@@ -2,7 +2,7 @@
#include "gl_headfile.h"
enum barrier_type_e barrier_type = BARRIER_NONE;
enum s_type_e s_type = S_NONE;
enum s_type_e s_type = S_NONE;
uint16 time_barrier;
float (*mid_track_s)[2];
int32_t mid_track_count_s;
@@ -24,32 +24,26 @@ void RunBarrier()
{
if (barrier_type == BARRIER_LEFT_BEGIN) {
track_type = TRACK_RIGHT;
if (barrier_begin_flag == 0)
{
if (barrier_begin_flag == 0) {
timer_clear(TIM_3);
timer_start(TIM_3);
barrier_begin_flag = 1;
}
if (barrier_begin_flag == 1)
{
}
if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING) {
if (barrier_begin_flag == 1) {
barrier_type = BARRIER_LEFT_BEGIN;
time_barrier = timer_get(TIM_3);
if (time_barrier >= 1000 && time_barrier < 2000)
{
if (time_barrier >= 300 && time_barrier < 600) {
barrier_type = BARRIER_LEFT_RUNNING;
} else if (time_barrier >= 600) {
barrier_type = BARRIER_NONE;
barrier_begin_flag = 0;
timer_stop(TIM_3);
timer_clear(TIM_3);
}
else if (time_barrier >= 1500)
{
barrier_type = BARRIER_NONE;
barrier_begin_flag = 0;
timer_stop(TIM_3);
timer_clear(TIM_3);
}
}
}
// if (Lpt0_found) {
// Lpt0_found_count++;
@@ -70,35 +64,29 @@ void RunBarrier()
// timer_start(TIM_3);
// timer_clear(TIM_3);
// }
}
if (barrier_type == BARRIER_RIGHT_BEGIN) {
track_type = TRACK_LEFT;
if (barrier_begin_flag == 0)
{
if (barrier_begin_flag == 0) {
timer_clear(TIM_3);
timer_start(TIM_3);
barrier_begin_flag = 1;
}
if (barrier_begin_flag == 1)
{
}
if (barrier_type == BARRIER_RIGHT_BEGIN || barrier_type == BARRIER_RIGHT_RUNNING) {
if (barrier_begin_flag == 1) {
barrier_type = BARRIER_RIGHT_BEGIN;
time_barrier = timer_get(TIM_3);
if (time_barrier >= 1000)
{
if (time_barrier >= 300 && time_barrier < 600) {
barrier_type = BARRIER_RIGHT_RUNNING;
if (time_barrier >= 2000)
{
barrier_type = BARRIER_NONE;
barrier_begin_flag = 0;
timer_stop(TIM_3);
timer_clear(TIM_3);
}
} else if (time_barrier >= 600) {
barrier_type = BARRIER_NONE;
barrier_begin_flag = 0;
timer_stop(TIM_3);
timer_clear(TIM_3);
}
}
}
// if (Lpt1_found) {
// Lpt1_found_count++;
@@ -119,11 +107,10 @@ void RunBarrier()
// barrier_type = BARRIER_NONE;
// track_type = TRACK_LEFT;
// }
}
}
void Check_s(){
void Check_s()
{
if (track_type == TRACK_LEFT) {
mid_track_s = mid_left;
mid_track_count_s = mid_left_count;
@@ -135,35 +122,26 @@ void Check_s(){
int min_s = 1000;
int max_s = 1;
for (int i = 0; i < mid_track_count_s; i++)
{
if (mid_track_s[i][1] < min_s)
{
min_s = mid_track_s[i][1];
for (int i = 0; i < mid_track_count_s; i++) {
if (mid_track_s[i][1] < min_s) {
min_s = mid_track_s[i][1];
temp_min_s = min_s;
}
if (mid_track_s[i][1] > max_s)
{
max_s = mid_track_s[i][1];
if (mid_track_s[i][1] > max_s) {
max_s = mid_track_s[i][1];
temp_max_s = max_s;
}
}
if (is_straight0 == false && is_straight1 == false && (max_s - min_s) <= 30 && (max_s - min_s) >= 5)
{
if (is_straight0 == false && is_straight1 == false && (max_s - min_s) <= 30 && (max_s - min_s) >= 5) {
s_type = S_BEGIN;
}
}
void RunS()
{
void RunS(){
if (temp_max_s - temp_min_s >30 || (is_straight0 && is_straight1))
{
if (temp_max_s - temp_min_s > 30 || (is_straight0 && is_straight1)) {
s_type = S_NONE;
}
}

4
app/gl_get_corners.c
View File

@@ -59,7 +59,7 @@ void get_corners()
float conf = fabs(angle_left_barrier[i]) - (fabs(angle_left_barrier[im1]) + fabs(angle_left_barrier[ip1])) / 2;
// L 角点阈值
if (Lpt0_found_barrier == false && (66. / 180. * PI32) < conf && conf < (140. / 180. * PI32) && i < 0.5 / RESAMPLEDIST) {
if (Lpt0_found_barrier == false && (40. / 180. * PI32) < conf && conf < (160. / 180. * PI32) && i < 0.6 / RESAMPLEDIST) {
Lpt0_rpts0s_id_barrier = i;
if (pts_resample_left[i][1] <= pts_resample_left[clip(i + 2, 0, pts_resample_left_count - 1)][1])
{
@@ -127,7 +127,7 @@ void get_corners()
int ip1 = clip(i + (int)round(ANGLEDIST_barrier / RESAMPLEDIST), 0, pts_resample_right_count - 1);
float conf = fabs(angle_right_barrier[i]) - (fabs(angle_right_barrier[im1]) + fabs(angle_right_barrier[ip1])) / 2;
if (Lpt1_found_barrier == false && (66. / 180. * PI32) < conf && conf < 140. / 180. * PI32 && i < 0.5 / RESAMPLEDIST) {
if (Lpt1_found_barrier == false && (40. / 180. * PI32) < conf && conf < 160. / 180. * PI32 && i < 0.6 / RESAMPLEDIST) {
Lpt1_rpts1s_id_barrier = i;
if (pts_resample_right[i][1] >= pts_resample_right[clip(i + 2, 0, pts_resample_left_count - 1)][1])

85
app/gl_tracking.c
View File

@@ -14,7 +14,7 @@ float (*rpts)[2];
int rpts_num;
float last_pure_angle = 0.0f;
int8_t turn_flag = 0;
int circle_flag = 1;
int circle_flag = 1;
// 计算最小二乘法斜率的函数
float leastSquaresSlope(float points[][2], int n)
{
@@ -270,31 +270,55 @@ void MidLineTrack()
float dy = cy - rptsn[aim_idx][0]; // + 0.2f * PIXPERMETER;
float dn = (dx * dx + dy * dy);
float temp_near = 0;
if (barrier_type == BARRIER_LEFT_BEGIN) {
//dx_near = mid_track[aim_idx_near][1] - cx + barrier_offset;
pure_angle = -25.f;
}else if (barrier_type == BARRIER_LEFT_RUNNING)
{
pure_angle = 25.f;
}else if (barrier_type == BARRIER_RIGHT_BEGIN)
{
pure_angle = 25.f;
}
else if (barrier_type == BARRIER_RIGHT_RUNNING) {
//dx_near = mid_track[aim_idx_near][1] - cx - barrier_offset;
pure_angle = -25.f;
} else {
// pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f;
if (dy > 0) {
pure_angle = -atanf(dx / dy) / PI32 * 180.0f;
last_pure_angle = pure_angle;
// last_pure_angle_half = pure_angle_half;
} else {
pure_angle = last_pure_angle;
// pure_angle_half = last_pure_angle_half;
}
switch (barrier_type) {
case BARRIER_LEFT_BEGIN:
// dx_near = mid_track[aim_idx_near][1] - cx + barrier_offset;
pure_angle = -45.f;
break;
case BARRIER_LEFT_RUNNING:
pure_angle = 25.f;
break;
case BARRIER_RIGHT_BEGIN:
pure_angle = 45.f;
break;
case BARRIER_RIGHT_RUNNING:
// dx_near = mid_track[aim_idx_near][1] - cx - barrier_offset;
pure_angle = -25.f;
default:
// pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f;
if (dy > 0) {
pure_angle = -atanf(dx / dy) / PI32 * 180.0f;
last_pure_angle = pure_angle;
// last_pure_angle_half = pure_angle_half;
} else {
pure_angle = last_pure_angle;
// pure_angle_half = last_pure_angle_half;
}
}
// if (barrier_type == BARRIER_LEFT_BEGIN) {
// // dx_near = mid_track[aim_idx_near][1] - cx + barrier_offset;
// pure_angle = -25.f;
// } else if (barrier_type == BARRIER_LEFT_RUNNING) {
// pure_angle = 25.f;
// } else if (barrier_type == BARRIER_RIGHT_BEGIN) {
// pure_angle = 25.f;
// } else if (barrier_type == BARRIER_RIGHT_RUNNING) {
// // dx_near = mid_track[aim_idx_near][1] - cx - barrier_offset;
// pure_angle = -25.f;
// } else {
// // pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f;
// if (dy > 0) {
// pure_angle = -atanf(dx / dy) / PI32 * 180.0f;
// last_pure_angle = pure_angle;
// // last_pure_angle_half = pure_angle_half;
// } else {
// pure_angle = last_pure_angle;
// // pure_angle_half = last_pure_angle_half;
// }
// }
// // 计算近锚点偏差值
// dx_near = rptsn[aim_idx_near][1] - cx;
// // float dy_near = cy - rptsn[aim_idx_near][0] + 0.2 * PIXPERMETER;
@@ -308,8 +332,7 @@ void MidLineTrack()
timer_start(TIM_3);
}
if (circle_type == CIRCLE_LEFT_RUNNING || circle_type == CIRCLE_RIGHT_RUNNING || circle_type == CIRCLE_RIGHT_OUT || circle_type == CIRCLE_LEFT_OUT || circle_type == CIRCLE_LEFT_END || circle_type == CIRCLE_RIGHT_END) {
if (circle_flag == 1)
{
if (circle_flag == 1) {
state_type = CIRCLE_STATE;
}
@@ -320,21 +343,15 @@ void MidLineTrack()
circle_flag = 0;
}
if (circle_flag == 0)
{
if (circle_flag == 0) {
state_type = CIRCLE_RUNNING_STATE;
}
}
if (circle_type == CIRCLE_NONE)
{
if (circle_type == CIRCLE_NONE) {
circle_flag = 1;
}
if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING || barrier_type == BARRIER_RIGHT_BEGIN || barrier_type == BARRIER_RIGHT_RUNNING) {
state_type = BARRIER_STATE;
}

12
app/main.c
View File

@@ -56,10 +56,10 @@ int main(void)
while (1) {
Page_Run();
by_buzzer_run();
tiny_frame_param[0].f32 = pure_angle;
tiny_frame_param[1].f32 = curvature;
tiny_frame_param[2].u8[0] = (uint8_t)state_type; // 0: 无状态 1: 弯道 2: 直行
tiny_frame_param[2].u8[1] = (uint8_t)garage_type; // 0: 无车库 1: 有车库
tiny_frame_param[0].f32 = pure_angle;
tiny_frame_param[1].f32 = curvature;
tiny_frame_param[2].u8[0] = (uint8_t)state_type; // 0: 无状态 1: 弯道 2: 直行
tiny_frame_param[2].u8[1] = (uint8_t)garage_type; // 0: 无车库 1: 有车库
if (mt9v03x_finish_flag) {
// 该操作消耗大概 1970 个 tick，折合约 110us
memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t)));
@@ -73,7 +73,9 @@ int main(void)
ElementRun();
aim_distance_select();
MidLineTrack();
//bt_printf("type= %d", (int)curvature);
// if (barrier_type) {
// bt_printf("type= %d", barrier_type);
// }
}
}
}