From 76859aa4b23ea8ebd90bd7553fd70e144bdc96ba Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期五, 15 五月 2020 15:53:20 +0800
Subject: [PATCH] 坐标

---
 lib/src/main/cpp/test_items2/road_exam.cpp |  276 +++++++++++++++++++++++++++++++++++++++++++++++-------
 1 files changed, 237 insertions(+), 39 deletions(-)

diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index f712b10..a682012 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -89,7 +89,8 @@
     int road;
     int separate;
     int lane;
-} CarOnLane;
+} CurrentLane;
+static bool laneChanging;
 
 static const int MAX_ENGINE_RPM = 2500;
 static const double START_CAR_MOVE_DISTANCE = 10.0;
@@ -100,6 +101,8 @@
 static const uint32_t CHANGE_ROAD_MIN_INTERVAL = D_SEC(10);
 static const uint32_t CRASH_DOTTED_LINE_TIMEOUT = D_SEC(10);
 static const uint32_t TURN_SIGNAL_LAMP_ADVANCE = D_SEC(3);
+
+static const double NEXT_ROAD_TIP = 100.0;                  // 鍒拌揪璺彛鍓嶆彁绀轰笅涓�涓�庝箞璧�
 
 static const int CRL_NONE = 0;
 static const int CRL_SEP_DOTTED = 1;
@@ -127,6 +130,7 @@
 static int CrashRoadLine(road_t &road, const car_model *car);
 static bool LaneIsSame(struct car_on_lane lane1, struct car_on_lane lane2);
 static bool LaneIsValid(struct car_on_lane lane);
+static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList);
 
 void InitRoadExam(road_exam_map &RoadMap)
 {
@@ -167,7 +171,8 @@
 
     currCarOnRoadIndex = FIND_POSITION;
 
-    CarOnLane.road = CarOnLane.separate = CarOnLane.lane = -1;
+    CurrentLane.road = CurrentLane.separate = CurrentLane.lane = -1;
+    laneChanging = false;
 
     InitThroughSomething(RoadMap);
 }
@@ -395,6 +400,16 @@
             reportStopCarOnRedArea = false;
 
             DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
+
+            if (slideNormalDistance) {
+                // 鍚庢粦锛屾墸10鍒�
+                AddExamFault(18, rtkTime);
+                DEBUG("鍚庢粦瓒呰繃10鍘樼背, 浣嗕笉瓒呰繃30鍘樼背");
+            }
+
+            slideNormalDistance = false;
+            slideLongDistance = false;
+            occurSlide = false;
         } else if (moveDirect == -1 && prevMoveDirect == 0) {
             DEBUG("寮�濮嬪悗婊�");
             stopPoint = car->basePoint;
@@ -402,6 +417,7 @@
         }
         prevMoveDirect = moveDirect;
     } else if (moveDirect == 0) {
+        // 鎸佺画鍋滆溅
         uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
 
         /*if (tp - stopTimepoint >= STOP_CAR_TIME && !reportStopCarOnRedArea && CrashRedArea(RoadMapList, car)) {
@@ -411,6 +427,7 @@
             reportStopCarOnRedArea = true;
         }*/
     } else if (moveDirect == -1) {
+        // 鎸佺画鍚庢粦
         if (occurSlide) {
             double slideDistance = DistanceOf(stopPoint, car->basePoint);
 
@@ -423,18 +440,32 @@
                 AddExamFault(5, rtkTime);
                 DEBUG("鍚庢粦瓒呰繃30鍘樼背");
                 slideLongDistance = true;
+                slideNormalDistance = false;
+                occurSlide = false;
             }
         }
     } else {
-        if (slideNormalDistance) {
-            // 鍚庢粦锛屾墸10鍒�
-            AddExamFault(18, rtkTime);
-            DEBUG("鍚庢粦瓒呰繃10鍘樼背, 浣嗕笉瓒呰繃30鍘樼背");
-        }
+        // 鍓嶈繘
 
-        slideNormalDistance = false;
-        slideLongDistance = false;
-        occurSlide = false;
+    }
+
+    // 杞悜鐏鍙�
+    switch (ReadCarStatus(TURN_SIGNAL_LAMP)) {
+        case LEFT_TURN_LIGHT:
+            if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
+                currTurnSignalStatus = LEFT_TURN_LIGHT;
+                Rtk2DriveTimer(turnSignalChangeTime, rtkTime);
+            }
+            break;
+        case RIGHT_TURN_LIGHT:
+            if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
+                currTurnSignalStatus = RIGHT_TURN_LIGHT;
+                Rtk2DriveTimer(turnSignalChangeTime, rtkTime);
+            }
+            break;
+        default:
+            currTurnSignalStatus = ReadCarStatus(TURN_SIGNAL_LAMP);
+            break;
     }
 
     // 妫�娴嬮�氳繃璺彛銆佷汉琛岄亾绛夊尯鍩熸椂锛岄噴鏀惧埞杞︽垨鍑忛��
