From 21834035950feaadac9d4c7cad82b90ee624c984 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期一, 17 八月 2020 19:30:06 +0800
Subject: [PATCH] 传感器变更

---
 lib/src/main/cpp/driver_test.cpp                   |    1 
 lib/src/main/cpp/test_common/car_sensor.h          |   33 +++
 lib/src/main/cpp/utils/num.h                       |    1 
 lib/src/main/cpp/test_common/Geometry.cpp          |    7 
 lib/src/main/cpp/master/comm_if.h                  |   38 +++
 lib/src/main/cpp/test_items2/road_exam.h           |    2 
 lib/src/main/cpp/utils/num.cpp                     |   12 +
 lib/src/main/cpp/rtk_platform/platform.h           |    1 
 lib/src/main/cpp/master/comm_if.cpp                |   81 +++++++
 lib/src/main/cpp/test_items2/through_something.cpp |    3 
 lib/src/main/cpp/test_items2/road_exam.cpp         |  226 ++++++++++++++++++++--
 lib/src/main/cpp/test_common/Geometry.h            |    1 
 lib/src/main/cpp/test_common/car_sensor.cpp        |   69 ++++++
 lib/src/main/cpp/mcu/mcu_if.cpp                    |    7 
 lib/src/main/cpp/rtk_platform/platform.cpp         |   80 +++++++
 15 files changed, 513 insertions(+), 49 deletions(-)

diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 1fd6e1d..5c15aa1 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -31,6 +31,7 @@
 #include "test_items/stop_and_start.h"
 #include "master/comm_if.h"
 #include "utils/xconvert.h"
+#include "utils/num.h"
 #include "test_common/car_sensor.h"
 #include "mcu/mcu_if.h"
 #include "test_common/car_sensor.h"
diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp
index c4c4bb6..1166297 100644
--- a/lib/src/main/cpp/master/comm_if.cpp
+++ b/lib/src/main/cpp/master/comm_if.cpp
@@ -62,7 +62,7 @@
 #define ID_SM_LIGHT_EXAM_REQ    0x0012
 #define ID_MS_LIGHT_EXAM_RES    0x8012
 #define ID_SM_DISTANCE              0x0020
-
+#define ID_SM_CARSENSOR              0x0013
 
 #define MA_OUT_GPS_BRIEF        0x0001
 #define MA_OUT_RTK_BRIEF        0x0002
@@ -262,12 +262,12 @@
     writer.String(brief->version);
     writer.Key("selftest");
     writer.Int(brief->selftest);
-    writer.Key("gpio");
-    writer.Int(brief->gpio);
-    writer.Key("speed");
-    writer.Int(brief->speed);
-    writer.Key("engine");
-    writer.Int(brief->engine);
+//    writer.Key("gpio");
+//    writer.Int(brief->gpio);
+//    writer.Key("speed");
+//    writer.Int(brief->speed);
+//    writer.Key("engine");
+//    writer.Int(brief->engine);
     writer.Key("sn");
     writer.String(brief->sn);
 
@@ -276,6 +276,73 @@
     SendMsgToMainProcIndep(ID_SM_MCU_BRIEF, sb.GetString());
 }
 
