| | |
| | | test_items2/dummy_light.cpp |
| | | test_items2/road_exam.cpp |
| | | test_items2/through_something.cpp |
| | | test_items2/drive_straight.cpp |
| | | |
| | | rtk_module/rtk.cpp |
| | | rtk_module/virtual_rtk.cpp |
| | |
| | | if (map.area.point != NULL) { |
| | | free(map.area.point); |
| | | } |
| | | |
| | | if (map.roadEdgeLine != NULL) { |
| | | for (int j = 0; j < map.roadEdgeLineNum; ++j) { |
| | | if (map.roadEdgeLine[j].point != NULL) |
| | | free(map.roadEdgeLine[j].point); |
| | | } |
| | | |
| | | free(map.roadEdgeLine); |
| | | } |
| | | } |
| | | |
| | | RoadMapList.clear(); |
| | |
| | | vector<vector<int>> &redAreas, |
| | | vector<vector<int>> &greenLines, |
| | | vector<vector<int>> &triggerLines, |
| | | vector<vector<int>> &roadEdgeLines, |
| | | vector<int> area, |
| | | vector<int> stopLine) |
| | | { |
| | |
| | | } |
| | | } else { |
| | | newMap.triggerLine = NULL; |
| | | }; |
| | | } |
| | | |
| | | if ((newMap.roadEdgeLineNum = roadEdgeLines.size()) > 0) { |
| | | newMap.roadEdgeLine = (Polygon *) malloc(sizeof(Polygon) * newMap.roadEdgeLineNum); |
| | | |
| | | DEBUG("道路边线 %d 根", newMap.roadEdgeLineNum); |
| | | for (int i = 0; i < newMap.roadEdgeLineNum; ++i) { |
| | | newMap.roadEdgeLine[i].num = roadEdgeLines[i].size(); |
| | | newMap.roadEdgeLine[i].point = (PointF *) malloc(sizeof(PointF) * newMap.roadEdgeLine[i].num); |
| | | |
| | | DEBUG(" 结点 %d 个", newMap.roadEdgeLine[i].num); |
| | | for (int j = 0; j < newMap.roadEdgeLine[i].num; ++j) { |
| | | newMap.roadEdgeLine[i].point[j] = RoadMapPoints.point[roadEdgeLines[i][j]]; |
| | | } |
| | | } |
| | | } else { |
| | | newMap.roadEdgeLine = NULL; |
| | | } |
| | | |
| | | if (area.size() > 0) { |
| | | newMap.area.num = area.size(); |
| | |
| | | int triggerLineNum; |
| | | trigger_line_t *triggerLine; |
| | | |
| | | int roadEdgeLineNum; // 道路边线,只有直线驾驶、靠边停车才有 |
| | | Polygon *roadEdgeLine; |
| | | |
| | | Polygon area; // 子项目的区域 |
| | | Line stopLine; // 诸如人行道、路口等考点的停止线 |
| | | int flagStop; // 到达开始线前,是否需要停车 |
| | |
| | | void CleanRoadMap(void); |
| | | void SetRoadMapPoints(vector<double> &mapPoints); |
| | | void AddRoadMapParent(int id, int type, string tts, int stopFlag, vector<vector<int>> &redLines, |
| | | vector<vector<int>> &redAreas, vector<vector<int>> &greenLines, vector<vector<int>> &triggerLines, |
| | | vector<vector<int>> &redAreas, vector<vector<int>> &greenLines, |
| | | vector<vector<int>> &triggerLines, vector<vector<int>> &roadEdgeLines, |
| | | vector<int> area, vector<int> stopLine); |
| | | |
| | | void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire, |
| | |
| | | vector<vector<int>> greenLines; |
| | | vector<vector<int>> triggerLines; |
| | | vector<vector<int>> redAreas; |
| | | vector<vector<int>> roadEdgeLines; |
| | | |
| | | vector<int> area; |
| | | vector<int> stopLine; |
| | | |
| | | roadEdgeLines.clear(); |
| | | stopLine.clear(); |
| | | area.clear(); |
| | | tts.clear(); |
| | |
| | | } |
| | | } |
| | | |
| | | if (itr->HasMember("road_edge_line")) { |
| | | const Value &s = (*itr)["road_edge_line"]; |
| | | |
| | | for (Value::ConstValueIterator itrLine = s.Begin(); |
| | | itrLine != s.End(); ++itrLine) { |
| | | points.clear(); |
| | | for (Value::ConstValueIterator itrPoint = (*itrLine).Begin(); |
| | | itrPoint != (*itrLine).End(); ++itrPoint) { |
| | | points.push_back((*itrPoint).GetInt()); |
| | | } |
| | | roadEdgeLines.push_back(points); |
| | | } |
| | | } |
| | | |
| | | if (itr->HasMember("area")) { |
| | | const Value &s = (*itr)["area"]; |
| | | |
| | |
| | | tts = s.GetString(); |
| | | } |
| | | |
| | | AddRoadMapParent(id, type, tts, stop_flag, redLines, redAreas, greenLines, triggerLines, area, stopLine); |
| | | AddRoadMapParent(id, type, tts, stop_flag, redLines, redAreas, greenLines, triggerLines, roadEdgeLines, area, stopLine); |
| | | } |
| | | } |
| | | } |
| | |
| | | const char *VIRTUAL_RTK_IP = "192.168.16.100"; |
| | | const int VIRTUAL_RTK_PORT = 9001; |
| | | |
| | | static pthread_mutex_t tts_mutex = PTHREAD_MUTEX_INITIALIZER; |
| | | |
| | | static int ttsSeq = 0; |
| | | |
| | | static void SendBootIndicate(union sigval sig); |
| | | |
| | | int DESEncrypt(const uint8_t *key, int key_length, |
| | |
| | | } |
| | | } |
| | | |
| | | void PlayTTS(const char *string, int id) |
| | | static int GetTtsSeq(void) |
| | | { |
| | | int seq = 0; |
| | | |
| | | pthread_mutex_lock(&tts_mutex); |
| | | seq = ttsSeq++; |
| | | pthread_mutex_unlock(&tts_mutex); |
| | | |
| | | return seq; |
| | | } |
| | | |
| | | int PlayTTS(const char *string) |
| | | { |
| | | DEBUG("PlayTTS: %s", string); |
| | | |
| | |
| | | // Attach主线程 |
| | | if (sg_jvm->AttachCurrentThread(&env, NULL) != JNI_OK) { |
| | | LOGE("%s: AttachCurrentThread() failed", __FUNCTION__); |
| | | return; |
| | | return -1; |
| | | } |
| | | } else { |
| | | ready_in_java_env = true; |
| | |
| | | |
| | | jclass cls = env->GetObjectClass(sg_obj); |
| | | jmethodID fun = env->GetMethodID(cls, "TextSpeak", "(Ljava/lang/String;I)V"); |
| | | |
| | | int id = GetTtsSeq(); |
| | | |
| | | env->CallVoidMethod(sg_obj, fun, env->NewStringUTF(string), id); |
| | | |
| | |
| | | LOGE("%s: DetachCurrentThread() failed", __FUNCTION__); |
| | | } |
| | | } |
| | | |
| | | return id; |
| | | } |
| | | |
| | | extern "C" |
| | |
| | | AppTimer_add(SendBootIndicate, 500); |
| | | |
| | | InitVirtualDevice(VIRTUAL_RTK_IP, VIRTUAL_RTK_PORT); |
| | | |
| | | pthread_mutex_init(&tts_mutex, NULL); |
| | | } |
| | | |
| | | static void SendBootIndicate(union sigval sig) { |
| | |
| | | void TextOsd(int type, const char *text); |
| | | void DrawScreen(const Polygon *map, const Polygon *car); |
| | | void SendMsgToMainProc(int cmd, const char *value); |
| | | void PlayTTS(const char *string, int id); |
| | | int PlayTTS(const char *string); |
| | | |
| | | #endif //RTKBASESTATION_NATIVE_LIB_H |
| | |
| | | |
| | | SensorNum = sensorNum; |
| | | for (int i = 0; i < sensorNum; ++i) { |
| | | DEBUG(" 配置<%d>: id %d - fun %d - lvl %d", i, sensor[i][0], sensor[i][1], sensor[i][2]); |
| | | |
| | | SensorConfig[i].gpioId = sensor[i][0]; |
| | | SensorConfig[i].funId = sensor[i][1]; |
| | | if (sensor[i][2] > 0) { |
| | |
| | | BREAK, |
| | | HAND_BREAK, |
| | | SECOND_BREAK, |
| | | DOOR, |
| | | CAR_STATUS_END ////////////// |
| | | }; |
| | | |
| | |
| | | EJECT_SEATBELT = 0, |
| | | ENGINE_START_INACTIVE = 0, |
| | | BREAK_INACTIVE = 0, |
| | | DOOR_OPEN = 0, |
| | | HAZARD_LIGHTS, |
| | | LEFT_TURN_LIGHT, |
| | | RIGHT_TURN_LIGHT, |
| | |
| | | GEAR_3, |
| | | GEAR_4, |
| | | GEAR_5, |
| | | BREAK_ACTIVE |
| | | BREAK_ACTIVE, |
| | | DOOR_CLOSE |
| | | }; |
| | | |
| | | void CarSensorInit(void); |
New file |
| | |
| | | // |
| | | // Created by YY on 2020/3/23. |
| | | // |
| | | |
| | | #include "drive_straight.h" |
| | | #include "../driver_test.h" |
| | | #include "../native-lib.h" |
| | | #include "../jni_log.h" |
| | | #include "road_exam.h" |
| | | #include <cmath> |
| | | |
| | | #define DEBUG(fmt, args...) LOGD("<drive_straight> <%s>: " fmt, __func__, ##args) |
| | | |
| | | static bool crossStartLine; |
| | | static bool reportOffsetOver; |
| | | static double edgeDistance; |
| | | |
| | | void StartDriveStraightExam(int index, LIST_ROAD_MAP &RoadMapList) { |
| | | if (index == -1) |
| | | return; |
| | | |
| | | DEBUG("进入路考直线行驶地图 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type); |
| | | |
| | | if (!RoadMapList[index].tts.empty()) { |
| | | DEBUG("播放TTS"); |
| | | PlayTTS(RoadMapList[index].tts.c_str()); |
| | | } else { |
| | | DEBUG("没有TTS"); |
| | | } |
| | | |
| | | crossStartLine = false; |
| | | reportOffsetOver = false; |
| | | } |
| | | |
| | | int ExecuteDriveStraightExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, |
| | | LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) { |
| | | Line road_edge; |
| | | |
| | | static PointF startPoint; |
| | | |
| | | MakeLine(&road_edge, &RoadMapList[index].roadEdgeLine[0].point[0], &RoadMapList[index].roadEdgeLine[0].point[1]); |
| | | |
| | | if (!crossStartLine) { |
| | | PointF p1, p2; |
| | | |
| | | p1.X = RoadMapList[index].stopLine.X1; |
| | | p1.Y = RoadMapList[index].stopLine.Y1; |
| | | p2.X = RoadMapList[index].stopLine.X2; |
| | | p2.Y = RoadMapList[index].stopLine.Y2; |
| | | |
| | | if (IntersectionOfLine(p1, p2, car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1) { |
| | | crossStartLine = true; |
| | | startPoint = car->basePoint; |
| | | edgeDistance = DistanceOf(car->basePoint, road_edge); |
| | | } |
| | | } else { |
| | | double runDistance; |
| | | runDistance = DistanceOf(car->basePoint, startPoint); |
| | | |
| | | if (runDistance < 100.0) { |
| | | if (!reportOffsetOver && fabs(DistanceOf(car->basePoint, road_edge) - edgeDistance) > 0.3) { |
| | | DEBUG("直线偏移大于30厘米"); |
| | | // 偏移大于30厘米,不合格 |
| | | AddExamFault(30, rtkTime); |
| | | reportOffsetOver = true; |
| | | } |
| | | } else { |
| | | startPoint = car->basePoint; |
| | | edgeDistance = DistanceOf(car->basePoint, road_edge); |
| | | reportOffsetOver = false; |
| | | } |
| | | } |
| | | |
| | | if (ExitSonArea(index, RoadMapList, car)) { |
| | | DEBUG("离开直线行驶区域"); |
| | | return -1; |
| | | } |
| | | |
| | | return index; |
| | | } |
New file |
| | |
| | | // |
| | | // Created by YY on 2020/3/23. |
| | | // |
| | | |
| | | #ifndef MYAPPLICATION2_DRIVE_STRAIGHT_H |
| | | #define MYAPPLICATION2_DRIVE_STRAIGHT_H |
| | | |
| | | #include "../driver_test.h" |
| | | |
| | | void StartDriveStraightExam(int index, LIST_ROAD_MAP &RoadMapList); |
| | | int ExecuteDriveStraightExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, |
| | | LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime); |
| | | |
| | | #endif //MYAPPLICATION2_DRIVE_STRAIGHT_H |
| | |
| | | |
| | | #define DEBUG(fmt, args...) LOGD("<dummy_light> <%s>: " fmt, __func__, ##args) |
| | | |
| | | enum { |
| | | TTS_NOT_START, |
| | | TTS_DOING, |
| | | TTS_DONE, |
| | | WAIT_OPERATE, |
| | | CHECK_OPERATE |
| | | }; |
| | | |
| | | static struct RtkTime currRtkTime; |
| | | static struct dummy_light_exam *content; |
| | | static int contentNum; |
| | | static int currContent; |
| | | |
| | | static int checkCnt; |
| | | static bool turn_left_active, flash_beam_active; |
| | | static int examTtsSeq = 0; |
| | | static bool testing; |
| | | |
| | | static void DummyLightCheckActive(union sigval sig); |
| | | static void ExamDummyLight(union sigval sig); |
| | |
| | | DEBUG("StartDummyLightExam"); |
| | | content = ptr; |
| | | contentNum = num; |
| | | currContent = 0; |
| | | |
| | | currRtkTime = *rtkTime; |
| | | |
| | | AppTimer_delete(DummyLightCheckActive); |
| | | for (int i = 0; i < contentNum; ++i) { |
| | | content[i].itemStatus = TTS_NOT_START; |
| | | } |
| | | testing = true; |
| | | AppTimer_delete(ExamDummyLight); |
| | | AppTimer_add(ExamDummyLight, D_SEC(2)); |
| | | } |
| | |
| | | int ExecuteDummyLightExam(const struct RtkTime* rtkTime) |
| | | { |
| | | currRtkTime = *rtkTime; |
| | | return (currContent == contentNum)?2:1; |
| | | return (testing)?1:2; |
| | | } |
| | | |
| | | void DummyLightTTSDone(int id) |
| | | { |
| | | DEBUG("DummyLightTTSDone %d", id); |
| | | // 等语音播报完毕后计时 |
| | | if (id == 100) { |
| | | AppTimer_add(DummyLightCheckActive, D_SEC(3), id); |
| | | } else if (id == 101) { |
| | | AppTimer_add(DummyLightCheckActive, D_SEC(1), id); |
| | | } else if (id == OVERTAKE) { |
| | | checkCnt = 0; |
| | | turn_left_active = flash_beam_active = false; |
| | | AppTimer_add(DummyLightCheckActive, D_SEC(1), id); |
| | | } else { |
| | | AppTimer_add(DummyLightCheckActive, D_SEC(5), id); |
| | | if (id == examTtsSeq) { |
| | | for (int i = 0; i < contentNum; ++i) { |
| | | if (content[i].itemStatus == TTS_DOING) { |
| | | content[i].itemStatus = TTS_DONE; |
| | | break; |
| | | } |
| | | } |
| | | |
| | | AppTimer_add(ExamDummyLight, 100); |
| | | } |
| | | } |
| | | |
| | | static void DummyLightCheckActive(union sigval sig) |
| | | { |
| | | AppTimer_delete(DummyLightCheckActive); |
| | | |
| | | DEBUG("DummyLightCheckActive %d", sig.sival_int); |
| | | DEBUG("DummyLightCheckActive item = %d", sig.sival_int); |
| | | |
| | | switch (sig.sival_int) { |
| | | case DRIVE_AT_NIGHT: |
| | |
| | | break; |
| | | } |
| | | |
| | | AppTimer_add(ExamDummyLight, D_SEC(1)); |
| | | for (int i = 0; i < contentNum; ++i) { |
| | | if (content[i].item == sig.sival_int) { |
| | | content[i].itemStatus = CHECK_OPERATE; |
| | | break; |
| | | } |
| | | } |
| | | |
| | | AppTimer_add(ExamDummyLight, 500); |
| | | } |
| | | |
| | | static void ExamDummyLight(union sigval sig) |
| | | { |
| | | int i = 0; |
| | | AppTimer_delete(ExamDummyLight); |
| | | |
| | | if (currContent < contentNum) { |
| | | DEBUG("模拟灯光测试 %s", content[currContent].tts); |
| | | for (; i < contentNum; ++i) { |
| | | switch (content[i].itemStatus) { |
| | | case TTS_NOT_START: |
| | | content[i].itemStatus = TTS_DOING; |
| | | examTtsSeq = PlayTTS(content[i].tts); |
| | | // 等待TTS播放完毕 |
| | | return; |
| | | case TTS_DOING: |
| | | return; |
| | | case TTS_DONE: |
| | | content[i].itemStatus = WAIT_OPERATE; |
| | | |
| | | PlayTTS(content[currContent].tts, content[currContent].item); |
| | | currContent++; |
| | | AppTimer_delete(DummyLightCheckActive); |
| | | if (content[i].item == OVERTAKE) { |
| | | checkCnt = 0; |
| | | turn_left_active = flash_beam_active = false; |
| | | AppTimer_add(DummyLightCheckActive, D_SEC(1), content[i].item); |
| | | } |
| | | else if (content[i].item >= 100) |
| | | AppTimer_add(DummyLightCheckActive, D_SEC(3), content[i].item); |
| | | else |
| | | AppTimer_add(DummyLightCheckActive, D_SEC(5), content[i].item); |
| | | return; |
| | | case WAIT_OPERATE: |
| | | return; |
| | | case CHECK_OPERATE: |
| | | default: |
| | | break; |
| | | } |
| | | } |
| | | |
| | | if (i >= contentNum) { |
| | | testing = false; |
| | | } |
| | | } |
| | |
| | | |
| | | struct dummy_light_exam { |
| | | int item; |
| | | int itemStatus; |
| | | char tts[512]; |
| | | }; |
| | | |
| | |
| | | #include "../native-lib.h" |
| | | #include "through_something.h" |
| | | #include "../master/comm_if.h" |
| | | #include "drive_straight.h" |
| | | |
| | | #include <vector> |
| | | #include <list> |
| | |
| | | using namespace std; |
| | | |
| | | #define TURN_CHECK_CNT 4 |
| | | |
| | | enum { |
| | | START_CAR_NOT_DO, |
| | | START_CAR_DOING, |
| | | START_CAR_DONE |
| | | }; |
| | | |
| | | static const int TURN_THRESHOLD = 5; |
| | | static const int TURN_CHECK_INTERVAL = 500; |
| | |
| | | static int prevMoveDirect; |
| | | static uint32_t stopTimepoint = 0; |
| | | static bool reportStopCarOnRedArea; |
| | | static PointF stopPoint; |
| | | static PointF stopPoint, startPoint; |
| | | static bool prevGearError = false; |
| | | static bool prevGearNSlide = false; |
| | | |
| | |
| | | static int gearErrorTime; |
| | | static int gearNSlideTime; |
| | | |
| | | static int startCar; |
| | | static int currExamMapIndex; |
| | | |
| | | static const int MAX_ENGINE_RPM = 2500; |
| | | static const double START_CAR_MOVE_DISTANCE = 0.5;//10.0; |
| | | static const double START_CAR_CHECK_DOOR_DISTANCE = 0.1;//1.0; |
| | | static const uint32_t GEAR_N_SLIDE_TIMEOUT = D_SEC(5); |
| | | static const uint32_t GEAR_ERROR_TIMEOUT = D_SEC(15); |
| | | static const uint32_t STOP_CAR_TIME = D_SEC(2); |
| | |
| | | static const double MAX_SPEED = 40.0 * 1000.0 / 3600.0; |
| | | static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}}; |
| | | |
| | | static void TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime); |
| | | static void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime); |
| | | static char isTurn(int currYaw, int prevYaw); |
| | | static char isTurn(int currYaw, int prevYaw, int &ang); |
| | | static char CheckCarTurn(LIST_CAR_MODEL &CarModelList); |
| | | static bool CrashRedLine(LIST_ROAD_MAP &RoadMapList, const car_model *car); |
| | | static bool CrashRedArea(LIST_ROAD_MAP &RoadMapList, const car_model *car); |
| | |
| | | gearNSlideTime = 0; |
| | | |
| | | currExamMapIndex = -1; |
| | | |
| | | startCar = START_CAR_NOT_DO; |
| | | } |
| | | |
| | | static void TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime) |
| | | { |
| | | double moveDistance; |
| | | static bool checkDoor = false; |
| | | static bool handBreakActive = false; |
| | | static bool reportRPMOver = false; |
| | | |
| | | if (startCar == START_CAR_NOT_DO) { |
| | | startPoint = car->basePoint; |
| | | reportRPMOver = false; |
| | | startCar = START_CAR_DOING; |
| | | |
| | | PlayTTS("请起步"); |
| | | } else if (startCar == START_CAR_DOING) { |
| | | moveDistance = DistanceOf(startPoint, car->basePoint); |
| | | if (moveDistance > START_CAR_MOVE_DISTANCE) { |
| | | |
| | | if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) { |
| | | DEBUG("Handbreak active move over 10m"); |
| | | // 手刹拉起状态下,行驶了10米以上,不合格 |
| | | AddExamFault(25, rtkTime); |
| | | } else if (handBreakActive) { |
| | | // 手刹拉起状态下,行驶了1米以上,扣10分 |
| | | DEBUG("Handbreak active move over 1M"); |
| | | AddExamFault(26, rtkTime); |
| | | } |
| | | startCar = START_CAR_DONE; |
| | | } else if (moveDistance >= START_CAR_CHECK_DOOR_DISTANCE) { |
| | | if (!checkDoor) { |
| | | checkDoor = true; |
| | | |
| | | if (ReadCarStatus(DOOR) == DOOR_OPEN) { |
| | | // 车门未完全关闭,不合格 |
| | | DEBUG("车门未关闭"); |
| | | AddExamFault(23, rtkTime); |
| | | } |
| | | |
| | | if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) { |
| | | handBreakActive = true; |
| | | } |
| | | } |
| | | } |
| | | |
| | | if (ReadCarStatus(ENGINE_RPM) > MAX_ENGINE_RPM && !reportRPMOver) { |
| | | // 转速超标,不合格 |
| | | DEBUG("转速超标"); |
| | | AddExamFault(29, rtkTime); |
| | | reportRPMOver = true; |
| | | } |
| | | } else { |
| | | |
| | | } |
| | | } |
| | | |
| | | void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) |
| | | { |
| | | // 起步检测 |
| | | TestRoadStartCar(car, speed, moveDirect, rtkTime); |
| | | |
| | | // 超速检测 |
| | | if (moveDirect != 0 && speed > MAX_SPEED) { |
| | | if (!occurOverSpeed) { |
| | |
| | | // 检查是否持续转向 |
| | | char turnDirect = CheckCarTurn(CarModelList); |
| | | if (turnDirect == 'L') { |
| | | PlayTTS("左1", 5692); |
| | | // PlayTTS("左1"); |
| | | if (currTurnSignalStatus != LEFT_TURN_LIGHT) { |
| | | if (!reportTurnSignalError) { |
| | | DEBUG("没打左转灯"); |
| | |
| | | } |
| | | } |
| | | } else if (turnDirect == 'R') { |
| | | PlayTTS("右1", 5692); |
| | | // PlayTTS("右1"); |
| | | if (currTurnSignalStatus != RIGHT_TURN_LIGHT) { |
| | | if (!reportTurnSignalError) { |
| | | DEBUG("没打右转灯"); |
| | |
| | | |
| | | // 检查变道前,是否提前转向灯 |
| | | if (inter == 1) { |
| | | PlayTTS("左2", 5698); |
| | | // PlayTTS("左2"); |
| | | // 向左侧变道 |
| | | DEBUG("向左侧变道"); |
| | | if (turnSignalStatusWhenCrashGreenLine != LEFT_TURN_LIGHT) { |
| | |
| | | AddExamFault(13, rtkTime); |
| | | } |
| | | } else { |
| | | PlayTTS("右2", 5698); |
| | | // PlayTTS("右2"); |
| | | // 向右侧变道 |
| | | DEBUG("向右侧变道"); |
| | | if (turnSignalStatusWhenCrashGreenLine != RIGHT_TURN_LIGHT) { |
| | |
| | | checkCrashGreenTimeout = 0; |
| | | } |
| | | |
| | | // 触发线检测 |
| | | if (currExamMapIndex == -1) { |
| | | // 完成起步后,触发线检测 |
| | | if (currExamMapIndex == -1 && startCar == START_CAR_DONE) { |
| | | currExamMapIndex = CrashTriggerLine(RoadMapList, car, CarModelList); |
| | | if (currExamMapIndex != -1) { |
| | | DEBUG("碰撞触发线"); |
| | | |
| | | MA_EnterMap(RoadMapList[currExamMapIndex].id, RoadMapList[currExamMapIndex].type, 1); |
| | | |
| | | if (RoadMapList[currExamMapIndex].type >= THROUGH_INTERSECTION_MAP && |
| | | RoadMapList[currExamMapIndex].type <= TURN_AROUND_MAP) { |
| | | StartThroughExam(currExamMapIndex, RoadMapList); |
| | | } else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) { |
| | | StartDriveStraightExam(currExamMapIndex, RoadMapList); |
| | | } |
| | | } else { |
| | | } |
| | | } else if (startCar == START_CAR_DONE) { |
| | | int prevIdx = currExamMapIndex; |
| | | |
| | | if (currExamMapIndex >= THROUGH_INTERSECTION_MAP && currExamMapIndex <= TURN_AROUND_MAP) { |
| | | currExamMapIndex = ExecuteThroughExam(currExamMapIndex, RoadMapList, car, |
| | | CarModelList, speed, moveDirect, rtkTime); |
| | | } |
| | | else if (currExamMapIndex == DRIVE_STRAIGHT_MAP) { |
| | | currExamMapIndex = ExecuteDriveStraightExam(currExamMapIndex, RoadMapList, car, |
| | | CarModelList, speed, moveDirect, rtkTime); |
| | | } |
| | | |
| | | if (currExamMapIndex == -1) { |
| | | MA_EnterMap(RoadMapList[prevIdx].id, RoadMapList[prevIdx].type, 1); |
| | |
| | | tm.msec = rtkTime->mss; |
| | | } |
| | | |
| | | static char isTurn(int currYaw, int prevYaw) |
| | | static char isTurn(int currYaw, int prevYaw, int &ang) |
| | | { |
| | | // DEBUG("currYaw %d prevYaw %d", currYaw, prevYaw); |
| | | |
| | | int deltaAng = 0; |
| | | |
| | | if (ABS(currYaw - prevYaw) > 180) { |
| | |
| | | } else { |
| | | deltaAng = ABS(currYaw - prevYaw); |
| | | } |
| | | |
| | | ang = deltaAng; |
| | | |
| | | // DEBUG("角度差值 %d", deltaAng); |
| | | |
| | |
| | | } |
| | | } |
| | | |
| | | return 0; |
| | | return 'N'; |
| | | } |
| | | |
| | | static char CheckCarTurn(LIST_CAR_MODEL &CarModelList) |
| | | { |
| | | // 最近3秒内,每秒的角度差大于10度,且方向相同,连续3秒,认为转向 |
| | | // 最近2秒内,每0.5秒的角度差大于5度,且方向相同,连续4次;或突现超30度的转向;认为转向。 |
| | | if (CarModelList.size() < 1) |
| | | return false; |
| | | |
| | |
| | | uint32_t tdiff = TimeGetDiff(c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10); |
| | | |
| | | if (tdiff >= TURN_CHECK_INTERVAL) { |
| | | turn[checkCnt] = isTurn((int)c1->yaw, (int)c2->yaw); |
| | | int ang = 0; |
| | | turn[checkCnt] = isTurn((int)c1->yaw, (int)c2->yaw, ang); |
| | | // DEBUG("%c 角度比较 %02d:%02d:%02d.%03d %02d:%02d:%02d.%03d", turn[checkCnt], c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10); |
| | | if (turn[checkCnt] == 0) { |
| | | |
| | | if (turn[checkCnt] == 'N') { |
| | | break; |
| | | } else if (ang >= 30) { |
| | | DEBUG("左右转确认 %c", turn[checkCnt]); |
| | | return turn[checkCnt]; |
| | | } |
| | | |
| | | c1 = c2; |
| | |
| | | } |
| | | return -1; |
| | | } |
| | | |
| | | |
| | |
| | | DEBUG("进入路考通过something地图 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type); |
| | | if (!RoadMapList[index].tts.empty()) { |
| | | DEBUG("播放TTS"); |
| | | PlayTTS(RoadMapList[index].tts.c_str(), 0); |
| | | PlayTTS(RoadMapList[index].tts.c_str()); |
| | | } else { |
| | | DEBUG("没有TTS"); |
| | | } |