@@ -563,32 +594,174 @@
         }
     }
 
+    // 妫�娴嬪帇绾跨姸鎬�
+    bool crashRedLineNow = false;
+    bool crashGreenLineNow = false;
+
     if (currExamMapIndex >= 0) {
         int crl = CrashRoadLine(RoadMap.roads[currExamMapIndex], car);
 
         if (crl == CRL_NONE) {
-            DEBUG("浠�涔堥兘娌″帇");
+//            DEBUG("浠�涔堥兘娌″帇");
         } else if (crl == CRL_SEP_DOTTED) {
-            DEBUG("鍘嬪垎閬撹櫄绾�");
+//            DEBUG("鍘嬪垎閬撹櫄绾�");
+            crashGreenLineNow = true;
         } else if (crl == CRL_SEP_SOLID) {
-            DEBUG("鍘嬪垎閬撳疄绾�");
+//            DEBUG("鍘嬪垎閬撳疄绾�");
+            crashRedLineNow = true;
         } else if (crl == CRL_EDGE_DOTTED) {
-            DEBUG("鍘嬭竟娌胯櫄绾�");
+//            DEBUG("鍘嬭竟娌胯櫄绾�");
+            crashGreenLineNow = true;
         } else if (crl == CRL_EDGE_SOLID) {
-            DEBUG("鍘嬭竟娌垮疄绾�");
+//            DEBUG("鍘嬭竟娌垮疄绾�");
+            crashRedLineNow = true;
         }
 
-        if (crl != CRL_SEP_DOTTED || crl != CRL_SEP_SOLID) {
-            struct car_on_lane lane;
-            UpdateLane(lane, RoadMap.roads[currExamMapIndex], car);
-            if (!LaneIsSame(lane, CarOnLane)) {
-                if (LaneIsValid(CarOnLane)) {
-                    // 杞﹂亾鍙樻崲
-                    DEBUG("鍙樻洿杞﹂亾");
+        struct car_on_lane lane;
+        if (UpdateLane(lane, RoadMap.roads[currExamMapIndex], car)) {
+
+            if (lane.road == CurrentLane.road && lane.separate == CurrentLane.separate &&
+                lane.lane == CurrentLane.lane) {
+                laneChanging = false;
+            }
+
+            if (lane.road == CurrentLane.road && lane.separate == CurrentLane.separate &&
+                lane.lane != CurrentLane.lane) {
+                if (crl == CRL_SEP_DOTTED) {
+                    // 鍙橀亾杩涜涓�
+                    if (!laneChanging) {
+                        laneChanging = true;
+                        DEBUG("鍙戠敓鍙橀亾");
+                        // 姣旇緝涓婃璺ㄧ嚎鏃堕棿
+                        if (crashGreenCmpTime.hour >= 0) {
+                            uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
+                                                        rtkTime->mss * 10,
+                                                        crashGreenCmpTime.hour,
+                                                        crashGreenCmpTime.min,
+                                                        crashGreenCmpTime.sec,
+                                                        crashGreenCmpTime.msec * 10);
+
+                            if (diff < CHANGE_ROAD_MIN_INTERVAL) {
+                                DEBUG("===================== 杩炵画鍙橀亾 ============!!");
+                                // 杩炵画鍙橀亾锛屼笉鍚堟牸
+                                AddExamFault(15, rtkTime);
+                            }
+                        }
+
+                        // 妫�鏌ュ彉閬撳墠锛屾槸鍚︽彁鍓嶈浆鍚戠伅
+                        if (lane.lane < CurrentLane.lane) {
+                            // 鍚戝乏渚у彉閬�
+                            DEBUG("鍚戝乏渚у彉閬�");
+                            if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
+                                DEBUG("鍙樿皟鏈墦鐏�!!");
+                                // 娌℃墦鐏紝涓嶅悎鏍�
+                                ReportTurnSignalError(13, rtkTime);
+                            } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
+                                                   rtkTime->mss * 10,
+                                                   turnSignalChangeTime.hour,
+                                                   turnSignalChangeTime.min,
+                                                   turnSignalChangeTime.sec,
+                                                   turnSignalChangeTime.msec * 10) <
+                                       TURN_SIGNAL_LAMP_ADVANCE) {
+                                if (!reportTurnSignalError) {
+                                    DEBUG("杞悜鐏椂闂翠笉瓒�");
+                                    // 涓嶈冻3绉掞紝涓嶅悎鏍�
+                                    reportTurnSignalError = true;
+                                    ReportTurnSignalError(14, rtkTime);
+                                }
+                            }
+                        } else {
+                            // 鍚戝彸渚у彉閬�
+                            DEBUG("鍚戝彸渚у彉閬�");
+                            if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
+                                DEBUG("鍙樿皟鏈墦鐏�!!");
+                                // 娌℃墦鐏紝涓嶅悎鏍�
+                                ReportTurnSignalError(13, rtkTime);
+                            } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
+                                                   rtkTime->mss * 10,
+                                                   turnSignalChangeTime.hour,
+                                                   turnSignalChangeTime.min,
+                                                   turnSignalChangeTime.sec,
+                                                   turnSignalChangeTime.msec * 10) <
+                                       TURN_SIGNAL_LAMP_ADVANCE) {
+                                if (!reportTurnSignalError) {
+                                    DEBUG("杞悜鐏椂闂翠笉瓒�");
+                                    // 涓嶈冻3绉掞紝涓嶅悎鏍�
+                                    reportTurnSignalError = true;
+                                    ReportTurnSignalError(14, rtkTime);
+                                }
+                            }
+                        }
+                    }
+                } else {
+                    // 鍙橀亾瀹屾垚
+                    DEBUG("鍙橀亾瀹屾垚");
+                    CurrentLane = lane;
+                    laneChanging = false;
+
+                    // 璁板綍鏈鍙橀亾鏃堕棿鐐�
+                    Rtk2DriveTimer(crashGreenCmpTime, rtkTime);
                 }
