日常更新

app/by_frame.c

@@ -7,83 +7,147 @@
 #include "lwrb.h"
 #include "crc16.h"
 
-lwrb_t lwrb_struct;
+uint8_t frame_buffer_recv[(2 * (4 + BY_FRAME_DATA_NUM * sizeof(uint32_t))) + 1];
-uint8_t lwrb_buffer[50];
+uint8_t frame_buffer_send[4 + BY_FRAME_DATA_NUM * sizeof(uint32_t)];
-uint8_t frame_buffer[100];
+uint8_t frame_parse_busy;
+lwrb_t lwrb_ctx;
 
 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);
-
-    lwrb_init(&lwrb_struct, lwrb_buffer, 50);
 }
 
-void by_frame_send(uint8_t data_num, uint32_t *data_array)
+void by_frame_send(uint32_t *data_array)
 {
-    uint16_t crc_cal = 0;
+    uint16_t crc_cal            = 0;
-    frame_buffer[0]  = BY_FRAME_HEAD_1;
+    const uint8_t data_byte_num = BY_FRAME_DATA_NUM * sizeof(uint32_t);
-    frame_buffer[1]  = BY_FRAME_HEAD_2;
 
-    memcpy(&frame_buffer[2], data_array, data_num * sizeof(uint32_t));
+    frame_buffer_send[0] = BY_FRAME_HEAD_1;
-    crc_cal = crc16_check(frame_buffer, 2 + data_num * sizeof(uint32_t));
+    frame_buffer_send[1] = BY_FRAME_HEAD_2;
 
-    frame_buffer[2 + data_num * sizeof(uint32_t)] = (uint8_t)(crc_cal >> 8);
+    // 当传入数组不足时，会发生越界情况
-    frame_buffer[3 + data_num * sizeof(uint32_t)] = (uint8_t)(crc_cal);
+    memcpy(frame_buffer_send + 2, data_array, data_byte_num);
+    crc_cal = crc16_check(frame_buffer_send, 2 + data_byte_num);
 
-    uart_write_buffer(BY_FRAME_UART_INDEX, frame_buffer, 4 + data_num * sizeof(uint32_t));
+    frame_buffer_send[2 + data_byte_num] = (uint8_t)(crc_cal >> 8);
-    system_delay_us(BY_FRAME_UART_IDLE_TIME_US);
+    frame_buffer_send[3 + data_byte_num] = (uint8_t)(crc_cal);
+
+    uart_write_buffer(BY_FRAME_UART_INDEX, frame_buffer_send, 4 + data_byte_num);
 }
 
