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