+void MA_SendCarSensorBrief(const struct carSensorBrief *brief)
+{
+    StringBuffer sb;
+    Writer<StringBuffer> writer(sb);
+
+    writer.StartObject();
+
+    writer.Key("odo");
+    writer.Int(brief->odo);
+    writer.Key("trip");
+    writer.Int(brief->trip);
+    writer.Key("trip_time");
+    writer.Int(brief->tripTime);
+    writer.Key("cell_volt");
+    writer.Double(brief->cellVolt);
+    writer.Key("engine");
+    writer.Double(brief->engine);
+    writer.Key("sas");
+    writer.Int(brief->sas);
+    writer.Key("key");
+    writer.Int(brief->key);
+    writer.Key("gear");
+    writer.Int(brief->gear);
+    writer.Key("aps");
+    writer.Int(brief->aps);
+    writer.Key("lock");
+    writer.Int(brief->lock);
+    writer.Key("seat_belt");
+    writer.Int(brief->seatBelt);
+    writer.Key("clutch");
+    writer.Int(brief->clutch);
+    writer.Key("horn");
+    writer.Int(brief->horn);
+    writer.Key("wiper");
+    writer.Int(brief->wiper);
+    writer.Key("hand_break");
+    writer.Int(brief->handBreak);
+    writer.Key("main_break");
+    writer.Int(brief->mainBreak);
+    writer.Key("left_turn_lamp");
+    writer.Int(brief->leftTurnLamp);
+    writer.Key("right_turn_lamp");
+    writer.Int(brief->rightTurnLamp);
+    writer.Key("clearance_lamp");
+    writer.Int(brief->clearanceLamp);
+    writer.Key("dipped_beam_lamp");
+    writer.Int(brief->dippedBeamLamp);
+    writer.Key("main_beam_lamp");
+    writer.Int(brief->mainBeamLamp);
+    writer.Key("fog_lamp");
+    writer.Int(brief->fogLamp);
+    writer.Key("assist_break");
+    writer.Int(brief->assBreak);
+    writer.Key("surround1");
+    writer.Int(brief->surround1);
+    writer.Key("surround2");
+    writer.Int(brief->surround2);
+    writer.Key("surround3");
+    writer.Int(brief->surround3);
+    writer.Key("surround4");
+    writer.Int(brief->surround4);
+
+    writer.EndObject();
+
+    SendMsgToMainProcIndep(ID_SM_CARSENSOR, sb.GetString());
+}
+
 void MA_SendGpsBrief(const struct gpsBrief *brief)
 {
     if (!(OnOff & MA_OUT_GPS_BRIEF))
diff --git a/lib/src/main/cpp/master/comm_if.h b/lib/src/main/cpp/master/comm_if.h
index 80d93a9..78750ee 100644
--- a/lib/src/main/cpp/master/comm_if.h
+++ b/lib/src/main/cpp/master/comm_if.h
@@ -11,10 +11,41 @@
 struct mcuBrief {
     char version[70];
     int selftest;
-    int gpio;
-    int speed;
-    int engine;
+//    int gpio;
+//    int speed;
+//    int engine;
     char sn[20];
+};
+
+struct carSensorBrief {
+    int odo;
+    int trip;
+    int tripTime;
+    double cellVolt;
+    double speed;
+    int engine;
+    int sas;
+    int key;
+    int gear;
+    int aps;
+    int lock;
+    int seatBelt;
+    int clutch;
+    int horn;
+    int wiper;
+    int handBreak;
+    int mainBreak;
+    int leftTurnLamp;
+    int rightTurnLamp;
+    int clearanceLamp;
+    int dippedBeamLamp;
+    int mainBeamLamp;
+    int fogLamp;
+    int assBreak;
+    int surround1;
+    int surround2;
+    int surround3;
+    int surround4;
 };
 
 struct cardBrief {
@@ -88,6 +119,7 @@
 void MA_SendExamStatus(int start, int errorCode);
 void MA_SendCardBrief(const struct cardBrief *brief);
 void MA_SendMcuBrief(const struct mcuBrief *brief);
+void MA_SendCarSensorBrief(const struct carSensorBrief *brief);
 void MA_SendGpsBrief(const struct gpsBrief *brief);
 void MA_SendRtkBrief(const struct rtkBrief *brief);
 void MA_SendCarPosition(const struct carBrief *brief);
diff --git a/lib/src/main/cpp/mcu/mcu_if.cpp b/lib/src/main/cpp/mcu/mcu_if.cpp
index 4682904..5d2ff23 100644
--- a/lib/src/main/cpp/mcu/mcu_if.cpp
+++ b/lib/src/main/cpp/mcu/mcu_if.cpp
@@ -39,7 +39,7 @@
 #define ID_MC_MCU_DFU_RSP       0x8004
 #define ID_CM_MCU_DFU_DATA       0x0005
 #define ID_CM_MCU_DFU_DATA_CMP  0x0006
-#define ID_MC_MCU_DFU_DATA_RSP  0x8006
+#define ID_MC_CAR_INFO2  0x8006
 #define ID_MC_CAR_INFO           0x8007
 #define ID_MC_RTK_DATA          0x8008
 #define ID_CM_RTK_DATA            0x0008
@@ -357,7 +357,10 @@
             break;
         case ID_MC_MCU_DFU_RSP:
             break;
-        case ID_MC_MCU_DFU_DATA_RSP:
+        case ID_MC_CAR_INFO2:
+            if (lenth > 0) {
+                PlatformStatusChanged(CAR_SENSOR_UPDATE_EVT, data, lenth);
+            }
             break;
         case ID_MC_CAR_INFO: {
             DEBUG("ID_MC_CAR_INFO %d", lenth);
diff --git a/lib/src/main/cpp/rtk_platform/platform.cpp b/lib/src/main/cpp/rtk_platform/platform.cpp
index c47a98e..25b8f82 100644
--- a/lib/src/main/cpp/rtk_platform/platform.cpp
+++ b/lib/src/main/cpp/rtk_platform/platform.cpp
@@ -408,14 +408,84 @@
 
         ConvertHex2String(brief.version, data, x);
         brief.selftest = BUILD_UINT16(data[33], data[32]);
-        brief.gpio = BUILD_UINT16(data[35], data[34]);
-        brief.speed = BUILD_UINT16(data[37], data[36]);
-        brief.engine = BUILD_UINT16(data[39], data[38]);
-        memcpy(brief.sn, data+40, 16);
+//        brief.gpio = BUILD_UINT16(data[35], data[34]);
+//        brief.speed = BUILD_UINT16(data[37], data[36]);
+//        brief.engine = BUILD_UINT16(data[39], data[38]);
+//        memcpy(brief.sn, data+40, 16);
+        memcpy(brief.sn, data+34, 16);
 
         MA_SendMcuBrief(&brief);
 
-        UpdateSensor(brief.gpio, brief.speed, brief.engine);
+//        UpdateSensor(brief.gpio, brief.speed, brief.engine);
+    }
+    if (events & CAR_SENSOR_UPDATE_EVT) {
+        struct carSensorBrief brief;
+        int x = 20;
+
+        brief.odo = BUILD_UINT32(data[3], data[2], data[1], data[0]);
+        brief.trip = BUILD_UINT32(data[7], data[6], data[5], data[4]);
+        brief.tripTime = BUILD_UINT32(data[11], data[10], data[9], data[8]);
+        brief.cellVolt = (double)(BUILD_UINT16(data[13], data[12])) / 10.0;
+        brief.speed = (double)(BUILD_UINT16(data[15], data[14])) / 10.0;
+        brief.engine = BUILD_UINT16(data[17], data[16]);
+        brief.sas = (short)BUILD_UINT16(data[19], data[18]);
+        brief.key = data[x++];
+        brief.gear = data[x++];
+        brief.aps = data[x++];
+        brief.lock = data[x++];
+        brief.seatBelt = data[x++];
+        brief.clutch = data[x++];
+        brief.horn = data[x++];
+        brief.wiper = data[x++];
+        brief.handBreak = data[x++];
+        brief.mainBreak = data[x++];
+        brief.leftTurnLamp = data[x++];
+        brief.rightTurnLamp = data[x++];
+        brief.clearanceLamp = data[x++];
+        brief.dippedBeamLamp = data[x++];
+        brief.mainBeamLamp = data[x++];
+        brief.fogLamp = data[x++];
+        brief.assBreak = data[x++];
+        brief.surround1 = data[x++];
+        brief.surround2 = data[x++];
+        brief.surround3 = data[x++];
+        brief.surround4 = data[x++];
+
+        MA_SendCarSensorBrief(&brief);
+
+        car_sensor_t sensor;
+        x = 20;
+
+        sensor.odo = BUILD_UINT32(data[3], data[2], data[1], data[0]);
+        sensor.trip = BUILD_UINT32(data[7], data[6], data[5], data[4]);
+        sensor.tripTime = BUILD_UINT32(data[11], data[10], data[9], data[8]);
+        sensor.cellVolt = (double)(BUILD_UINT16(data[13], data[12])) / 10.0;
+        sensor.speed = (double)(BUILD_UINT16(data[15], data[14])) / 10.0;
+        sensor.engine = BUILD_UINT16(data[17], data[16]);
+        sensor.sas = (short)BUILD_UINT16(data[19], data[18]);
+        sensor.key = data[x++];
+        sensor.gear = data[x++];
+        sensor.aps = data[x++];
+        sensor.lock = data[x++];
+        sensor.seatBelt = data[x++];
+        sensor.clutch = data[x++];
+        sensor.horn = data[x++];
+        sensor.wiper = data[x++];
+        sensor.handBreak = data[x++];
+        sensor.mainBreak = data[x++];
+        sensor.leftTurnLamp = data[x++];
+        sensor.rightTurnLamp = data[x++];
+        sensor.clearanceLamp = data[x++];
+        sensor.dippedBeamLamp = data[x++];
+        sensor.mainBeamLamp = data[x++];
+        sensor.fogLamp = data[x++];
+        sensor.assBreak = data[x++];
+        sensor.surround1 = data[x++];
+        sensor.surround2 = data[x++];
+        sensor.surround3 = data[x++];
+        sensor.surround4 = data[x++];
+
+        UpdateSensor(&sensor);
     }
     if (events & CARD_UPDATE_EVT) {
         DEBUG("CARD_UPDATE_EVT length %d", length);
diff --git a/lib/src/main/cpp/rtk_platform/platform.h b/lib/src/main/cpp/rtk_platform/platform.h
index 4063643..62c0c60 100644
--- a/lib/src/main/cpp/rtk_platform/platform.h
+++ b/lib/src/main/cpp/rtk_platform/platform.h
@@ -16,6 +16,7 @@
 #define MCU_UPDATE_EVT                              0x0040
 #define CARD_UPDATE_EVT                             0x0080
 #define PLAY_TTS_DONE_EVT                           0x0100
+#define CAR_SENSOR_UPDATE_EVT                           0x0200
 
 typedef struct {
     char domain_name[32];
diff --git a/lib/src/main/cpp/test_common/Geometry.cpp b/lib/src/main/cpp/test_common/Geometry.cpp
index 7a8884b..242f55f 100644
--- a/lib/src/main/cpp/test_common/Geometry.cpp
+++ b/lib/src/main/cpp/test_common/Geometry.cpp
@@ -42,13 +42,6 @@
     return (radians * 180.0) / M_PI;
 }
 
-double round(double number, unsigned int bits) {
-    stringstream ss;
-    ss << setiosflags(ios::fixed) << setprecision(bits) << number;
-    ss >> number;
-    return number;
-}
-
 void MakeLine(Line *line, const PointF *p1, const PointF *p2)
 {
     line->X1 = p1->X;
diff --git a/lib/src/main/cpp/test_common/Geometry.h b/lib/src/main/cpp/test_common/Geometry.h
index ca673d8..399bafb 100644
--- a/lib/src/main/cpp/test_common/Geometry.h
+++ b/lib/src/main/cpp/test_common/Geometry.h
@@ -37,7 +37,6 @@
 inline double toDegree(double radians);
 inline bool isEqual(double a, double b);
 inline bool isEqual2(double a, double b);
-double round(double number, unsigned int bits);
 
 void MakeLine(Line *line, const PointF *p1, const PointF *p2);
 void MakePolygon(Polygon *polygon, std::initializer_list<PointF> point_set);
diff --git a/lib/src/main/cpp/test_common/car_sensor.cpp b/lib/src/main/cpp/test_common/car_sensor.cpp
index ea4c242..8e35440 100644
--- a/lib/src/main/cpp/test_common/car_sensor.cpp
+++ b/lib/src/main/cpp/test_common/car_sensor.cpp
@@ -56,9 +56,11 @@
 } SensorConfig[MAX_SENSOR_NUM];
 static int SensorNum = 0;
 
+static car_sensor_t Sensor;
 static pthread_mutex_t sonser_mutex = PTHREAD_MUTEX_INITIALIZER;
 static pthread_mutex_t status_rw_mutex = PTHREAD_MUTEX_INITIALIZER;
 
+inline static int BX(int value);
 static void WriteCarStatus(uint16_t id, int value);
 static void LRLightTimeout(union sigval sig);
 static void ChangeLRLight(int light);
@@ -74,6 +76,8 @@
     memset(SensorConfig, 0, sizeof(SensorConfig));
 
     memset(CarStatus, 0, sizeof(CarStatus));
+
+    memset(&Sensor, 0, sizeof(Sensor));
 
     left_turn_signal = right_turn_signal = 0;
     flashMainBeamCnt = 0;
@@ -143,6 +147,66 @@
             }
         }
     }