-                CarOnLane = lane;
+            }
+
+            if (lane.road != CurrentLane.road || lane.separate != CurrentLane.separate) {
+                // 璺垨娈靛彉鏇达紝鎾ら攢鍙橀亾璺熻釜
+                DEBUG("============== 璺垨娈靛彉鏇� CURR %d, %d NEW %d, %d", CurrentLane.road,
+                      CurrentLane.separate,
+                      lane.road, lane.separate);
+                CurrentLane = lane;
+                laneChanging = false;
             }
         }
+    }
+
+    // 鎾炵孩绾�
+    if (crashRedLineNow) {
+        if (!occurCrashRedLine) {
+            // 杞﹁締琛岄┒涓獞杞ц溅閬撲腑蹇冨疄绾挎垨鑰呰溅閬撹竟缂樺疄绾匡紝涓嶅悎鏍�
+            DEBUG("鎾為亾璺竟缂樼嚎");
+            AddExamFault(11, rtkTime);
+            occurCrashRedLine = true;
+        }
+    } else {
+        occurCrashRedLine = false;
+    }
+
+    // 鍘嬭櫄绾�
+    if (crashGreenLineNow) {
+        // 鍘嬭櫄绾�
+        if (moveDirect != 0) {
+            if (checkCrashGreenTimeout == 0) {
+                checkCrashGreenTimeout = 1;
+                Rtk2DriveTimer(crashGreenRunTime, rtkTime);         // 杩愬姩涓帇铏氱嚎鐨勫紑濮嬫椂闂寸偣
+            } else if (checkCrashGreenTimeout == 1) {
+                uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
+                                            crashGreenRunTime.hour, crashGreenRunTime.min, crashGreenRunTime.sec, crashGreenRunTime.msec*10);
+
+                if (diff >= CRASH_DOTTED_LINE_TIMEOUT) {
+                    DEBUG("闀挎椂闂村帇铏氱嚎");
+                    checkCrashGreenTimeout = 2;
+                    // 闀挎椂闂撮獞杞ц溅閬撳垎鐣岀嚎琛岄┒锛屼笉鍚堟牸
+                    AddExamFault(12, rtkTime);
+                }
+            }
+        } else {
+            // 鍋滆溅灏变笉璁℃椂浜�
+            checkCrashGreenTimeout = 0;
+        }
+
+        // 妫�娴嬪綋鍓嶈溅杈嗕簬铏氱嚎鐨勪綅缃紝鍋氬彉閬撴娴�;
+        // 妫�娴嬫槸鍚�3绉掑墠鏈夊紑鍚搴斾箣杞悜鐏�
+        if (!occurCrashGreenLine) {
+            occurCrashGreenLine = true;
+            // 璁板綍寮�濮嬪帇绾跨殑鏃堕棿锛屼笉纭畾鏄惁鏈夊彉閬撴剰鍥撅紝寰呯‘璁ゅ彉閬撳悗鍐嶅鐞嗕箣
+            Rtk2DriveTimer(crashGreenStartTime, rtkTime);
+            turnSignalStatusWhenCrashGreenLine = currTurnSignalStatus;
+        }
+    } else {
+        // 涓嶅啀鍘嬭櫄绾�
+        occurCrashGreenLine = false;
+        checkCrashGreenTimeout = 0;
     }
 }
 
