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

更新

Browse Source
This commit is contained in:
Glz
2024-06-18 16:21:19 +08:00
parent 3605e98cec
commit 57e132c535
8 changed files with 85 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
.eide/eide.json
View File

@@ -65,10 +65,9 @@
"libList": [
"libraries/zf_device"
],
"sourceDirList": [],
"defineList": []
}
}
},
"version": "3.3"
"version": "3.4"
}

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 大小减一
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)

83
app/gl_tracking.c
View File

@@ -1,12 +1,15 @@
#include "zf_common_headfile.h"
#include "gl_headfile.h"
#include "jj_blueteeth.h"
#include <float.h>
float (*mid_track)[2];
int32_t mid_track_count;
float pure_angle;
float pure_angle_half;
float curvature;
float dx_near;
float curvature;
float last_curvature;
float (*rpts)[2];
int rpts_num;
float last_pure_angle = 0.0f;
@@ -42,6 +45,39 @@ float leastSquaresSlope(float points[][2], int n)
return temp;
}
// 计算曲率的函数
// 计算曲率的函数
float calculate_curvature(float x[], float y[], int n)
{
float total_curvature = 0.0;
for (int i = 1; i < n - 1; i++) {
float x1 = x[i - 1], y1 = y[i - 1];
float x2 = x[i], y2 = y[i];
float x3 = x[i + 1], y3 = y[i + 1];
float dx1 = x2 - x1;
float dy1 = y2 - y1;
float dx2 = x3 - x2;
float dy2 = y3 - y2;
float dx = (dx1 + dx2) / 2;
float dy = (dy1 + dy2) / 2;
float ddx = x3 - 2 * x2 + x1;
float ddy = y3 - 2 * y2 + y1;
float numerator = dx * ddy - dy * ddx;
float denominator = powf(dx * dx + dy * dy, 1.5f);
if (fabs(denominator) > FLT_EPSILON) {
total_curvature += numerator / denominator;
}
}
return total_curvature / (float)(n - 2);
}
float calculateX(float a_x, float a_y, float slope, float b_y)
{
float b_x = a_x - (b_y - a_y) * slope;
@@ -117,7 +153,7 @@ void ElementRun()
} else if (barrier_type != BARRIER_NONE) {
RunBarrier();
} else if (s_type != S_NONE) {
RunS();
// RunS();
}
}
@@ -141,15 +177,29 @@ void MidLineTrack()
mid_track = mid_right;
mid_track_count = mid_right_count;
}
float x[mid_track_count / 2];
float y[mid_track_count / 2];
for (int i = 0; i < mid_track_count / 2; i++) {
x[i] = mid_track[i][1];
y[i] = mid_track[i][0];
}
if (mid_track_count <= 4) {
curvature = last_curvature;
/* code */
} else {
curvature = calculate_curvature(x, y, mid_track_count / 2);
}
last_curvature = curvature;
}
// 车轮对应点 (纯跟踪起始点)
float cx = InverseMapW[(int)(IMAGE_H * 0.8f)][70];
float cy = InverseMapH[(int)(IMAGE_H * 0.8f)][70];
int neary = mid_track[0][0];
int nearx = mid_track[0][1];
int neary = (int)mid_track[0][0];
int nearx = (int)mid_track[0][1];
// 找最近点 (起始点中线归一化)
float min_dist = 1e10;
@@ -172,6 +222,23 @@ void MidLineTrack()
rptsn_num = sizeof(rptsn) / sizeof(rptsn[0]);
GetLinesResample(mid_track + begin_id, mid_track_count - begin_id, rptsn, &rptsn_num, RESAMPLEDIST * PIXPERMETER);
if (cross_type == CROSS_IN) {
float x[rptsn_num / 2];
float y[rptsn_num / 2];
for (int i = 0; i < rptsn_num / 2; i++) {
x[i] = rptsn[i][1];
y[i] = rptsn[i][0];
}
if (rptsn_num <= 4) {
curvature = last_curvature;
/* code */
} else {
curvature = calculate_curvature(x, y, rptsn_num / 2);
}
last_curvature = curvature;
}
// 远预锚点位置-
int aim_idx = clip(round(aim_distance / RESAMPLEDIST), 0, rptsn_num - 1);
int aim_idx_judge = clip(round(aim_judge_far / RESAMPLEDIST), 0, mid_track_count - 1);
@@ -213,20 +280,18 @@ void MidLineTrack()
if (dy > 0) {
pure_angle = -atanf(dx / dy) / PI32 * 180.0f;
last_pure_angle = pure_angle;
//last_pure_angle_half = pure_angle_half;
// last_pure_angle_half = pure_angle_half;
} else {
pure_angle = last_pure_angle;
//pure_angle_half = last_pure_angle_half;
// 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;
// // float dn_near = Q_sqrt(dx_near * dx_near + dy_near * dy_near);
// // float error_near = -atan2f(dx_near, dy_near) * 180 / PI32;
}
if (circle_type == CIRCLE_LEFT_IN || circle_type == CIRCLE_LEFT_OUT || circle_type == CIRCLE_RIGHT_IN || circle_type == CIRCLE_RIGHT_OUT || circle_type == CIRCLE_LEFT_RUNNING || circle_type == CIRCLE_RIGHT_RUNNING || circle_type == CIRCLE_LEFT_BEGIN || circle_type == CIRCLE_RIGHT_BEGIN) {
state_type = CIRCLE_STATE;
@@ -244,7 +309,7 @@ void MidLineTrack()
// aim_distance = common_aim;
// }
if (last_state == STRAIGHT_STATE && (state_type == TURN_STATE || state_type == CIRCLE_STATE)) {
if (last_state == STRAIGHT_STATE && (state_type == TURN_STATE || state_type == CIRCLE_STATE)) {
turn_flag = 1;
timer_clear(TIM_3);
timer_start(TIM_3);

1
app/gl_tracking.h
View File

@@ -6,6 +6,7 @@ extern int32_t mid_track_count;
extern float pure_angle;
extern float pure_angle_half;
extern float dx_near;
extern float curvature;
extern int8_t turn_flag ;
extern float curvature;

2
app/jj_param.c
View File

@@ -24,7 +24,7 @@ void jj_param_eeprom_init(void)
PARAM_REG(aim_mid, &mid_aim, EFLOAT, 1, "mid:");
PARAM_REG(offset_barrier, &barrier_offset, EFLOAT, 1, "offs:");
PARAM_REG(delta_x, &pure_angle, EFLOAT, 2, "far:");
PARAM_REG(delta_y, &dx_near, EFLOAT, 2, "near:");
PARAM_REG(delta_y, &curvature, EFLOAT, 2, "cur:");
PARAM_REG(aim_straight, &straight_aim, EFLOAT, 1, "str:");
PARAM_REG(aim_turn, &turn_aim, EFLOAT, 1, "tur:");
PARAM_REG(baoguang, &FIX_BINTHRESHOLD, EFLOAT, 1, "bao:");

4
app/main.c
View File

@@ -57,7 +57,7 @@ int main(void)
Page_Run();
by_buzzer_run();
tiny_frame_param[0].f32 = pure_angle;
tiny_frame_param[1].f32 = pure_angle_half;
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) {
@@ -73,7 +73,7 @@ int main(void)
ElementJudge();
ElementRun();
MidLineTrack();
//bt_printf("type= %d", circle_type);
//bt_printf("type= %d", (int)curvature);
}
}
}

1
app/page/page_dparam.c
View File

@@ -55,6 +55,7 @@ static void Loop()
ips200_show_int(90, 18 + 74, cross_type, 1);
ips200_show_int(90, 18 + 92, circle_type, 1);
ips200_show_int(90, 18 + 110, s_type, 1);
ips200_show_float(90, 18 + 128, curvature, 4, 5);
}
/**
* @brief 页面事件

4
app/page/page_ui_widget.c
View File

@@ -171,10 +171,10 @@ void Show_Marked_Image(void)
uint16_t mix_y = START_Y + (uint16_t)(rptsn[i][0] * vertical_zoom_rate);
// 近线数组显示
if (i < pts_inv_l_count) {
if (i < pts_resample_left_count) {
ips200_draw_rect(l_x, l_y, l_x + 2, l_y + 1, RGB565_RED);
}
if (i < pts_inv_r_count) {
if (i < pts_resample_right_count) {
ips200_draw_rect(r_x - 2, r_y, r_x, r_y + 1, RGB565_BLUE);
}