+}
+
+void UpdateSensor(const car_sensor_t *s)
+{
+    WriteCarStatus(OBD_SPEED, s->speed);
+    WriteCarStatus(ENGINE_RPM, s->engine);
+
+    if (s->leftTurnLamp != Sensor.leftTurnLamp && s->leftTurnLamp != '#') {
+        SensorChanged(SENSOR_LEFT_TURN_SIGNAL, BX(s->leftTurnLamp));
+    }
+    if (s->rightTurnLamp != Sensor.leftTurnLamp && s->rightTurnLamp != '#') {
+        SensorChanged(SENSOR_RIGHT_TURN_SIGNAL, BX(s->rightTurnLamp));
+    }
+    if (s->fogLamp != Sensor.fogLamp && s->fogLamp != '#') {
+        SensorChanged(SENSOR_FOG_LIGHT, BX(s->fogLamp));
+    }
+    if (s->clearanceLamp != Sensor.clearanceLamp && s->clearanceLamp != '#') {
+        SensorChanged(SENSOR_CLEARANCE_LIGHT, BX(s->clearanceLamp));
+    }
+    if (s->dippedBeamLamp != Sensor.dippedBeamLamp && s->dippedBeamLamp != '#') {
+        SensorChanged(SENSOR_DIPPED_BEAM_LIGHT, BX(s->dippedBeamLamp));
+    }
+    if (s->mainBeamLamp != Sensor.leftTurnLamp && s->mainBeamLamp != '#') {
+        SensorChanged(SENSOR_MAIN_BEAM_LIGHT, BX(s->mainBeamLamp));
+    }
+    if (s->seatBelt != Sensor.seatBelt && s->seatBelt != '#') {
+        SensorChanged(SENSOR_SEATBELT, BX(s->seatBelt));
+    }
+    if (s->key != Sensor.key && s->key != '#') {
+        SensorChanged(SENSOR_ENGINE_START, s->key == 3? 1 : 0);
+    }
+    if (s->mainBreak != Sensor.mainBreak && s->mainBreak != '#') {
+        SensorChanged(SENSOR_BREAK, BX(s->mainBreak));
+    }
+    if (s->assBreak != Sensor.assBreak && s->assBreak != '#') {
+        SensorChanged(SENSOR_BREAK2, BX(s->assBreak));
+    }
+    if (s->handBreak != Sensor.handBreak && s->handBreak != '#') {
+        SensorChanged(SENSOR_HANDBREAK, BX(s->handBreak));
+    }
+    if (s->lock != Sensor.lock && s->lock != '#') {
+        SensorChanged(SENSOR_DOOR, BX(s->lock));
+    }
+    if (s->surround1 != Sensor.surround1 && s->surround1 != '#') {
+        SensorChanged(SENSOR_SURROUND_CAR_1, BX(s->surround1));
+    }
+    if (s->surround2 != Sensor.surround2 && s->surround2 != '#') {
+        SensorChanged(SENSOR_SURROUND_CAR_2, BX(s->surround2));
+    }
+    if (s->surround3 != Sensor.surround3 && s->surround3 != '#') {
+        SensorChanged(SENSOR_SURROUND_CAR_3, BX(s->surround3));
+    }
+    if (s->surround4 != Sensor.surround4 && s->surround4 != '#') {
+        SensorChanged(SENSOR_SURROUND_CAR_4, BX(s->surround4));
+    }
+    if (s->gear != Sensor.gear && s->gear != '#') {
+        SensorChanged(GEAR, GEAR_N + s->gear);
+    }
+
+    Sensor = *s;
 }
 
 int ReadCarStatus(uint16_t id)
