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

日常

Browse Source
This commit is contained in:
井明江
2024-05-27 21:17:40 +08:00
parent 3fe85dc7db
commit a086ce17e4
4 changed files with 12 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
app/by_frame.c
View File

@@ -16,7 +16,7 @@ void by_frame_init(void)
{ {
lwrb_init(&lwrb_ctx, frame_buffer_recv, sizeof(frame_buffer_recv)); // lwrb 最大元素数量为 buff 大小减一 lwrb_init(&lwrb_ctx, frame_buffer_recv, sizeof(frame_buffer_recv)); // lwrb 最大元素数量为 buff 大小减一
uart_init(BY_FRAME_UART_INDEX, BY_FRAME_UART_BAUDRATE, BY_FRAME_UART_TX_PIN, BY_FRAME_UART_RX_PIN); uart_init(BY_FRAME_UART_INDEX, BY_FRAME_UART_BAUDRATE, BY_FRAME_UART_TX_PIN, BY_FRAME_UART_RX_PIN);
uart_rx_interrupt(BY_FRAME_UART_INDEX, ENABLE); // uart_rx_interrupt(BY_FRAME_UART_INDEX, ENABLE);
} }
void by_frame_send(uint32_t *data_array) void by_frame_send(uint32_t *data_array)

10
app/gl_tracking.c
View File

@@ -4,10 +4,12 @@
float (*mid_track)[2]; float (*mid_track)[2];
int32_t mid_track_count; int32_t mid_track_count;
float pure_angle; float pure_angle;
float pure_angle_half;
float dx_near; float dx_near;
float (*rpts)[2]; float (*rpts)[2];
int rpts_num; int rpts_num;
float last_pure_angle = 0.0f; float last_pure_angle = 0.0f;
float last_pure_angle_half = 0.0f;
// 计算最小二乘法斜率的函数 // 计算最小二乘法斜率的函数
float leastSquaresSlope(float points[][2], int n) float leastSquaresSlope(float points[][2], int n)
@@ -222,7 +224,7 @@ void MidLineTrack()
int aim_idx_judge = clip(round(aim_judge_far / RESAMPLEDIST), 0, mid_track_count - 1); int aim_idx_judge = clip(round(aim_judge_far / RESAMPLEDIST), 0, mid_track_count - 1);
// 近锚点位置 // 近锚点位置
int aim_idx_near = clip(round(0.09 / RESAMPLEDIST), 0, rptsn_num - 1); int aim_idx_near = clip(round(aim_distance / 2 / RESAMPLEDIST), 0, rptsn_num - 1);
float dx1 = mid_track[3 * (mid_track_count / 4)][1] - mid_track[aim_idx_judge][1]; float dx1 = mid_track[3 * (mid_track_count / 4)][1] - mid_track[aim_idx_judge][1];
float dy1 = mid_track[3 * (mid_track_count / 4)][0] - mid_track[aim_idx_judge][0]; float dy1 = mid_track[3 * (mid_track_count / 4)][0] - mid_track[aim_idx_judge][0];
@@ -245,6 +247,9 @@ void MidLineTrack()
float dx = rptsn[aim_idx][1] - cx; float dx = rptsn[aim_idx][1] - cx;
float dy = cy - rptsn[aim_idx][0]; // + 0.2f * PIXPERMETER; float dy = cy - rptsn[aim_idx][0]; // + 0.2f * PIXPERMETER;
float dn = (dx * dx + dy * dy); float dn = (dx * dx + dy * dy);
float dx_half = rptsn[aim_idx_near][1] - cx;
float dy_half = cy - rptsn[aim_idx_near][0]; // + 0.2f * PIXPERMETER;
float dn_half = (dx_half * dx_half + dy_half * dy_half);
float temp_near = 0; float temp_near = 0;
if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING) { if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING) {
dx_near = mid_track[aim_idx_near][1] - cx + barrier_offset; dx_near = mid_track[aim_idx_near][1] - cx + barrier_offset;
@@ -273,9 +278,12 @@ void MidLineTrack()
//pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f; //pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f;
if (dy > 0) { if (dy > 0) {
pure_angle = -atanf(dx / dy) / PI32 * 180.0f; pure_angle = -atanf(dx / dy) / PI32 * 180.0f;
pure_angle_half = -atanf(dx_half / dy_half) / PI32 * 180.0f;
last_pure_angle = pure_angle; last_pure_angle = pure_angle;
last_pure_angle_half = pure_angle_half;
} else { } else {
pure_angle = last_pure_angle; pure_angle = last_pure_angle;
pure_angle_half = last_pure_angle_half;
} }
} }

1
app/gl_tracking.h
View File

@@ -4,6 +4,7 @@
extern float (*mid_track)[2]; extern float (*mid_track)[2];
extern int32_t mid_track_count; extern int32_t mid_track_count;
extern float pure_angle; extern float pure_angle;
extern float pure_angle_half;
extern float dx_near; extern float dx_near;
void aim_distance_select(void); void aim_distance_select(void);

2
app/main.c
View File

@@ -57,7 +57,7 @@ int main(void)
Page_Run(); Page_Run();
by_buzzer_run(); by_buzzer_run();
tiny_frame_param[0].f32 = pure_angle; tiny_frame_param[0].f32 = pure_angle;
tiny_frame_param[1].f32 = dx_near; tiny_frame_param[1].f32 = pure_angle_half;
tiny_frame_param[2].u8[0] = (uint8_t)state_type; // 0: 无状态 1: 弯道 2: 直行 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[2].u8[1] = (uint8_t)garage_type; // 0: 无车库 1: 有车库
if (mt9v03x_finish_flag) { if (mt9v03x_finish_flag) {