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