@@ -368,4 +432,7 @@
     }
 }
 
-
+inline static int BX(int value)
+{
+    return (value == 1) ? 1 : 0;
+}
diff --git a/lib/src/main/cpp/test_common/car_sensor.h b/lib/src/main/cpp/test_common/car_sensor.h
index 20695f7..2818c27 100644
--- a/lib/src/main/cpp/test_common/car_sensor.h
+++ b/lib/src/main/cpp/test_common/car_sensor.h
@@ -62,9 +62,42 @@
     SURROUND_CAR_ACTIVE
 };
 
+typedef struct {
+    int odo;
+    int trip;
+    int tripTime;
+    double cellVolt;
+    double speed;
+    int engine;
+    int sas;
+    int key;
+    int gear;
+    int aps;
+    int lock;
+    int seatBelt;
+    int clutch;
+    int horn;
+    int wiper;
+    int handBreak;
+    int mainBreak;
+    int leftTurnLamp;
+    int rightTurnLamp;
+    int clearanceLamp;
+    int dippedBeamLamp;
+    int mainBeamLamp;
+    int fogLamp;
+    int assBreak;
+    int surround1;
+    int surround2;
+    int surround3;
+    int surround4;
+} car_sensor_t;
+
 void CarSensorInit(void);
 int ReadCarStatus(uint16_t id);
 void UpdateSensor(uint16_t gpio, uint16_t speed, uint16_t rpm);
