| lib/src/main/cpp/driver_test.cpp | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 | |
| lib/src/main/cpp/native-lib.cpp | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 | |
| lib/src/main/cpp/rtk_platform/platform.cpp | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 | |
| lib/src/main/cpp/test_common/car_sensor.cpp | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 | |
| lib/src/main/cpp/test_items2/change_lane.cpp | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 | |
| lib/src/main/cpp/test_items2/drive_straight.cpp | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 | |
| lib/src/main/cpp/test_items2/drive_straight.h | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 | |
| lib/src/main/cpp/test_items2/operate_gear.cpp | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 | |
| lib/src/main/cpp/test_items2/overtake.cpp | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 | |
| lib/src/main/cpp/test_items2/road_exam.cpp | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 |
lib/src/main/cpp/driver_test.cpp
@@ -513,7 +513,8 @@ if (TimeGetDiff(rtkTime, &cTime) >= D_SEC(3)) { cTime = *rtkTime; DEBUG("ENGINE_RPM %d OBD_SPEED %f SPEED %f", DEBUG("GEAR %d RPM %d OBD_SPEED %f SPEED %f", ReadCarStatus(GEAR) - GEAR_N, ReadCarStatus(ENGINE_RPM), ((double)ReadCarStatus(OBD_SPEED)) / 10.0, speed * 3.6); lib/src/main/cpp/native-lib.cpp
@@ -27,7 +27,7 @@ const int RTK_PLATFORM_PORT = 12125; const uint8_t phone[] = {0x20,0x19,0x10,0x15,0x00,0x00,0x00,0x01}; const char *VIRTUAL_RTK_IP = "192.168.1.6"; const char *VIRTUAL_RTK_IP = "192.168.1.7"; const int VIRTUAL_RTK_PORT = 9001; static pthread_mutex_t tts_mutex = PTHREAD_MUTEX_INITIALIZER; lib/src/main/cpp/rtk_platform/platform.cpp
@@ -489,6 +489,9 @@ sensor.surround3 = data[x++]; sensor.surround4 = data[x++]; if (sensor.clutch == 1) sensor.gear = 0; UpdateSensor(&sensor); } if (events & CARD_UPDATE_EVT) { lib/src/main/cpp/test_common/car_sensor.cpp
@@ -77,6 +77,7 @@ memset(CarStatus, 0, sizeof(CarStatus)); CarStatus[DOOR] = DOOR_CLOSE; CarStatus[GEAR] = GEAR_N; memset(&Sensor, 0, sizeof(Sensor)); @@ -265,7 +266,7 @@ static void SensorChanged(int id, int value) { DEBUG("状态改变 %d = %d", id, value); // DEBUG("状态改变 %d = %d", id, value); switch (id) { case SENSOR_LEFT_TURN_SIGNAL: { lib/src/main/cpp/test_items2/change_lane.cpp
@@ -18,6 +18,7 @@ { maxMoveDistance = ReadOdo(); start = true; DEBUG("变道开始里程 %ld", maxMoveDistance); } void StartChaneLaneExam(int ori_lane) @@ -41,7 +42,7 @@ if (ReadOdo() - maxMoveDistance > 100) { // 超车未完成 DEBUG("超车固定距离内未完成"); DEBUG("变道固定距离内未完成 当前里程 %ld", ReadOdo()); AddExamFault(3, rtkTime); return false; } lib/src/main/cpp/test_items2/drive_straight.cpp
@@ -20,6 +20,7 @@ static int setup; static double beginOdo; static int yaw_stat; static double offsetBase; static void TtsBack(int seq) { @@ -33,9 +34,13 @@ PlayTTS("请保持直线行驶", TtsBack); } bool TestDriveStraight(const car_model *car, const struct RtkTime *rtkTime) { bool TestDriveStraight(road_exam_map &RoadMap, int roadIndex, const car_model *car, const struct RtkTime *rtkTime) { static double yaw; static Line baseLine; static double offset1, offset2; if (roadIndex < 0 || roadIndex >= RoadMap.roads.size()) return false; if (setup == 1) { // 偏航角平均值 @@ -47,7 +52,7 @@ yaws.push_back(car->yaw); yaw_stat++; if (yaw_stat == 5) { if (yaw_stat == 15) { yaw = AvgYaw(yaws); vector<double>().swap(yaws); @@ -55,13 +60,28 @@ PointF extPoint = PointExtend(car->basePoint, 100, yaw); MakeLine(&baseLine, &car->basePoint, &extPoint); beginOdo = ReadOdo(); PointF px = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, car->carXY[car->axial[AXIAL_FRONT]]); offsetBase = DistanceOf(px, car->carXY[car->axial[AXIAL_FRONT]]); DEBUG("起点偏离 %f", offsetBase); setup = 2; } } else if (setup == 2) { double offset = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine); offset1 = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine); if (offset > MAX_OFFSET_DISTANCE) { DEBUG("直线偏移大于30厘米 offset = %f", offset); if (offset1 > MAX_OFFSET_DISTANCE) { DEBUG("虚拟直线偏移大于30厘米 offset1 = %f", offset1); // // 偏移大于30厘米，不合格 // AddExamFault(30, rtkTime); // return false; } PointF px = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, car->carXY[car->axial[AXIAL_FRONT]]); offset2 = DistanceOf(px, car->carXY[car->axial[AXIAL_FRONT]]); if (fabs(offset2 - offsetBase) > MAX_OFFSET_DISTANCE) { DEBUG("直线偏移大于30厘米 offset2 = %f", fabs(offset2 - offsetBase)); // 偏移大于30厘米，不合格 AddExamFault(30, rtkTime); return false; @@ -69,7 +89,8 @@ } if (setup == 2 && ReadOdo() - beginOdo > CHECK_STAGE_DISTANCE) { DEBUG("直线行驶结束"); DEBUG("直线行驶结束 offset1 = %f offset2 = %f", offset1, fabs(offset2 - offsetBase)); PlayTTS("直线行驶结束", NULL); return false; } lib/src/main/cpp/test_items2/drive_straight.h
@@ -8,6 +8,6 @@ #include "../driver_test.h" void StartDriveStraightExam(void); bool TestDriveStraight(const car_model *car, const struct RtkTime *rtkTime); bool TestDriveStraight(road_exam_map &RoadMap, int roadIndex, const car_model *car, const struct RtkTime *rtkTime); #endif //MYAPPLICATION2_DRIVE_STRAIGHT_H lib/src/main/cpp/test_items2/operate_gear.cpp
@@ -47,9 +47,10 @@ if (setup == 0) { return true; } else if (setup == 1) { if (sensor.value != GEAR_N) if (sensor.value != GEAR_N) { oldGear = sensor.value; DEBUG("准备首次换挡 GEAR = %d", oldGear); } switch (sensor.value) { case GEAR_1: { expectGear = GEAR_2; @@ -89,49 +90,56 @@ } } else if (setup == 2) { if (sensor.value == GEAR_N || sensor.value == oldGear) { DEBUG("等待首次换挡"); } else if (sensor.value != expectGear) { // 未按指令操作挡位，不合格 DEBUG("未按指令操作挡位"); DEBUG("首次换挡错误 GEAR %d 希望 %d", sensor.value, expectGear); AddExamFault(31, rtkTime); return false; } else { // 在此挡位行驶一定距离，再执行下一个 DEBUG("首次换挡成功，下一个..."); oldGear = expectGear; shiftTime = *rtkTime; setup = 3; } } else if (setup == 3) { if (TimeGetDiff(&shiftTime, rtkTime) >= D_SEC(4)) { if (TimeGetDiff(rtkTime, &shiftTime) >= D_SEC(3)) { setup = 4; char buff[128]; expectGear += 0 - upDownShift; sprintf(buff, "请%s到%d挡", upDownShift > 0 ? "减": "加", expectGear - GEAR_N); PlayTTS(buff, NULL); DEBUG("%s", buff); DEBUG("准备二次换挡 希望 %d", expectGear); } } else if (setup == 4) { if (sensor.value == GEAR_N || sensor.value == oldGear) { DEBUG("等待二次换挡"); } else if (sensor.value != expectGear) { // 未按指令操作挡位，不合格 DEBUG("未按指令操作挡位"); DEBUG("二次换挡错误 GEAR %d 希望 %d", sensor.value, expectGear); AddExamFault(31, rtkTime); return false; } else { // 在此挡位行驶一定距离，再执行下一个 shiftTime = *rtkTime; setup = 5; DEBUG("二次换挡成功，下一个..."); } } else if (setup == 5) { if (TimeGetDiff(&shiftTime, rtkTime) >= D_SEC(5)) { if (TimeGetDiff(rtkTime, &shiftTime) >= D_SEC(3)) { PlayTTS("加减挡位操作结束", NULL); DEBUG("加减挡位操作结束"); return false; } } if (ReadOdo() - maxMoveDistance > 120) { // 未按指令操作挡位，不合格 DEBUG("未按指令操作挡位"); DEBUG("未按指令操作挡位，超时"); AddExamFault(31, rtkTime); return false; } lib/src/main/cpp/test_items2/overtake.cpp
@@ -18,6 +18,7 @@ { maxMoveDistance = ReadOdo(); setup = 1; DEBUG("超车开始里程 %ld", maxMoveDistance); } void StartOvertakeExam(int ori_lane) @@ -61,7 +62,7 @@ if (ReadOdo() - maxMoveDistance > 150) { // 超车未完成 DEBUG("超车固定距离内未完成"); DEBUG("超车固定距离内未完成 当前里程 %ld", ReadOdo()); AddExamFault(3, rtkTime); return false; } lib/src/main/cpp/test_items2/road_exam.cpp
@@ -72,7 +72,7 @@ const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3; const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1; const double EXAM_RANGE = 3000.0; // 至少驾驶距离 const double EXAM_RANGE = 2700.0; // 至少驾驶距离 static bool occurOverSpeed; static bool occurSecondBreak; @@ -93,6 +93,8 @@ static struct RtkTime gearErrorTimePoint; static struct RtkTime gearNSlideTimePoint; static car_sensor_value_t turnSignalStatus; static int gearErrorTime; @@ -127,13 +129,14 @@ } RoadExamItem_t; static map<int, int> RoadExamItem; static int RoadExamStatus; static bool win; static struct RtkTime beginTurnTime, prevDetectTurnTime; static int startTurnYaw, prevYaw; static int turnCnt, turnTimeCnt; static int prevTurnWise; static const uint32_t GEAR_N_SLIDE_TIMEOUT = D_SEC(5); static const uint32_t GEAR_N_SLIDE_TIMEOUT = D_SEC(10); static const uint32_t GEAR_ERROR_TIMEOUT = D_SEC(15); static const uint32_t STOP_CAR_TIME = D_SEC(2); static const uint32_t CHANGE_LANE_MIN_INTERVAL = D_SEC(10); @@ -199,6 +202,8 @@ RoadExamItem[ROAD_EXAM_ITEM_CAR_STOP] = ROAD_EXAM_ITEM_NOT_EXEC; RoadExamStatus = ROAD_EXAM_READY_NEXT; win = false; } void TerminateRoadExam(void) @@ -879,7 +884,7 @@ checkCrashGreenTimeout = 0; // 记录开始压线的时间，不确定是否有变道意图，待确认变道后再处理之 crashGreenStartTime = *rtkTime; turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP); DEBUG("开始压虚线 %02d-%02d-%02d %02d:%02d:%02d", crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD, crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss, crashGreenStartTime.mss * 10); } crashDottedLine = true; @@ -926,40 +931,45 @@ if (newLane.road == Lane.road && newLane.sep == Lane.sep) { gain = newLane.no - Lane.no; } else { DEBUG("车道分段切换"); DEBUG("车道分段切换 Old Lane road = %d sep = %d no = %d New Lane road = %d sep = %d no = %d", Lane.road, Lane.sep, Lane.no, newLane.road, newLane.sep, newLane.no); ChangeLane.gain = 0; } // 检查转向灯 if (gain != 0) { DEBUG("变道 new lane %d, gain %d", newLane.no, gain); car_sensor_value_t lamp = ReadCarSensorValue(TURN_SIGNAL_LAMP); if (lamp.name == TURN_SIGNAL_LAMP) { if (gain < 0) { if (lamp.value != LEFT_TURN_LIGHT) { if (turnSignalStatus.value != LEFT_TURN_LIGHT) { DEBUG("变调未打灯!!"); // 没打灯，不合格 AddExamFault(13, rtkTime); } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) < TURN_SIGNAL_LAMP_ADVANCE) { } else if (TimeGetDiff(&crashGreenStartTime, &turnSignalStatus.time) < TURN_SIGNAL_LAMP_ADVANCE) { DEBUG("转向灯时间不足 %02d-%02d-%02d %02d:%02d:%02d.%03d -> %02d-%02d-%02d %02d:%02d:%02d.%03d", crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD, crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss, crashGreenStartTime.mss * 10, lamp.time.YY, lamp.time.MM, lamp.time.DD, lamp.time.hh, lamp.time.mm, lamp.time.ss, lamp.time.mss * 10); crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD, crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss, crashGreenStartTime.mss * 10, turnSignalStatus.time.YY, turnSignalStatus.time.MM, turnSignalStatus.time.DD, turnSignalStatus.time.hh, turnSignalStatus.time.mm, turnSignalStatus.time.ss, turnSignalStatus.time.mss * 10); // 不足3秒，不合格 AddExamFault(14, rtkTime); } } else { if (lamp.value != RIGHT_TURN_LIGHT) { if (turnSignalStatus.value != RIGHT_TURN_LIGHT) { DEBUG("变调未打灯!!"); // 没打灯，不合格 AddExamFault(13, rtkTime); } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) < TURN_SIGNAL_LAMP_ADVANCE) { } else if (TimeGetDiff(&crashGreenStartTime, &turnSignalStatus.time) < TURN_SIGNAL_LAMP_ADVANCE) { DEBUG("转向灯时间不足 %02d-%02d-%02d %02d:%02d:%02d.%03d -> %02d-%02d-%02d %02d:%02d:%02d.%03d", crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD, crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss, crashGreenStartTime.mss * 10, lamp.time.YY, lamp.time.MM, lamp.time.DD, lamp.time.hh, lamp.time.mm, lamp.time.ss, lamp.time.mss * 10); crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD, crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss, crashGreenStartTime.mss * 10, turnSignalStatus.time.YY, turnSignalStatus.time.MM, turnSignalStatus.time.DD, turnSignalStatus.time.hh, turnSignalStatus.time.mm, turnSignalStatus.time.ss, turnSignalStatus.time.mss * 10); // 不足3秒，不合格 AddExamFault(14, rtkTime); } } } @@ -1238,6 +1248,11 @@ UpdataOdo(speed, moveDirect, rtkTime); if (!win) { win = true; turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP); } // 刹车提示音 RingBreak(); @@ -1323,6 +1338,7 @@ // 挡位不匹配超时 if (currGearError && prevGearError) { DEBUG("挡位错误增加 %ld毫秒 当前挡位 %d 时速 %f", TimeGetDiff(rtkTime, &gearErrorTimePoint), ReadCarStatus(GEAR), ConvertMs2KMh(speed)); gearErrorTime += TimeGetDiff(rtkTime, &gearErrorTimePoint); } if (gearErrorTime > GEAR_ERROR_TIMEOUT) { @@ -1425,10 +1441,11 @@ // DEBUG("Lane信息 road %d sep %d total %d no %d guide %d", Lane.road, Lane.sep, Lane.total, Lane.no, Lane.guide); if (Lane.guide > 0 && currExamMapIndex >= 0) { int stop_line_index; int act = NearbyCrossingGuide(stop_line_index, currExamMapIndex, RoadMap.roads[currExamMapIndex], car); if (!(NearbyCrossingGuide(stop_line_index, currExamMapIndex, RoadMap.roads[currExamMapIndex], car) & Lane.guide)) { if (act != 0 && !(act & Lane.guide)) { if (!GetErrorLaneRpt(currExamMapIndex, stop_line_index)) { DEBUG("不按规定车道标向行驶 %d: %d guide = %d", currExamMapIndex, stop_line_index, Lane.guide); DEBUG("不按规定车道标向行驶 %d: %d 期望 = %d guide = %d", currExamMapIndex, stop_line_index, act, Lane.guide); AddExamFault(9, rtkTime); SetErrorLaneRpt(currExamMapIndex, stop_line_index, true); } @@ -1547,7 +1564,7 @@ testing = TestOvertake(Lane.no, rtkTime); break; case ROAD_EXAM_ITEM_STRAIGHT: testing = TestDriveStraight(car, rtkTime); testing = TestDriveStraight(RoadMap, roadIndex, car, rtkTime); break; case ROAD_EXAM_ITEM_CAR_STOP: testing = TestStopCar(RoadMap, roadIndex, car, moveDirect, rtkTime); @@ -1650,6 +1667,8 @@ turnCnt++; beginTurnTime = *rtkTime; startTurnYaw = (int) car->yaw; turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP); } } else if (turnCnt == 1) { if (angle != 0 && moveDirect != 0) { @@ -1661,6 +1680,8 @@ } else { beginTurnTime = *rtkTime; startTurnYaw = (int) car->yaw; turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP); } } else { turnCnt = 0; @@ -1676,28 +1697,27 @@ // 确认转弯行为，检测开始刚转弯时转向灯情况 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) { if (turnSignalStatus.value != LEFT_TURN_LIGHT) { DEBUG("变调未打灯!!"); // 没打灯，不合格 AddExamFault(13, rtkTime); } else if (TimeGetDiff(&beginTurnTime, &lamp.time) < TURN_SIGNAL_LAMP_ADVANCE) { } else if (TimeGetDiff(&beginTurnTime, &turnSignalStatus.time) < TURN_SIGNAL_LAMP_ADVANCE) { DEBUG("转向灯时间不足"); // 不足3秒，不合格 AddExamFault(14, rtkTime); } } else { if (lamp.value != RIGHT_TURN_LIGHT) { if (turnSignalStatus.value != RIGHT_TURN_LIGHT) { DEBUG("变调未打灯!!"); // 没打灯，不合格 AddExamFault(13, rtkTime); } else if (TimeGetDiff(&beginTurnTime, &lamp.time) < TURN_SIGNAL_LAMP_ADVANCE) { } else if (TimeGetDiff(&beginTurnTime, &turnSignalStatus.time) < TURN_SIGNAL_LAMP_ADVANCE) { DEBUG("转向灯时间不足"); // 不足3秒，不合格 AddExamFault(14, rtkTime); } } } } else if (turnTimeCnt > D_SEC(10)) {