@@ -636,6 +809,8 @@
                 rightExt = true;
                 MakeLine(&rightExtLine, &car->carXY[car->axial[AXIAL_FRONT]], &vp);
                 goto RIGHT_EXT_CMP;
+            } else {
+                DEBUG("鍙充晶涓嶅瀭鐐� %d p1(%f,%f) p2(%f,%f) (%f,%f)", j, p1.X, p1.Y, p2.X, p2.Y, vp.X, vp.Y);
             }
             p1 = p2;
         }
@@ -643,6 +818,7 @@
 RIGHT_EXT_CMP:
 
     if (!leftExt || !rightExt) {
+        DEBUG("宸﹀彸杈圭晫涓嶅尮閰�");
         return false;
     }
 
@@ -669,20 +845,20 @@
                         orthogonal.insert(pair<int, int>(j, 1));
                         orthogonalInSegment = true;
                         intersection = true;
-                        DEBUG("鍒嗛亾绾� %d 宸︽浜�", j);
+//                        DEBUG("鍒嗛亾绾� %d 宸︽浜�", j);
                         break;
                     } else if (IntersectionOf(rightExtLine, sep) == GM_Intersection) {
                         orthogonal.insert(pair<int, int>(j, 2));
                         orthogonalInSegment = true;
                         intersection = true;
-                        DEBUG("鍒嗛亾绾� %d 鍙虫浜�", j);
+//                        DEBUG("鍒嗛亾绾� %d 鍙虫浜�", j);
                         break;
                     }
                     p1 = p2;
                 }
             }
         }
-        DEBUG("鐩爣 %d 褰撳墠 %d", road.separate[i].lines.size(), orthogonal.size());
+//        DEBUG("鐩爣 %d 褰撳墠 %d", road.separate[i].lines.size(), orthogonal.size());
 
         if (orthogonal.size() > 0) {
             if (orthogonal.size() == road.separate[i].lines.size()) {
@@ -696,7 +872,7 @@
                             lane.separate = i;
                             lane.lane = itx->first;
 
-                            DEBUG("璺� %d 娈� %d 杞﹂亾 %d", lane.road, lane.separate, lane.lane);
+//                            DEBUG("璺� %d 娈� %d 杞﹂亾 %d", lane.road, lane.separate, lane.lane);
                             break;
                         }
                     }
@@ -706,16 +882,18 @@
                     lane.separate = i;
                     lane.lane = orthogonal.size();
 
-                    DEBUG("璺� %d 娈� %d 杞﹂亾 %d", lane.road, lane.separate, lane.lane);
+//                    DEBUG("璺� %d 娈� %d 杞﹂亾 %d", lane.road, lane.separate, lane.lane);
                 }
                 out = lane;
                 return true;
             } else {
                 // 涓嶅畬鍏ㄦ浜わ紝鐩存帴閫�鍑�
             }