+void UpdateSensor(const car_sensor_t *s);
+
 void SetSensorCfg(int (*sensor)[3], int sensorNum);
 
 #endif //MYAPPLICATION2_COMM_TEST_H
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index 5196886..a1e3357 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -74,7 +74,7 @@
 
 static const int INVALID_ROAD = -1;
 
-static const int TURN_THRESHOLD = 1;
+static const int TURN_THRESHOLD = 3;
 static const int TURN_CHECK_INTERVAL = 500;
 const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
 const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;
@@ -94,7 +94,7 @@
 static char carIntersectionOfGreenLine;
 static int currTurnSignalStatus;
 static int turnSignalStatusWhenCrashGreenLine;
-static bool reportTurnSignalError;
+
 static int prevMoveDirect;
 static uint32_t stopTimepoint = 0;
 static bool reportStopCarOnRedArea;
@@ -135,6 +135,12 @@
 
 static map<int, car_sensor_value_t> CarSensorValue;
 static map<int, int> CrossingHint;
+static map<int, bool> ErrorLaneReport;
+
+static struct RtkTime beginTurnTime, prevDetectTurnTime;
+static int startTurnYaw, prevYaw;
+static int turnCnt, turnTimeCnt;
+static int prevTurnWise;
 
 static const int MAX_ENGINE_RPM = 2500;
 static const double START_CAR_MOVE_DISTANCE = 10.0;
@@ -157,8 +163,9 @@
 
 static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime);
 
-static char isTurn(int currYaw, int prevYaw, int &ang);
-static char CheckCarTurn(LIST_CAR_MODEL &CarModelList);
+static int isTurn(int currYaw, int prevYaw, int thres);
+static void ResetTurnDetect(const car_model *car);
+static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime);
 
 
 static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
@@ -179,7 +186,6 @@
 
     carIntersectionOfGreenLine = 0;
 
