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" 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)) 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); 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); 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); 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]; 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; 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); 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; } 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 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) 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); 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("不按规定减速"); 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; } 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