+            DEBUG("鍒嗛亾绾块暱鐭笉鍚岋紝閫�鍑�");
             return false;
         }
     }
+    DEBUG("娈靛尮閰嶅け璐�");
     return false;
 }
 
@@ -800,12 +978,12 @@
                             // 鍘嬪疄绾�
                             return CRL_SEP_SOLID;
                         } else if (road.separate[i].lines[j][k].character == LINE_HALF_SOLID_LEFT) {
-                            if (LaneIsValid(CarOnLane) && CarOnLane.lane <= j) {
+                            if (LaneIsValid(CurrentLane) && CurrentLane.lane <= j) {
                                 return CRL_SEP_SOLID;
                             }
                             return CRL_SEP_DOTTED;
                         } else if (road.separate[i].lines[j][k].character == LINE_HALF_SOLID_RIGHT) {
-                            if (LaneIsValid(CarOnLane) && CarOnLane.lane > j) {
+                            if (LaneIsValid(CurrentLane) && CurrentLane.lane > j) {
                                 return CRL_SEP_SOLID;
                             }
                             return CRL_SEP_DOTTED;
@@ -1164,11 +1342,11 @@
                     RoadMapList[currExamMapIndex].type <= TURN_AROUND_MAP) {
                 StartThroughExam(currExamMapIndex, RoadMapList);
             } else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) {
-                StartDriveStraightExam(currExamMapIndex, RoadMapList);
+
             } else if (RoadMapList[currExamMapIndex].type == STOP_CAR_MAP) {
-                StartStopCarExam(currExamMapIndex, RoadMapList);
+
             } else if (RoadMapList[currExamMapIndex].type == OP_GEAER_MAP) {
-                StartOperateGearExam(currExamMapIndex, RoadMapList, rtkTime);
+
             }
         }
     } else if (startCar == START_CAR_DONE) {
@@ -1179,14 +1357,11 @@
                                                   CarModelList, speed, moveDirect, rtkTime);
         }
         else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) {
-            currExamMapIndex = ExecuteDriveStraightExam(currExamMapIndex, RoadMapList, car,
-                                     CarModelList, speed, moveDirect, rtkTime);
+
         } else if (RoadMapList[currExamMapIndex].type == STOP_CAR_MAP) {
-            currExamMapIndex = ExecuteStopCarExam(currExamMapIndex, RoadMapList, car,
-                                                        CarModelList, speed, moveDirect, rtkTime);
+
         } else if (RoadMapList[currExamMapIndex].type == OP_GEAER_MAP) {
-            currExamMapIndex = ExecuteOperateGearExam(currExamMapIndex, RoadMapList, car,
-                                   CarModelList, speed, moveDirect, rtkTime);
+
         }
 
         if (currExamMapIndex == -1) {
@@ -1636,6 +1811,29 @@
     return px;
 }
 
+static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
+{
+    // 璁$畻杞﹀墠杩涜建杩瑰欢闀跨嚎
+    double yaw = YawOf(car->carXY[ car->axial[AXIAL_FRONT] ], car->carXY[ car->axial[AXIAL_REAR] ]);
+
+    PointF extPoint = PointExtend(car->carXY[ car->axial[AXIAL_FRONT] ], NEXT_ROAD_TIP, yaw);
+    Line extLine;
+
+    MakeLine(&extLine, &car->carXY[ car->axial[AXIAL_FRONT] ], &extPoint);
+
+    if (IntersectionOf(extLine, road.stopLine) == GM_Intersection &&
+        IntersectionOfLine(extPoint, road.stopLine) == -1) {
+            if (DistanceOf(extPoint, road.stopLine) > 1.0 && !road.arrivedTail) {
+                // 鎺ヨ繎璺彛鍚庯紝瑕佹鏌ヨ溅杈嗘槸鍚﹁繘鍏ラ敊璇溅閬�
+                road.arrivedTail = true;
+                if (!road.tts.empty())
+                    PlayTTS(road.tts.c_str());
+            }
+    } else if (road.arrivedTail) {
+        road.arrivedTail = false;
+    }
+}
+
 #if 0
 
 typedef struct {

--
Gitblit v1.8.0