-    reportTurnSignalError = false;
     currTurnSignalStatus = OFF_LIGHT;
     prevMoveDirect = 0;
 
@@ -211,6 +217,7 @@
     checkTurn = false;
 
     CrashLineType = -1;
+    turnCnt = -1;
 
     ResetOdo();
     ResetTarget(RoadMap);
@@ -973,7 +980,6 @@
                     } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) {
                         DEBUG("杞悜鐏椂闂翠笉瓒�");
                         // 涓嶈冻3绉掞紝涓嶅悎鏍�
-                        reportTurnSignalError = true;
                         AddExamFault(14, rtkTime);
                     }
                 } else {
@@ -984,7 +990,6 @@
                     } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) {
                         DEBUG("杞悜鐏椂闂翠笉瓒�");
                         // 涓嶈冻3绉掞紝涓嶅悎鏍�
-                        reportTurnSignalError = true;
                         AddExamFault(14, rtkTime);
                     }
                 }
@@ -1027,6 +1032,62 @@
     return CROSSING_NOT_HINT;
 }
 
+static void ResetCrossingStatus(int roadIndex)
+{
+    while (true) {
+        bool w = false;
+        for (auto it = CrossingHint.begin(); it != CrossingHint.end(); ++it) {
+            if (it->first / 100 == roadIndex && it->second != CROSSING_NOT_HINT) {
+                ChangeCrossingStatus(it->first / 100, it->first % 100, CROSSING_NOT_HINT);
+                w = true;
+                break;
+            }
+        }
+        if (!w)
+            break;
+    }
+}
+
+static void SetErrorLaneRpt(int roadIndex, int index, bool status)
+{
+    int key = roadIndex * 100 + index;
+
+    auto it = ErrorLaneReport.find(key);
+
+    if (it != ErrorLaneReport.end()) {
+        ErrorLaneReport.erase(it);
+    }
+    ErrorLaneReport.insert(pair<int, bool>(key, status));
+}
+
+static bool GetErrorLaneRpt(int roadIndex, int index)
+{
+    int key = roadIndex * 100 + index;
+
+    auto it = ErrorLaneReport.find(key);
+
+    if (it != ErrorLaneReport.end()) {
+        return it->second;
+    }
+    return false;
+}
+
+static void ResetErrorLaneRpt(int roadIndex)
+{
+    while (true) {
+        bool w = false;
+        for (auto it = ErrorLaneReport.begin(); it != ErrorLaneReport.end(); ++it) {
+            if (it->first / 100 == roadIndex && it->second == true) {
+                SetErrorLaneRpt(it->first / 100, it->first % 100, false);
+                w = true;
+                break;
+            }
+        }
+        if (!w)
+            break;
+    }
+}
+
 static void HintCrossing(int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
 {
     for (int i = 0; i < road.stopLine.size(); ++i) {
@@ -1053,12 +1114,13 @@
     }
 }
 
-static int NearbyCrossingGuide(int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
+static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
 {
-    int guide = 0;
+    int guide = 0, stopLine = 0;
     double distance;
 
     struct nearby_crossing {
+        int stopLine;
         int guide;
         double distance;
     };
@@ -1077,6 +1139,7 @@
         if (distance > 1e-3) {
             struct nearby_crossing temp;
 
+            temp.stopLine = i;
             temp.guide = road.stopLine[i].active;
             temp.distance = distance;
 
@@ -1092,10 +1155,12 @@
             if (distance > set[i].distance) {
                 distance = set[i].distance;
                 guide = set[i].guide;
+                stopLine = set[i].stopLine;
             }
         }
     }
 
+    stopLineIndex = stopLine;
     return guide;
 }
 
@@ -1286,6 +1351,15 @@
     // 妫�娴嬬寮�姝よ矾娈碉紝鍏ㄨ溅闇�涓嶅湪鑼冨洿鍐�
     int oldid = currExamMapIndex;
     currExamMapIndex = CalcRoadIndex(currExamMapIndex, RoadMap, car);
+    if (oldid != currExamMapIndex) {
+        DEBUG("閬撹矾ID鍒囨崲 %d ======> %d", oldid, currExamMapIndex);
+
+        if (oldid >= 0) {
+            ResetCrossingStatus(oldid);
+            ResetErrorLaneRpt(oldid);
+        }
+    }
+
     if (currExamMapIndex >= 0) {
         car_sensor_value_t brk = ReadCarSensorValue(BREAK);
         HintCrossing(currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList);
@@ -1294,10 +1368,6 @@
         ApproachTarget(RoadMap, car, currExamMapIndex, (brk.value == BREAK_ACTIVE), speed, moveDirect, rtkTime);
     }
     ExitTarget(RoadMap, car, CarModelList, rtkTime);
-
-    if (oldid != currExamMapIndex) {
-        DEBUG("閬撹矾ID鍒囨崲 %d", currExamMapIndex);
-    }
 
     oldid = CrashLineType;
 
@@ -1311,9 +1381,14 @@
     oldid = Lane.guide;
     DetectLane(RoadMap, car, currExamMapIndex, rtkTime);
     if (Lane.guide > 0 && currExamMapIndex >= 0) {
-        if (!(NearbyCrossingGuide(currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList) & Lane.guide)) {
-            DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囧悜琛岄┒");
-            AddExamFault(9, rtkTime);
+        int stop_line;
+
+        if (!(NearbyCrossingGuide(stop_line, currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList) & Lane.guide)) {
+            if (!GetErrorLaneRpt(currExamMapIndex, stop_line)) {
+                DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囧悜琛岄┒");
+                AddExamFault(9, rtkTime);
+                SetErrorLaneRpt(currExamMapIndex, stop_line, true);
+            }
         }
     }
 
@@ -1323,6 +1398,9 @@
 
     // 妫�娴嬪帇绾跨姸鎬�
     TestRoadStartCar(car, speed, moveDirect, rtkTime);
+
+    // 棰濆鐨勮浆鍚戞娴�
+    DetectTurn(car, moveDirect, rtkTime);
 
     if (startCar != START_CAR_DONE)
         return;
@@ -1399,7 +1477,15 @@
     tm.msec = rtkTime->mss;
 }
 
-static char isTurn(int currYaw, int prevYaw, int &ang)
+void CrossRoadCallback(int road, int stop_line, int active, const car_model *car)
+{
+    SetErrorLaneRpt(road, stop_line, false);
+    if (active != ROAD_ACTIVE_FORWARD) {
+        ResetTurnDetect(car);
+    }
+}
+
+static int isTurn(int currYaw, int prevYaw, int thres)
 {
 //    DEBUG("currYaw %d prevYaw %d", currYaw, prevYaw);
     int deltaAng = 0;
@@ -1410,21 +1496,117 @@
         deltaAng = ABS(currYaw - prevYaw);
     }
 
-    ang = deltaAng;
 
 //    DEBUG("瑙掑害宸�� %d", deltaAng);
 
-    if (deltaAng >= TURN_THRESHOLD) {
+    if (deltaAng >= thres) {
         if((( currYaw + 360 - prevYaw) % 360) < 180) {
 //            DEBUG("鍙宠浆");
-            return 'R';
+            return deltaAng;
         } else {
 //            DEBUG("宸﹁浆");
-            return 'L';
+            return 0 - deltaAng;
         }
     }
 
-    return 'N';
+    return 0;
+}
+
+static void ResetTurnDetect(const car_model *car)
+{
+    turnCnt = 0;
+    turnTimeCnt = 0;
+    prevDetectTurnTime = car->tm;
+    prevYaw = car->yaw;
+}
+
+static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime)
+{
+    int angle;
+
+    if (turnCnt < 0 || TimeGetDiff(rtkTime, &prevDetectTurnTime) < 500) {
+        return;
+    }
+
+    angle = isTurn((int) car->yaw, prevYaw, TURN_THRESHOLD);
+
+    prevYaw = (int) car->yaw;
+    prevDetectTurnTime = *rtkTime;
+
+    if (turnCnt == 0) {
+        if (angle != 0 && moveDirect != 0) {
+            DEBUG("鏈夎浆鍚戣抗璞�");
+            turnCnt++;
+            beginTurnTime = *rtkTime;
+            startTurnYaw = (int) car->yaw;
+        }
+    } else if (turnCnt == 1) {
+        if (angle != 0 && moveDirect != 0) {
+            if (angle * prevTurnWise > 0) {
+                DEBUG("纭杞悜");
+                // 鍚屽悜杞姩
+                turnCnt++;
+                turnTimeCnt = 0;
+            } else {
+                beginTurnTime = *rtkTime;
+                startTurnYaw = (int) car->yaw;
+            }
+        } else {
+            turnCnt = 0;
+        }
+    } else if (turnCnt >= 2) {
+        if (moveDirect == 0) {
+            // 鏆傚仠
+        } else {
+            turnTimeCnt += TimeGetDiff(rtkTime, &prevDetectTurnTime);
+            int wise = isTurn((int) car->yaw, startTurnYaw, TURN_THRESHOLD);
+            DEBUG("杞姩瑙掑害 %d", wise);
+            if (ABS(wise) > 60) {
+                // 纭杞集琛屼负锛屾娴嬪紑濮嬪垰杞集鏃惰浆鍚戠伅鎯呭喌
+                turnCnt = -1;
+
+                car_sensor_value_t lamp = ReadCarSensorValue(TURN_SIGNAL_LAMP);
+                if (lamp.name == TURN_SIGNAL_LAMP) {
+                    if (wise < 0) {
+                        if (lamp.value != LEFT_TURN_LIGHT) {
+                            DEBUG("鍙樿皟鏈墦鐏�!!");
+                            // 娌℃墦鐏紝涓嶅悎鏍�
+                            AddExamFault(13, rtkTime);
+                        } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= D_SEC(3)) {
+                            DEBUG("杞悜鐏椂闂翠笉瓒�");
+                            // 涓嶈冻3绉掞紝涓嶅悎鏍�
+                            AddExamFault(14, rtkTime);
+                        }
+                    } else {
+                        if (lamp.value != RIGHT_TURN_LIGHT) {
+                            DEBUG("鍙樿皟鏈墦鐏�!!");
+                            // 娌℃墦鐏紝涓嶅悎鏍�
+                            AddExamFault(13, rtkTime);
+                        } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= D_SEC(3)) {
+                            DEBUG("杞悜鐏椂闂翠笉瓒�");
+                            // 涓嶈冻3绉掞紝涓嶅悎鏍�
+                            AddExamFault(14, rtkTime);
+                        }
+                    }
+                }
+            } else if (turnTimeCnt > D_SEC(10)) {
+                // 鍙栨秷杞悜妫�娴�
+                DEBUG("鍙栨秷杞悜妫�娴�1");
+                turnCnt = -1;
+            }
+        }
+    }
+
+    if (turnCnt < 2 && moveDirect != 0) {
+        turnTimeCnt += TimeGetDiff(rtkTime, &prevDetectTurnTime);
+        if (turnTimeCnt > D_SEC(15)) {
+            // 鍙栨秷杞悜妫�娴�
+            DEBUG("鍙栨秷杞悜妫�娴�2");
+            turnCnt = -1;
+        }
+    }
+
+    prevTurnWise = angle;
 }
 
 static char CheckCarTurn(LIST_CAR_MODEL &CarModelList)