-void by_frame_parse(uint8_t data_num, uint32_t *data_array)
+/**
+ * @brief
+ *
+ * @param data_num
+ * @param data_array
+ * @todo 将其中写死的数据长度按照宏定义给出
+ */
+void by_frame_parse(uint32_t *data_array)
 {
-    uint8_t cnt     = 0;
+    uint32_t len                = lwrb_get_full(&lwrb_ctx); // 缓冲区大小
-    uint8_t cnt_crc = 2;
+    uint8_t status              = 0;                        // 状态 0-未找到帧头 1-找到帧头 2-校验
-    uint8_t data    = 0;
+    uint16_t frame_start        = 0;                        // 帧起始位置
-    uint8_t data_array_temp[100];
+    uint8_t frame_buf[4 + BY_FRAME_DATA_NUM * sizeof(uint32_t)]    = {0};                      // 帧
-    uint16_t crc_cal = 0;
+    uint8_t buf[(4 + BY_FRAME_DATA_NUM * sizeof(uint32_t)) * 2]    = {0};                      // 用于解析的数据块
+    const uint8_t data_byte_num = BY_FRAME_DATA_NUM * sizeof(uint32_t);
 
-    if (lwrb_get_full(&lwrb_struct) >= (4 + data_num * sizeof(uint32_t))) {
+    if (len < 2 * (4 + data_byte_num)) {
-        while (lwrb_read(&lwrb_struct, &data, 1)) {
+        // 当前要求缓冲区满
-            printf("char : %0.2X\r\n", data);
+        // (x) 缓冲区内长度小于帧长度，直接返回
-            if ((0 == cnt) && (BY_FRAME_HEAD_1 == data)) {
+        // 要是每次读的时候缓冲区内就只有前一帧的尾部和后一帧的头部，岂不是很尴尬
-                cnt                = 1;
+        // 是不是应该正确解析之后再把过的部分清空？但是是异步操作，实际上缓冲区内已经是新数据了
-                data_array_temp[0] = data;
+        // 可是直接读取 fifo 的话也是异步操作
-                continue;
+        // 发的慢的话就很有可能有同步问题，导致一直解析不出来
+        // 喵的，为啥不直接丢中断里解析算了
+
+        // 目前的解决办法大概是缓冲区开两帧长的大小，然后一次性读完
+        // 读取的时候不清除，等待新帧覆盖
+        // 用 lwrb 的话就只能清除了
+        return;
+    }
+
+    // 从环形缓冲区里读取数据
+    lwrb_read(&lwrb_ctx, buf, len);
+
+    // 递归解析有效帧
+    while (1) {
+        if (0 == status) // 没找到帧头
+        {
+            // 读到最后一个元素还没找到帧头
+            if (frame_start >= len - 2) {
+                return;
             }
+            // printf("finding frame head, now frame_start %d\r\n", frame_start);
+            uint16_t temp = (buf[frame_start] | (buf[frame_start + 1] << 8));
+            frame_start++;
+            // printf("now find %02X\r\n", temp);
 
-            if ((1 == cnt) && (BY_FRAME_HEAD_2 == data)) {
+            // 递归寻找帧头，直接俩拼起来找 注意比较的时候低位在前
-                cnt                = 2;
+            if ((BY_FRAME_HEAD_2 << 8 | BY_FRAME_HEAD_1) == temp) {
-                data_array_temp[1] = data;
+                status = 1; // 找到了好耶
-                continue;
+                // printf("frame head found!!!!!!!!!\r\n");
             }
+            continue;
+        }
 
-            if ((2 <= cnt) && (cnt < 2 + data_num * sizeof(uint32_t))) {
+        // 开始读数据
-                data_array_temp[cnt] = data;
+        if (1 == status) {
-                cnt++;
+            // 剩下的数据不够组成一帧
-                continue;
+            if ((frame_start + 4 + data_byte_num - 1) > len) {
+                // printf("failed! length not enough \r\n");
+                // 解析出错，缓冲区中没有有效帧
+                return;
+            } else {
+                // 复制到帧缓冲区，减一是因为之前多加了一次
+                memcpy(frame_buf, buf + frame_start - 1, 4 + data_byte_num);
+
+                // for (uint8_t i = 0; i < 12; i++) {
+                //     printf("%02X", frame_buf[i]);
+                // }
+                // printf("\r\n");
+
+                status = 2;
             }
+            continue;
+        }
 
-            if (cnt_crc) {
+        if (2 == status) // 校验 CRC
-                crc_cal |= ((uint16_t)data << (--cnt_crc * 8));
+        {
-                continue;
+            if ((frame_buf[2 + data_byte_num] << 8 | frame_buf[2 + data_byte_num + 1]) == crc16_check(frame_buf, 2 + data_byte_num)) {
-            }
+                // 解析成功了✌
+                // printf("parsed done!!!!!!!!\r\n");
 
-            printf("GET CRC %0.4X\r\n", crc_cal);
+                // 复制数据
-            printf("CAL CRC %0.4X\r\n", crc16_check((uint8_t *)data_array_temp, 2 + data_num * sizeof(uint32_t)));
+                if (NULL != (frame_buf + 2)) {
-
+                    memcpy(data_array, frame_buf + 2, data_byte_num);
-            if (!cnt_crc) {
-                if (crc_cal == crc16_check((uint8_t *)data_array_temp, 2 + data_num * sizeof(uint32_t))) {
-                    memcpy(data_array, data_array_temp + 2, data_num * sizeof(uint32_t));
-                    printf("parsed done!\r\n");
-                    break;
                 }
+                return;
+            } else {
+                status = 0;
+                // 这样无法应对连续帧之间缺字节的的问题，但是减少了重新遍历寻找帧头的时间
+                // frame_start += (8 - 1);
+                // 从上一个帧头之后开始解析
+                frame_start += (2 - 1);
+                continue;
             }
         }
     }
+
+    return;
 }
 
 void by_frame_parse_uart_handle(uint8_t data)
 {
-    lwrb_write(&lwrb_struct, &data, 1);
+    // fifo_write_element(&frame_fifo, data);
+    lwrb_write(&lwrb_ctx, &data, 1);
+}
+
+/**
+ * @brief 定时器回调，用于接收超时判断 1ms 调用一次
+ *
+ */
+void by_frame_parse_timer_handle(void)
+{
 }

app/by_frame.h

@@ -16,11 +16,14 @@
 #define BY_FRAME_UART_INDEX        (UART_2)
 #define BY_FRAME_UART_BAUDRATE     (115200)
 
-#define BY_FRAME_UART_IDLE_TIME_US ((1000000 / BY_FRAME_UART_BAUDRATE * 8) * 4)
+#define BY_FRAME_DATA_NUM      (3)
+
+extern uint8_t frame_buffer[50];
 
 extern void by_frame_init(void);
-extern void by_frame_send(uint8_t data_num, uint32_t *data_array);
+void by_frame_send(uint32_t *data_array);
-extern void by_frame_parse(uint8_t data_num, uint32_t *data_array);
+void by_frame_parse(uint32_t *data_array);
 extern void by_frame_parse_uart_handle(uint8_t data);
 
+
 #endif

app/gl_barrier.c

@@ -20,7 +20,7 @@ void RunBarrier()
         if (Lpt0_found) {
             Lpt0_found_count++;
         }
-        if (Lpt0_found_count >= 1 && time_barrier >= 5000) {
+        if (Lpt0_found_count >= 1) {
             Lpt0_found_count = 0;
             barrier_type     = BARRIER_LEFT_RUNNING;
             track_type       = TRACK_RIGHT;
@@ -30,7 +30,7 @@ void RunBarrier()
     } else if (barrier_type == BARRIER_LEFT_RUNNING) {
         track_type   = TRACK_RIGHT;
         time_barrier = timer_get(TIM_3);
-        if (time_barrier >= 5000) {
+        if (time_barrier >= 1000) {
             barrier_type = BARRIER_NONE;
             track_type   = TRACK_RIGHT;
             timer_start(TIM_3);
@@ -53,7 +53,7 @@ void RunBarrier()
     } else if (barrier_type == BARRIER_RIGHT_RUNNING) {
         track_type   = TRACK_LEFT;
         time_barrier = timer_get(TIM_3);
-        if (time_barrier >= 5000) {
+        if (time_barrier >= 1000) {
             timer_start(TIM_3);
             timer_clear(TIM_3);
             barrier_type = BARRIER_NONE;

app/gl_circle.c

@@ -124,8 +124,8 @@ void RunCircle()
         }
     } else if (circle_type == CIRCLE_RIGHT_RUNNING) // 正常巡线，寻外圆左线
     {
-        // track_type = TRACK_RIGHT;
+        track_type = TRACK_RIGHT;
-        track_type = TRACK_LEFT; // 看看加一个如果丢线才切换
+        //track_type = TRACK_LEFT; // 看看加一个如果丢线才切换
         if (Lpt0_found)          // 外环存在拐点，可再加拐点距离判据 (左 L 点)
         {
             pts_resample_left_count = mid_left_count = Lpt0_rpts0s_id;

app/gl_data.c

@@ -103,11 +103,14 @@ bool is_straight1;
 bool is_far_straight0;
 bool is_far_straight1;
 
-bool is_turn0;
+bool is_turn0_l;
-bool is_turn1;
+bool is_turn1_l;
+bool is_turn0_r;
+bool is_turn1_r;
 
 float rptsn[PT_MAXLEN][2];
 int32_t rptsn_num;
 float aim_distance;
+float aim_judge_far=0.3f;
 
 track_type_e track_type = TRACK_RIGHT;

app/gl_data.h

@@ -114,12 +114,15 @@ extern int Lpt1_found_barrier_in_id;
 extern int32_t Lpt0_found_count;
 extern int32_t Lpt1_found_count;
 
-extern bool is_turn0;
+extern bool is_turn0_l;
-extern bool is_turn1;
+extern bool is_turn1_l;
+extern bool is_turn0_r;
+extern bool is_turn1_r;
 
 extern float rptsn[PT_MAXLEN][2];
 extern int32_t rptsn_num;
 extern float aim_distance;
+extern float aim_judge_far;
 
 extern track_type_e track_type;
 

app/gl_garage.c

@@ -40,28 +40,69 @@ float calculate_vector_angle(float x1, float y1, float x2, float y2)
 
 void CheckGarage()
 {
-    int change_num     = 0;
+    //变量标志位
-    int check_garage_h = 60;
+    int banmaxian_hangshu = 0;//斑马线行数
-    for (int check_garage_w = 50; check_garage_w < IMAGE_W - 50; check_garage_w++) {
+
-        if ((GET_PIX_1C(mt9v03x_image_copy[0], check_garage_h, check_garage_w) < FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], check_garage_h, check_garage_w + 1) >= FIX_BINTHRESHOLD) ||
+    //从下往上扫描
-            (GET_PIX_1C(mt9v03x_image_copy[0], check_garage_h, check_garage_w) >= FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], check_garage_h, check_garage_w + 1) < FIX_BINTHRESHOLD)) {
+        for (int y = BEGINH_L - 6; y >= BEGINH_L - 9; y--)
-            change_num++;
+        {
+            int banmaxian_kuandu=0;
+            //int banmaxian_hangshu=0;
+            int banmaxian_geshu=0;
+             //从右往左扫描
+            for (int x =130; x >=20; x--)
+            {
+                int baidian_heng=0;
+                //扫描到黑色，就进判断
+                if (GET_PIX_1C(mt9v03x_image_copy[0], y, x + 2) > FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], y, x + 1) > FIX_BINTHRESHOLD 
+                && GET_PIX_1C(mt9v03x_image_copy[0], y, x) < FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], y, x - 1) < FIX_BINTHRESHOLD)
+                {
+                    for (int a = x; a > x - 30; a--)//从黑色点向左侧扫描
+                    {
+                        //找到白色点
+                        if(GET_PIX_1C(mt9v03x_image_copy[0], y, a) > FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], y, a - 1) > FIX_BINTHRESHOLD)
+                        {
+                            //记录白色点的位置，跳出循环
+                            baidian_heng=a;
+                            break;
+                        }
+                    }//斑马线宽度等于黑白点的差
+                    banmaxian_kuandu=x-baidian_heng;
+ 
+ 
+                }
+                else
+                {   //斑马线的宽度在4~8之间认为它成立为斑马线黑色块
+                    if (banmaxian_kuandu >= 2 && banmaxian_kuandu <= 6)
+                    {
+                        //斑马线黑色块++
+                        banmaxian_geshu++;
+                        //斑马线色块宽度清零，进行下一个黑色块的扫描计算
+                        banmaxian_kuandu = 0;
+                    }
+                    else
+                    {
+                        //如果不满足对黑色块的认为要求就直接清零，去计算下一个黑色块
+                        banmaxian_kuandu = 0;
+                    }
+                }
+            }
+            //如果色块的个数在6~9之间则认为这一行的斑马线满足要求，在去扫下一行
+            if (banmaxian_geshu >= 6 ){banmaxian_hangshu++;}
         }
-    }
+        //如果有大于等于4行的有效斑马线
-
+        if(banmaxian_hangshu>=2)
-    if (change_num > 14) {
+        {
-        garage_type = GARAGE_FOUND;
+            //斑马线标准位置1
-        // printf("跳变点的数量为：%d\r\n", change_num);
+            garage_type = GARAGE_FOUND;
-    }
+        }
-
+        else{garage_type = GARAGE_NONE;}
-    change_num = 0;
 }
 
 void RunGarage()
 {
-
     if (garage_type == GARAGE_FOUND) {
         printf("识别到车库\r\n");
-        garage_type = GARAGE_NONE; // TFIXME 原来是 garage_type == GARAGE_NONE，确认更改后无问题
+        //garage_type = GARAGE_NONE; // TFIXME 原来是 garage_type == GARAGE_NONE，确认更改后无问题
     }
 }

app/gl_garage.h

@@ -5,15 +5,6 @@
 enum garage_type_e {
   GARAGE_NONE, // 非车库模式
   GARAGE_FOUND,
-  GARAGE_OUT_LEFT,
-  GARAGE_OUT_RIGHT, // 出库，陀螺仪转过45°，即出库完毕
-  GARAGE_FOUND_LEFT,
-  GARAGE_FOUND_RIGHT, // 发现车库，即斑马线+单侧L角点(未使用)
-  GARAGE_IN_LEFT,
-  GARAGE_IN_RIGHT, // 进库，发现车库后判断第几次，从而决定是否进库
-  GARAGE_PASS_LEFT,
-  GARAGE_PASS_RIGHT, // 不进库，发现车库后判断第几次，从而决定是否进库
-  GARAGE_STOP // 进库完毕，停车
 };
 extern enum garage_type_e garage_type;
 

app/gl_get_corners.c

@@ -2,121 +2,140 @@
 #include "math.h"
 #include "gl_headfile.h"
 
-
+void get_corners()
-
+{
-void get_corners() {
     Lpt0_found = Lpt1_found = false;
     Lpt_in0_found = Lpt_in1_found = false;
-    is_straight0 = pts_resample_left_count > 1.0 / RESAMPLEDIST;
+    is_straight0                  = pts_resample_left_count > 1.0 / RESAMPLEDIST;
-    is_straight1 = pts_resample_right_count > 1.0 / RESAMPLEDIST;
+    is_straight1                  = pts_resample_right_count > 1.0 / RESAMPLEDIST;
+    // is_turn0_l = is_turn1_l = is_turn0_r = is_turn1_r = 0;
     for (int i = 0; i < pts_resample_left_count; i++) {
         if (angle_new_left[i] == 0) continue;
-        int im1 = clip(i - (int) round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_left_count - 1);
+        int im1    = clip(i - (int)round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_left_count - 1);
-        int ip1 = clip(i + (int) round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_left_count - 1);
+        int ip1    = clip(i + (int)round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_left_count - 1);
         float conf = fabs(angle_left[i]) - (fabs(angle_left[im1]) + fabs(angle_left[ip1])) / 2;
-
+         //  114_show_float(120, 80, conf, 3, 4);
-        //L 角点阈值
+            // L 角点阈值
-        if (Lpt0_found == false && (66. / 180. * PI32) < conf && conf < (140. / 180. * PI32) && i < 0.5 / RESAMPLEDIST) {
+            if (Lpt0_found == false && (66. / 180. * PI32) < conf && conf < (140. / 180. * PI32) && i < 0.5 / RESAMPLEDIST)
+        {
             Lpt0_rpts0s_id = i;
-            Lpt0_found = true;
+            Lpt0_found     = true;
             // transform(pts_resample_left[Lpt0_rpts0s_id][1],pts_resample_left[Lpt0_rpts0s_id][0],&Lpt0[1],&Lpt0[0]);    //待优化，是否用到，无用则删
         }
-        //长直道阈值
+        // 长直道阈值
-        if (conf > (7. / 180. * PI32) && i < 0.8 / RESAMPLEDIST) is_straight0 = false;
+        if (conf > (6. / 180. * PI32) && i < 0.8 / RESAMPLEDIST) {
+            is_straight0 = false;
+        }
         if (Lpt0_found == true && is_straight0 == false) break;
     }
 
-    if (is_straight0 == false && Lpt0_found == false)
+    // if (Lpt0_found == false)
-    {
+    // {
-        is_turn0 = is_curve(angle_left ,clip(angle_left_num - 10, 0,angle_left_num),0.05);
+    //     // is_turn0 = is_curve(angle_left ,clip(angle_left_num - 10, 0,angle_left_num),0.05);
-    }
+    //     int  gap = 0;
-    if (is_turn0)
+    //     gap = pts_resample_left[clip(pts_resample_left_count - 30, 0, pts_resample_left_count - 1)][1] - pts_resample_left[0][1];
-    {
+    //     ips200_show_uint(200, 205, gap, 3);
-        state_type = TURN_LEFT_STATE;
+    //     if (gap <= -10)
-    }
+    //     {
-    
+    //         is_turn0_l = true;
-
+    //     }
+    //     if (gap >= 10)
+    //     {
+    //         is_turn1_l = true;
+    //     }
+        
+    // }
+    //if (is_turn0)
+    //{
+    // state_type = TURN_LEFT_STATE;
+    //}
 
     Lpt0_found_barrier = Lpt1_found_barrier = false;
     Lpt0_found_barrier_in = Lpt1_found_barrier_in = false;
     for (int i = 0; i < pts_resample_left_count; i++) {
         if (angle_new_left_barrier[i] == 0) continue;
-        int im1 = clip(i - (int) round(ANGLEDIST_barrier / RESAMPLEDIST), 0, pts_resample_left_count - 1);
+        int im1    = clip(i - (int)round(ANGLEDIST_barrier / RESAMPLEDIST), 0, pts_resample_left_count - 1);
-        int ip1 = clip(i + (int) round(ANGLEDIST_barrier / RESAMPLEDIST), 0, pts_resample_left_count - 1);
+        int ip1    = clip(i + (int)round(ANGLEDIST_barrier / RESAMPLEDIST), 0, pts_resample_left_count - 1);
         float conf = fabs(angle_left_barrier[i]) - (fabs(angle_left_barrier[im1]) + fabs(angle_left_barrier[ip1])) / 2;
 
-        //L 角点阈值
+        // L 角点阈值
         if (Lpt0_found_barrier == false && (66. / 180. * PI32) < conf && conf < (140. / 180. * PI32) && i < 0.5 / RESAMPLEDIST) {
             Lpt0_rpts0s_id_barrier = i;
-            Lpt0_found_barrier = true;
+            Lpt0_found_barrier     = true;
         }
     }
-    if(Lpt0_found_barrier){
+    if (Lpt0_found_barrier) {
-        float angle1 = calculate_vector_angle(pts_resample_left[Lpt0_rpts0s_id_barrier][1],pts_resample_left[Lpt0_rpts0s_id_barrier][0],pts_resample_left[Lpt0_rpts0s_id_barrier+5][1],pts_resample_left[Lpt0_rpts0s_id_barrier+5][0]);
+        float angle1 = calculate_vector_angle(pts_resample_left[Lpt0_rpts0s_id_barrier][1], pts_resample_left[Lpt0_rpts0s_id_barrier][0], pts_resample_left[Lpt0_rpts0s_id_barrier + 5][1], pts_resample_left[Lpt0_rpts0s_id_barrier + 5][0]);
-        if(angle1 < 85.)    {
+        if (angle1 < 85.) {
-            Lpt0_found_barrier_in = true;
+            Lpt0_found_barrier_in    = true;
-            Lpt0_found_barrier = false;
+            Lpt0_found_barrier       = false;
             Lpt0_found_barrier_in_id = Lpt0_rpts0s_id_barrier;
         }
     }
 
-
-
-
     for (int i = 0; i < pts_resample_right_count; i++) {
         if (angle_new_right[i] == 0) continue;
-        int im1 = clip(i - (int) round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_right_count - 1);
+        int im1    = clip(i - (int)round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_right_count - 1);
-        int ip1 = clip(i + (int) round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_right_count - 1);
+        int ip1    = clip(i + (int)round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_right_count - 1);
         float conf = fabs(angle_right[i]) - (fabs(angle_right[im1]) + fabs(angle_right[ip1])) / 2;
-
+        // ips114_show_float(100, 100, conf, 3, 4);
 
         if (Lpt1_found == false && (66. / 180. * PI32) < conf && conf < 140. / 180. * PI32 && i < 0.5 / RESAMPLEDIST) {
             Lpt1_rpts1s_id = i;
-            Lpt1_found = true;
+            Lpt1_found     = true;
             // transform(pts_resample_right[Lpt1_rpts1s_id][1],pts_resample_right[Lpt1_rpts1s_id][0],&Lpt1[1],&Lpt1[0]);
         }
 
-        if (conf > (7. / 180. * PI32) && i < 0.8 / RESAMPLEDIST) is_straight1 = false;
+        if (conf > (6. / 180. * PI32) && i < 0.8 / RESAMPLEDIST) is_straight1 = false;
         if (Lpt1_found == true && is_straight1 == false) break;
     }
 
-    if(is_straight1 && is_straight0){
+    // if(is_straight1 && is_straight0){
-        state_type = STRAIGHT_STATE;
+    //     state_type = STRAIGHT_STATE;
-    }
+    // }
 
-    if (is_straight1 == false && Lpt1_found == false)
+    // if (Lpt1_found == false)
-    {
+    // {
-        is_turn1 = is_curve(angle_right ,clip(angle_right_num - 10, 0,angle_right_num),0.05);
+    //     // is_turn1 = is_curve(angle_right ,clip(angle_right_num - 10, 0,angle_right_num),0.05);
-    }
+    //     int  gap = 0;
+    //     gap = pts_resample_right[clip(pts_resample_right_count - 30, 0, pts_resample_right_count - 1)][1] - pts_resample_right[0][1];
+    //     ips200_show_uint(200, 224, gap, 3);
+    //     if (gap <= -10)
+    //     {
+    //         is_turn1_l = true;
+    //     }
+    //     if (gap >= 10)
+    //     {
+    //         is_turn1_r = true;
+    //     }
+    // }
 
     for (int i = 0; i < pts_resample_right_count; i++) {
         if (angle_new_right_barrier[i] == 0) continue;
-        int im1 = clip(i - (int) round(ANGLEDIST_barrier / RESAMPLEDIST), 0, pts_resample_right_count - 1);
+        int im1    = clip(i - (int)round(ANGLEDIST_barrier / RESAMPLEDIST), 0, pts_resample_right_count - 1);
-        int ip1 = clip(i + (int) round(ANGLEDIST_barrier / RESAMPLEDIST), 0, pts_resample_right_count - 1);
+        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) {
             Lpt1_rpts1s_id_barrier = i;
-            Lpt1_found_barrier = true;
+            Lpt1_found_barrier     = true;
         }
     }
-    if(Lpt1_found_barrier){
+    if (Lpt1_found_barrier) {
-        float angle2 = calculate_vector_angle(pts_resample_right[Lpt1_rpts1s_id_barrier][1],pts_resample_right[Lpt1_rpts1s_id_barrier][0],pts_resample_right[Lpt1_rpts1s_id_barrier+5][1],pts_resample_right[Lpt1_rpts1s_id_barrier+5][0]);
+        float angle2 = calculate_vector_angle(pts_resample_right[Lpt1_rpts1s_id_barrier][1], pts_resample_right[Lpt1_rpts1s_id_barrier][0], pts_resample_right[Lpt1_rpts1s_id_barrier + 5][1], pts_resample_right[Lpt1_rpts1s_id_barrier + 5][0]);
-        if(angle2 > 100.)   {
+        if (angle2 > 100.) {
-            Lpt1_found_barrier_in = true;
+            Lpt1_found_barrier_in    = true;
-            Lpt1_found_barrier = false;
+            Lpt1_found_barrier       = false;
             Lpt1_found_barrier_in_id = Lpt1_rpts1s_id_barrier;
         }
     }
 
-    if (is_turn1)
+    // if (is_turn0_l && is_turn1_l)
-    {
+    // {
-        state_type = TURN_RIGHT_STATE;
+    //     state_type = TURN_LEFT_STATE; 
-    }
+    // }
-    
+    // if (is_turn0_r && is_turn1_r)
-    
+    // {
-
+    //     state_type = TURN_RIGHT_STATE;
+    // }
 }
-
-

app/gl_handle_img.c

@@ -228,8 +228,8 @@ void GetMidLine_Right(float pts_right[][2], int32_t pts_right_count, float mid_r
 
 int is_curve(float angle[], int n, float threshold) {
     for (int i = 1; i < n - 1; i++) {
-        float da = fabs(angle[i] - angle[i-1]);
+        float da = fabs(angle[i] - angle[i-3]);
-        float db = fabs(angle[i+1] - angle[i]);
+        float db = fabs(angle[i+3] - angle[i]);
         if (da > threshold && db > threshold) {
             return 1; // 是弯道
         }

app/gl_state.h

@@ -3,9 +3,9 @@
 
 enum state_type_e {
     COMMON_STATE,
-    TURN_LEFT_STATE,
+    TURN_STATE,
-    TURN_RIGHT_STATE,
     STRAIGHT_STATE,
+
 };
 
 extern enum state_type_e state_type;

app/gl_tracking.c

@@ -1,6 +1,6 @@
 #include "zf_common_headfile.h"
 #include "gl_headfile.h"
-
+#include "jj_blueteeth.h"
 float (*mid_track)[2];
 int32_t mid_track_count;
 float pure_angle;
@@ -127,18 +127,42 @@ void MidLineTrack()
 
         // 远预锚点位置-
         int aim_idx = clip(round(aim_distance / RESAMPLEDIST), 0, rptsn_num - 1);
+        int aim_idx_judge = clip(round(aim_judge_far / RESAMPLEDIST), 0, rptsn_num - 1);
 
         // 近锚点位置
         int aim_idx_near = clip(round(0.09 / RESAMPLEDIST), 0, rptsn_num - 1);
 
+        int gap_1     = fabs(rptsn[3 * (rptsn_num / 4)][1] - rptsn[0][1]);
+        float dx1     = rptsn[3 * (rptsn_num / 4)][0] - rptsn[aim_idx_judge][0];
+        float dy1     = rptsn[3 * (rptsn_num / 4)][1] - rptsn[aim_idx_judge][1];
+        float dn1     = Q_sqrt(dx1 * dx1 + dy1 * dy1);
+        float dx2     = rptsn[aim_idx_judge][0] - rptsn[aim_idx_near][0];
+        float dy2     = rptsn[aim_idx_judge][1] - rptsn[aim_idx_near][1]; 
+        float dn2     = Q_sqrt(dx2 * dx2 + dy2 * dy2);
+        float c1      = dx1 / dn1;
+        float s1      = dy1 / dn1;
+        float c2      = dx2 / dn2;
+        float s2      = dy2 / dn2;
+        float angle_1 = atan2f(c1 * s2 - c2 * s1, c2 * c1 + s2 * s1);
+        ips114_show_float(120, 80, angle_1, 3, 4);
+        if (angle_1 >= 0.3f || angle_1 <= -0.3f) {
+            state_type = TURN_STATE;
+        } else {
+            state_type = STRAIGHT_STATE;
+        }
+        // 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)
+        //{
+        //  state_type = TURN_STATE;
+        //}
+
         // 计算远锚点偏差值
         float dx = rptsn[aim_idx][1] - cx;
         float dy = cy - rptsn[aim_idx][0] + 0.2f * PIXPERMETER;
         float dn = (float)Q_sqrt(dx * dx + dy * dy);
         //  float error = -atan2f(dx, dy) * 180 / PI32;
-
+        bt_printf("%d", barrier_type);
         if (barrier_type == BARRIER_LEFT_BEGIN) {
-            dx_near    = rptsn[aim_idx_near][1] - cx +barrier_offset;
+            dx_near    = rptsn[aim_idx_near][1] - cx + barrier_offset;
             pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
         } else if (barrier_type == BARRIER_RIGHT_BEGIN) {
             dx_near    = rptsn[aim_idx_near][1] - cx - barrier_offset;
@@ -159,4 +183,12 @@ void MidLineTrack()
         // // //考虑远点
         // pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
     }
+    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) {
+        state_type = TURN_STATE;
+    }
+    if (cross_type == CROSS_BEGIN || cross_type == CROSS_IN)
+    {
+        state_type = STRAIGHT_STATE;
+    }
+    
 }

app/isr.c

@@ -279,7 +279,7 @@ void EXTI15_10_IRQHandler(void)
 void TIM1_UP_IRQHandler(void)
 {
     if (TIM_GetITStatus(TIM1, TIM_IT_Update) != RESET) {
-        by_frame_send(2, &tiny_frame_param[0].u32);
+        by_frame_send(&tiny_frame_param[0].u32);
         TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
         by_led_warn_blink();
     }

app/jj_blueteeth.c

@@ -21,7 +21,7 @@ void jj_bt_run()
 {
 }
 
-void jj_bt_printf(const char *format, ...)
+void bt_printf(const char *format, ...)
 {
     char sbuf[40];
     va_list args;

app/jj_blueteeth.h

@@ -10,5 +10,5 @@
 
 void jj_bt_init();
 void jj_bt_run();
-void jj_bt_printf(const char *format, ...);
+void bt_printf(const char *format, ...);
 #endif

app/jj_param.h

@@ -47,7 +47,7 @@ typedef union {
     uint32_t u32;
     int32_t s32;
     float f32;
-    uint8_t u8;
+    uint8_t u8[4];
 } TYPE_UNION;
 
 typedef struct {

app/main.c

@@ -58,7 +58,8 @@ int main(void)
         by_buzzer_run();
         tiny_frame_param[0].f32 = pure_angle;
         tiny_frame_param[1].f32 = dx_near;
-        system_delay_ms(10);
+        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)));

app/page/page_rtcam.c

@@ -28,8 +28,8 @@ static void Setup()
 
     ips200_show_string(5, 165, "pts_l:");
     ips200_show_string(5, 185, "pts_r:");
-    ips200_show_string(5, 205, "con_l:");
+    ips200_show_string(5, 205, "str_l:");
-    ips200_show_string(5, 224, "con_r:");
+    ips200_show_string(5, 224, "str_r:");
     ips200_show_string(100, 165, "lpt0: ");
     ips200_show_string(100, 185, "lpt1: ");
     ips200_show_string(100, 205, "lptin0: ");
@@ -53,16 +53,16 @@ static void Exit()
 static void Loop()
 {
     Show_Marked_Image();
-    ips200_show_uint(60, 165, pts_inv_l_count, 3);
+    ips200_show_uint(60, 165, pts_resample_left_count, 3);
-    ips200_show_uint(60, 185, pts_inv_r_count, 3);
+    ips200_show_uint(60, 185, pts_resample_right_count, 3);
-    ips200_show_uint(60, 205, Lpt0_found, 3);
+    ips200_show_uint(60, 205, is_straight0, 3);
-    ips200_show_uint(60, 224, Lpt1_found, 3);
+    ips200_show_uint(60, 224, is_straight1, 3);
     ips200_show_uint(160, 165, Lpt0_found_barrier, 3);
     ips200_show_uint(160, 185, Lpt1_found_barrier, 3);
     ips200_show_uint(160, 205, Lpt0_found_barrier_in, 3);
     ips200_show_uint(160, 224, Lpt1_found_barrier_in, 3);
     ips200_show_uint(200, 165, barrier_type, 3);
-    ips200_show_uint(200, 185, time_barrier, 5);
+    ips200_show_uint(200, 185, state_type, 5);
 }
 
 /**