diff --git a/lib/src/main/cpp/test_items2/road_exam.h b/lib/src/main/cpp/test_items2/road_exam.h
index 6959ca8..626a110 100644
--- a/lib/src/main/cpp/test_items2/road_exam.h
+++ b/lib/src/main/cpp/test_items2/road_exam.h
@@ -25,7 +25,7 @@
     int sec;
     int msec;
 };
-
+void CrossRoadCallback(int road, int stop_line, int active, const car_model *car);
 void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime);
 void InitRoadExam(road_exam_map &RoadMap);
 void TerminateRoadExam(void);
diff --git a/lib/src/main/cpp/test_items2/through_something.cpp b/lib/src/main/cpp/test_items2/through_something.cpp
index c86aaec..7372634 100644
--- a/lib/src/main/cpp/test_items2/through_something.cpp
+++ b/lib/src/main/cpp/test_items2/through_something.cpp
@@ -154,6 +154,9 @@
 
             if (CrashTheLine(RoadMap.roads[road].stopLine[x].line, car, CarModelList)) {
                 DEBUG("瓒婅繃璺彛 road %d 璺彛 %d", road, x);
+
+                CrossRoadCallback(road, x, RoadMap.roads[road].stopLine[x].active, car);
+
                 if (!(it->second & REDUCE_SPEED)) {
                     // 涓嶆寜瑙勫畾鍑忛�燂紝涓嶅悎鏍�
                     DEBUG("涓嶆寜瑙勫畾鍑忛��");
diff --git a/lib/src/main/cpp/utils/num.cpp b/lib/src/main/cpp/utils/num.cpp
index 0019ccf..6627921 100644
--- a/lib/src/main/cpp/utils/num.cpp
+++ b/lib/src/main/cpp/utils/num.cpp
@@ -3,6 +3,11 @@
 //
 
 #include "num.h"
+#include <sstream>
+#include <iosfwd>
+#include <iomanip>
+
+using namespace std;
 
 long str2int(const uint8_t *s, uint16_t length)
 {
@@ -85,3 +90,10 @@
 
     return n ;
 }
+
+double round(double number, unsigned int bits) {
+    stringstream ss;
+    ss << setiosflags(ios::fixed) << setprecision(bits) << number;
+    ss >> number;
+    return number;
+}
diff --git a/lib/src/main/cpp/utils/num.h b/lib/src/main/cpp/utils/num.h
index 174d46f..21299b1 100644
--- a/lib/src/main/cpp/utils/num.h
+++ b/lib/src/main/cpp/utils/num.h
@@ -10,5 +10,6 @@
 long str2int(const uint8_t *s, uint16_t length);
 bool str2float(double *f, const uint8_t *s, uint16_t length);
 int BitCount(uint32_t n);
+double round(double number, unsigned int bits);
 
 #endif //RTKDRIVERTEST_NUM_H

--
Gitblit v1.8.0