lib/src/main/cpp/CMakeLists.txt
@@ -46,6 +46,7 @@ test_items2/stop_car.cpp test_items2/operate_gear.cpp test_items2/smart_item.cpp test_items2/car_start.cpp rtk_module/rtk.cpp rtk_module/virtual_rtk.cpp lib/src/main/cpp/driver_test.cpp
@@ -578,7 +578,7 @@ char buff[128]; sprintf(buff, "%s，%s", NAME[i], VALUE[ cs_temp[i] ]); PlayTTS(buff); PlayTTS(buff, NULL); } }
@@ -108,14 +108,6 @@ #define BUS_STATION_AREA 2 #define GRID_AREA 3 #define ROAD_ITEM_NONE 0 #define ROAD_ITEM_CHANGE_LANE 1 #define ROAD_ITEM_OVERTAKE 2 #define ROAD_ITEM_STRAIGHT 3 #define ROAD_ITEM_OPERATE_GEAR 4 #define ROAD_ITEM_START_CAR 5 #define ROAD_ACTIVE_FORWARD LANE_FORWARD #define ROAD_ACTIVE_TURN_LEFT LANE_LEFT #define ROAD_ACTIVE_TURN_RIGHT LANE_RIGHT lib/src/main/cpp/native-lib.cpp
@@ -3,6 +3,7 @@ #include <pthread.h> #include <unistd.h> #include <cstdlib> #include <map> #include "common/serial_port.h" #include "jni_log.h" #include "common/net.h" @@ -34,6 +35,8 @@ static int ttsSeq = 1; static void SendBootIndicate(union sigval sig); static std::map<int, void (*)(int)> TTSCallBack; int DESEncrypt(const uint8_t *key, int key_length, const uint8_t *plaintext, int plaintext_length, @@ -210,7 +213,7 @@ return seq; } int PlayTTS(const char *string) int PlayTTS(const char *string, void (*callback)(int)) { DEBUG("PlayTTS: %s", string); @@ -242,6 +245,8 @@ LOGE("%s: DetachCurrentThread() failed", __FUNCTION__); } } TTSCallBack.insert(pair<int, void (*)(int)>(id, callback)); return id; } @@ -314,5 +319,17 @@ JNIEXPORT void JNICALL Java_com_anyun_exam_lib_RemoteService_TextSpeakEnd(JNIEnv *env, jobject thiz, jint id) { // TODO: implement TextSpeakEnd() PlatformStatusChanged(PLAY_TTS_DONE_EVT, (uint8_t *)&id, sizeof(id)); auto it = TTSCallBack.find(id); if (it != TTSCallBack.end()) { if (it->second != NULL) { tts_back_t tb; tb.seq = id; tb.callback = (void (*)(int)) it->second; PlatformStatusChanged(PLAY_TTS_DONE_EVT, (uint8_t *)&tb, sizeof(tb)); } TTSCallBack.erase(it); } } lib/src/main/cpp/native-lib.h
@@ -8,6 +8,11 @@ #include <cstdint> #include "test_common/Geometry.h" typedef struct { int seq; void (*callback)(int); } tts_back_t; char * GetImei(void); int DESEncrypt(const uint8_t *key, int key_length, @@ -18,6 +23,6 @@ void TextOsd(int type, const char *text); void DrawScreen(const Polygon *map, const Polygon *car); void SendMsgToMainProc(int cmd, const char *value); int PlayTTS(const char *string); int PlayTTS(const char *string, void (*callback)(int)); #endif //RTKBASESTATION_NATIVE_LIB_H lib/src/main/cpp/rtk_platform/platform.cpp
@@ -508,10 +508,10 @@ MA_SendCardBrief(&brief); } if (events & PLAY_TTS_DONE_EVT) { DummyLightTTSDone(*((int *)data)); StopCarTTSDone(*((int *)data)); OperateGearTTSDone(*((int *)data)); DriveStraightTTSDone(*((int *)data)); tts_back_t *cb = (tts_back_t *) data; if (cb->callback != NULL) { cb->callback(cb->seq); } } }
@@ -110,9 +110,9 @@ AddExamFault(27, rtkTime); DEBUG("车轮压边线"); if (who == 1) { PlayTTS("压左曲线"); PlayTTS("压左曲线", NULL); } else if (who == 2) { PlayTTS("压右曲线"); PlayTTS("压右曲线", NULL); } } } else { lib/src/main/cpp/test_items/park_bottom.cpp
@@ -115,9 +115,9 @@ AddExamFault(7, rtkTime); DEBUG("车轮压线"); if (who == 1) { PlayTTS("压左库位线"); PlayTTS("压左库位线", NULL); } else if (who == 2) { PlayTTS("压右库位线"); PlayTTS("压右库位线", NULL); } } } else { lib/src/main/cpp/test_items2/car_start.cpp
New file @@ -0,0 +1,107 @@ // // Created by YY on 2020/8/20. // #include "car_start.h" #include "../driver_test.h" #include "../test_common/odo_graph.h" #include "../native-lib.h" #include "../test_common/car_sensor.h" #include "road_exam.h" #include "../jni_log.h" #include "../common/apptimer.h" #define DEBUG(fmt, args...) LOGD("<car_start> <%s>: " fmt, __func__, ##args) static const int MAX_ENGINE_RPM = 2500; static const double START_CAR_MOVE_DISTANCE = 10.0; static const double START_CAR_CHECK_DOOR_DISTANCE = 1.0; static double startCarMoveDistance; static bool checkEngineRPM, checkStartCarSignal, checkDoor, handBreakActive; void cb(int seq) { DEBUG("语音结束 %d", seq); } void CarStartInit(void) { startCarMoveDistance = ReadOdo(); checkEngineRPM = false; checkStartCarSignal = false; checkDoor = false; handBreakActive = false; int id = PlayTTS("请起步", cb); DEBUG("语音开始 %d", id); } bool TestCarStart(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime) { car_sensor_value_t sensor; double moveDistance = ReadOdo() - startCarMoveDistance; DEBUG("起步行驶距离 %f", moveDistance); if (!checkStartCarSignal && moveDirect == 1) { checkStartCarSignal = true; sensor = ReadCarSensorValue(TURN_SIGNAL_LAMP); if (sensor.name == TURN_SIGNAL_LAMP) { if (sensor.value != LEFT_TURN_LIGHT) { DEBUG("变调未打灯!!"); // 没打灯，不合格 AddExamFault(13, rtkTime); } else if (TimeGetDiff(rtkTime, &sensor.time) >= D_SEC(3)) { DEBUG("转向灯时间不足"); // 不足3秒，不合格 AddExamFault(14, rtkTime); } } } if (moveDistance > START_CAR_MOVE_DISTANCE) { sensor = ReadCarSensorValue(HAND_BREAK); if (sensor.name == HAND_BREAK && sensor.value == 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); } PlayTTS("完成起步", NULL); DEBUG("############# 完成起步 ############"); return false; } else if (moveDistance >= START_CAR_CHECK_DOOR_DISTANCE) { if (!checkDoor) { checkDoor = true; sensor = ReadCarSensorValue(DOOR); if (sensor.name == DOOR && sensor.value == DOOR_OPEN) { // 车门未完全关闭，不合格 DEBUG("车门未关闭"); AddExamFault(23, rtkTime); } sensor = ReadCarSensorValue(HAND_BREAK); if (sensor.name == HAND_BREAK && sensor.value == BREAK_ACTIVE) { handBreakActive = true; } } } if (ReadCarStatus(ENGINE_RPM) > MAX_ENGINE_RPM && !checkEngineRPM) { // 转速超标，不合格 DEBUG("转速超标"); AddExamFault(29, rtkTime); checkEngineRPM = true; } return true; } lib/src/main/cpp/test_items2/car_start.h
New file @@ -0,0 +1,13 @@ // // Created by YY on 2020/8/20. // #ifndef MYAPPLICATION2_CAR_START_H #define MYAPPLICATION2_CAR_START_H #include "../driver_test.h" void CarStartInit(void); bool TestCarStart(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime); #endif //MYAPPLICATION2_CAR_START_H lib/src/main/cpp/test_items2/drive_straight.cpp
@@ -31,9 +31,9 @@ ttsPlayEnd = 0; if (!tts.empty()) { examTtsSeq = PlayTTS(tts.c_str()); examTtsSeq = PlayTTS(tts.c_str(), NULL); } else { examTtsSeq = PlayTTS("请保持直线行驶"); examTtsSeq = PlayTTS("请保持直线行驶", NULL); } distanceToStartSum = 0; @@ -88,7 +88,7 @@ if (distanceToStart + distanceToStartSum > 105) { DEBUG("离开直线行驶区域"); PlayTTS("直线行驶结束"); PlayTTS("直线行驶结束", NULL); return -1; } return 1; lib/src/main/cpp/test_items2/dummy_light.cpp
@@ -172,7 +172,7 @@ switch (content[i].itemStatus) { case TTS_NOT_START: content[i].itemStatus = TTS_DOING; examTtsSeq = PlayTTS(content[i].tts); examTtsSeq = PlayTTS(content[i].tts, NULL); // 等待TTS播放完毕 return; case TTS_DOING: lib/src/main/cpp/test_items2/operate_gear.cpp
@@ -8,155 +8,127 @@ #include "../native-lib.h" #include "../test_common/car_sensor.h" #include "../jni_log.h" #include "../test_common/odo_graph.h" #include "road_exam.h" #define DEBUG(fmt, args...) LOGD("<operate_gear> <%s>: " fmt, __func__, ##args) static struct RtkTime currRtkTime; static const int MAX_READ_GEAR = 6; static int opGear; static bool testing; static int examTtsSeq = 0; static int expectGear; static int upDownShift; static int readGearCnt; static int opCnt = 0; enum { SEND_OP_INS, CHECK_OP }; static int setup; static void CheckGear(union sigval sig); static double maxMoveDistance; static double gearMoveDistance; void StartOperateGearExam(const struct RtkTime *rtkTime) { static void TtsBack(int seq) { maxMoveDistance = ReadOdo(); setup = 1; } void StartOperateGearExam(void) { DEBUG("开始加减挡操作"); testing = true; opGear = SEND_OP_INS; readGearCnt = 0; upDownShift = 0; opCnt = 0; currRtkTime = *rtkTime; setup = 0; AppTimer_delete(CheckGear); AppTimer_add(CheckGear, 100); PlayTTS("请进行加减挡位操作", TtsBack); } void TerminateOperateGearExam(void) bool TestOperateGear(const struct RtkTime *rtkTime) { AppTimer_delete(CheckGear); } car_sensor_value_t sensor = ReadCarSensorValue(GEAR); static void CheckGear(union sigval sig) { AppTimer_delete(CheckGear); if (sensor.name != GEAR) return false; if (opGear == SEND_OP_INS) { readGearCnt++; switch (ReadCarStatus(GEAR)) { if (setup == 0) { return true; } else if (setup == 1) { switch (sensor.value) { case GEAR_1: { upDownShift = 1; expectGear = GEAR_2; examTtsSeq = PlayTTS("请加到二挡"); upDownShift = 1; PlayTTS("请加到二挡", NULL); setup = 2; break; } case GEAR_2: { if (upDownShift == 0) { upDownShift = 1; expectGear = GEAR_3; examTtsSeq = PlayTTS("请加到三挡"); } else if (upDownShift == 1) { upDownShift = -1; expectGear = GEAR_1; examTtsSeq = PlayTTS("请减到一挡"); } else { upDownShift = 1; expectGear = GEAR_3; examTtsSeq = PlayTTS("请加到三挡"); } PlayTTS("请加到三挡", NULL); setup = 2; break; } case GEAR_3: { if (upDownShift == 0) { upDownShift = -1; expectGear = GEAR_2; examTtsSeq = PlayTTS("请减到二挡"); } else if (upDownShift == 1) { upDownShift = -1; expectGear = GEAR_2; examTtsSeq = PlayTTS("请减到二挡"); } else { upDownShift = 1; expectGear = GEAR_4; examTtsSeq = PlayTTS("请加到四挡"); } PlayTTS("请减到二挡", NULL); setup = 2; break; } case GEAR_4: { if (upDownShift == 0) { upDownShift = -1; expectGear = GEAR_3; examTtsSeq = PlayTTS("请减到三挡"); } else if (upDownShift == 1) { upDownShift = -1; expectGear = GEAR_3; examTtsSeq = PlayTTS("请减到三挡"); } else { upDownShift = 1; expectGear = GEAR_5; examTtsSeq = PlayTTS("请加到五挡"); } PlayTTS("请减到三挡", NULL); setup = 2; break; } case GEAR_5: { upDownShift = -1; expectGear = GEAR_4; examTtsSeq = PlayTTS("请减到四挡"); PlayTTS("请减到四挡", NULL); setup = 2; break; } default: // 未读到有效挡位，继续尝试 if (readGearCnt > MAX_READ_GEAR) { DEBUG("总是读到空挡或无效挡位"); AddExamFault(31, &currRtkTime); testing = false; } else { AppTimer_add(CheckGear, 500); } break; } } else { DEBUG("检测挡位 %d", expectGear); opCnt++; // 检查挡位 if (ReadCarStatus(GEAR) != expectGear) { } else if (setup == 2) { if (sensor.value == GEAR_N) { } else if (sensor.value != expectGear) { // 未按指令操作挡位，不合格 DEBUG("未按指令操作挡位"); AddExamFault(31, &currRtkTime); testing = false; } else if (opCnt < 2) { opGear = SEND_OP_INS; readGearCnt = 0; AppTimer_add(CheckGear, 500); AddExamFault(31, rtkTime); return false; } else { testing = false; // 在此挡位行驶一定距离，再执行下一个 gearMoveDistance = ReadOdo(); setup = 3; } } else if (setup == 3) { if (ReadOdo() - gearMoveDistance > 10) { setup = 4; char buff[128]; expectGear += 0 - upDownShift; sprintf(buff, "请%s到%d挡", upDownShift > 0 ? "减": "加", expectGear - GEAR_N); } } else if (setup == 4) { if (sensor.value == GEAR_N) { } else if (sensor.value != expectGear) { // 未按指令操作挡位，不合格 DEBUG("未按指令操作挡位"); AddExamFault(31, rtkTime); return false; } else { // 在此挡位行驶一定距离，再执行下一个 gearMoveDistance = ReadOdo(); setup = 5; } } else if (setup == 5) { if (ReadOdo() - gearMoveDistance > 10) { PlayTTS("加减挡位操作结束", NULL); return false; } } int ExecuteOperateGearExam(const struct RtkTime *rtkTime) { currRtkTime = *rtkTime; return testing ? 1 : -1; if (ReadOdo() - maxMoveDistance > 120) { // 未按指令操作挡位，不合格 DEBUG("未按指令操作挡位"); AddExamFault(31, rtkTime); return false; } void OperateGearTTSDone(int id) { // 等语音播报完毕后计时 if (id == examTtsSeq) { DEBUG("升降挡指令发出完毕 %d", id); opGear = CHECK_OP; AppTimer_add(CheckGear, D_SEC(7)); } return true; } lib/src/main/cpp/test_items2/operate_gear.h
@@ -7,9 +7,7 @@ #include "../driver_test.h" void StartOperateGearExam(const struct RtkTime *rtkTime); int ExecuteOperateGearExam(const struct RtkTime *rtkTime); void OperateGearTTSDone(int id); void TerminateOperateGearExam(void); void StartOperateGearExam(void); bool TestOperateGear(const struct RtkTime *rtkTime); #endif //MYAPPLICATION2_OPERATE_GEAR_H lib/src/main/cpp/test_items2/road_exam.cpp
@@ -16,6 +16,7 @@ #include "stop_car.h" #include "operate_gear.h" #include "../test_common/odo_graph.h" #include "car_start.h" #include <cmath> #include <vector> @@ -62,13 +63,6 @@ PointF point; } projection_t; typedef struct { int name; int value; struct RtkTime time; } car_sensor_value_t; typedef struct { int gain; struct RtkTime time; @@ -83,6 +77,7 @@ static const int INVALID_ROAD = -1; static const int CROSSING_TURN_THRESHOLD = 45; static const int TURN_THRESHOLD = 3; static const int TURN_CHECK_INTERVAL = 500; const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3; @@ -107,14 +102,13 @@ static int prevMoveDirect; static uint32_t stopTimepoint = 0; static bool reportStopCarOnRedArea; static PointF stopPoint, startPoint; static PointF stopPoint; static bool prevGearError = false; static bool prevGearNSlide = false; static bool slideLongDistance; static bool slideNormalDistance; static bool occurSlide; static bool startCarLeftTurnSignal, checkStartCarSignal; static struct RtkTime crashGreenRunTime, crashGreenStartTime; @@ -126,16 +120,13 @@ static int gearErrorTime; static int gearNSlideTime; static int startCar, stopCar; static int stopCar; static int currExamMapIndex; static trigger_line_t *currRoadItem; static PointF roadItemStartPoint; static struct drive_timer roadItemStartTime; static bool overtake = false; static bool checkTurn = false; static bool checkDoor = false; static bool handBreakActive = false; static bool reportRPMOver = false; static lane_t Lane; static change_lane_t ChangeLane; @@ -146,14 +137,32 @@ static map<int, int> CrossingHint; static map<int, bool> ErrorLaneReport; #define ROAD_EXAM_READY_NEXT 0 #define ROAD_EXAM_FREE_RUN 1 #define ROAD_EXAM_ITEM_CAR_START 2 #define ROAD_EXAM_ITEM_STRAIGHT 3 #define ROAD_EXAM_ITEM_OP_GEAR 4 #define ROAD_EXAM_ITEM_CHANGE_LANE 5 #define ROAD_EXAM_ITEM_OVER_TAKE 6 #define ROAD_EXAM_ITEM_CAR_STOP 7 #define ROAD_EXAM_ITEM_NOT_EXEC 0 #define ROAD_EXAM_ITEM_EXECING 1 #define ROAD_EXAM_ITEM_EXECED 2 typedef struct { int name; int status; } RoadExamItem_t; static map<int, int> RoadExamItem; static int RoadExamStatus; 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; static const double START_CAR_CHECK_DOOR_DISTANCE = 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); @@ -170,8 +179,7 @@ static const double MAX_SPEED = 60.0 * 1000.0 / 3600.0; static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}}; static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime); static void ItemExam(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end); 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); @@ -212,16 +220,9 @@ currExamMapIndex = -1; startCar = START_CAR_NOT_DO; stopCar = STOP_CAR_NOT_DO; currRoadItem = NULL; checkDoor = false; handBreakActive = false; reportRPMOver = false; checkStartCarSignal = startCarLeftTurnSignal = false; Lane.road = Lane.sep = Lane.no = -1; Lane.guide = 0; @@ -234,14 +235,23 @@ ResetOdo(); ResetTarget(RoadMap); // 初始化考项 RoadExamItem.clear(); RoadExamItem[ROAD_EXAM_ITEM_CAR_START] = ROAD_EXAM_ITEM_NOT_EXEC; RoadExamItem[ROAD_EXAM_ITEM_STRAIGHT] = ROAD_EXAM_ITEM_NOT_EXEC; RoadExamItem[ROAD_EXAM_ITEM_OP_GEAR] = ROAD_EXAM_ITEM_NOT_EXEC; RoadExamItem[ROAD_EXAM_ITEM_CHANGE_LANE] = ROAD_EXAM_ITEM_NOT_EXEC; RoadExamItem[ROAD_EXAM_ITEM_OVER_TAKE] = ROAD_EXAM_ITEM_NOT_EXEC; RoadExamItem[ROAD_EXAM_ITEM_CAR_STOP] = ROAD_EXAM_ITEM_NOT_EXEC; RoadExamStatus = ROAD_EXAM_READY_NEXT; } void TerminateRoadExam(void) { TerminateDummyLightExam(); TerminateStopCarExam(); TerminateOperateGearExam(); TerminateDriveStraightExam(); // TerminateDummyLightExam(); // TerminateStopCarExam(); // TerminateOperateGearExam(); // TerminateDriveStraightExam(); } /********************************************************************* @@ -821,7 +831,7 @@ } } static car_sensor_value_t ReadCarSensorValue(int name) car_sensor_value_t ReadCarSensorValue(int name) { car_sensor_value_t nv = {.name = -1}; @@ -843,78 +853,6 @@ WriteCarSensorValue(SECOND_BREAK, ReadCarStatus(SECOND_BREAK), rtkTime); WriteCarSensorValue(GEAR, ReadCarStatus(GEAR), rtkTime); WriteCarSensorValue(BREAK, ReadCarStatus(BREAK), rtkTime); } static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime) { double moveDistance; 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); DEBUG("起步行驶距离 %f", moveDistance); if (!startCarLeftTurnSignal && ReadCarStatus(TURN_SIGNAL_LAMP) == LEFT_TURN_LIGHT) { startCarLeftTurnSignal = true; Rtk2DriveTimer(startCarLeftTurnSignalTime, rtkTime); } if (!checkStartCarSignal && moveDirect == 1) { checkStartCarSignal = true; if (!startCarLeftTurnSignal) { AddExamFault(13, rtkTime); } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10, startCarLeftTurnSignalTime.hour, startCarLeftTurnSignalTime.min, startCarLeftTurnSignalTime.sec, startCarLeftTurnSignalTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) { AddExamFault(14, rtkTime); } } 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); } PlayTTS("完成起步"); DEBUG("############# 完成起步 ############"); 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 { } return startCar; } static void DetectLine(int roadIndex, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, int moveDirect, const struct RtkTime *rtkTime) @@ -1209,7 +1147,7 @@ if (GetCrossingStatus(roadIndex, stopIndex) == CROSSING_NOT_HINT) { if (!RoadMap.roads[roadIndex].stopLine[stopIndex].tts.empty()) { DEBUG("路口提示 %s", RoadMap.roads[roadIndex].stopLine[stopIndex].tts.c_str()); PlayTTS(RoadMap.roads[roadIndex].stopLine[stopIndex].tts.c_str()); PlayTTS(RoadMap.roads[roadIndex].stopLine[stopIndex].tts.c_str(), NULL); } ChangeCrossingStatus(roadIndex, stopIndex, CROSSING_HAS_HINT); } @@ -1273,31 +1211,7 @@ void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) { // uint32_t cts = AppTimer_GetTickCount(); // int ri = CalcRoadIndex(-1, RoadMap, car); // bool crash = CrashRedLine(CrashLineType, 0, RoadMap, car, CarModelList); // lane_t laneInfo; // double redist = -1; // // laneInfo.road = -1; // laneInfo.sep = -1; // laneInfo.no = -1; // // if (ri >= 0) { // GetLane(laneInfo, car->carXY[car->axial[AXIAL_FRONT]], RoadMap, ri); // // int m = RoadMap.roads[ri].rightEdge.size(); // int n = RoadMap.roads[ri].rightEdge[m-1].points.size(); // // PointF base; // // base.X = 428922.2985; base.Y = 3292119.5457; // // redist = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], base, // RoadMap.roads[ri].rightEdge); // } // // DEBUG("当前道路索引 %d, 触发红线 %d lane %d 距离 %f %ld", ri, crash, laneInfo.no, redist, AppTimer_GetTickCount() - cts); double BigStraightRoadFree = 0, RoadCrossingFree = 0; UpdateCarSensor(rtkTime); @@ -1504,7 +1418,7 @@ } if (currExamMapIndex >= 0 && Lane.guide == 0) { double BigStraightRoad = AnalysisRoad(RoadMap, currExamMapIndex, Lane, car); BigStraightRoadFree = AnalysisRoad(RoadMap, currExamMapIndex, Lane, car); road_end_point_t ep = NearbyRoadEndPoint(currExamMapIndex, RoadMap, car); @@ -1517,88 +1431,17 @@ freeSepDis = ep.distance; } DEBUG("直道剩余距离 %f, 车道剩余距离 %f", BigStraightRoad, freeSepDis); RoadCrossingFree = freeSepDis; if (startCar == START_CAR_DONE) { if (itemExec[0] == 1 || itemExec[1] == 1 || itemExec[2] == 1 || itemExec[3] == 1) { DEBUG("项目执行距离<%d %d %d %d> %f", itemExec[0], itemExec[1], itemExec[2], itemExec[3], ReadOdo() - odo); if (ReadOdo() - odo > 120) { odo = ReadOdo(); if (itemExec[0] == 1) { itemExec[0] = 2; } if (itemExec[1] == 1) { itemExec[1] = 2; } if (itemExec[2] == 1) { itemExec[2] = 2; } if (itemExec[3] == 1) { itemExec[3] = 2; } } goto BIG_DOG; } if (itemExec[0] == 2 || itemExec[1] == 2 || itemExec[2] == 2 || itemExec[3] == 2) { DEBUG("项目休息距离<%d %d %d %d> %f", itemExec[0], itemExec[1], itemExec[2], itemExec[3], ReadOdo() - odo); if (ReadOdo() - odo > 100) { if (itemExec[0] == 2) { itemExec[0] = 3; } if (itemExec[1] == 2) { itemExec[1] = 3; } if (itemExec[2] == 2) { itemExec[2] = 3; } if (itemExec[3] == 2) { itemExec[3] = 3; } } goto BIG_DOG; } if (BigStraightRoad > 170 && ep.distance > 170) { if (itemExec[0] == 0) { PlayTTS("二狗直线行驶"); itemExec[0] = 1; odo = ReadOdo(); } } else if (BigStraightRoad > 150 && ep.distance > 150) { if (itemExec[3] == 0) { PlayTTS("二狗加减档"); itemExec[3] = 1; odo = ReadOdo(); } } else if (freeSepDis > 150) { if (itemExec[2] == 0) { PlayTTS("二狗变道"); itemExec[2] = 1; odo = ReadOdo(); } else if (itemExec[1] == 0) { PlayTTS("二狗超车"); itemExec[1] = 1; odo = ReadOdo(); } } BIG_DOG:; } DEBUG("直道剩余距离 %f, 车道剩余距离 %f", BigStraightRoadFree, RoadCrossingFree); } // 检测压线状态 TestRoadStartCar(car, speed, moveDirect, rtkTime); // 额外的转向检测 DetectTurn(car, moveDirect, rtkTime); if (startCar != START_CAR_DONE) return; ItemExam(RoadMap, car, CarModelList, speed, moveDirect, rtkTime, BigStraightRoadFree, RoadCrossingFree); if (ReadOdo() > EXAM_RANGE && currRoadItem == NULL && AllCmp(RoadMap) && stopCar == STOP_CAR_NOT_DO) { // 在合适条件下停车结束考试 @@ -1661,6 +1504,63 @@ } } }*/ } static void ItemExam(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end) { static double freeRunDistance; static double totalRunDistance; if (RoadExamStatus == ROAD_EXAM_READY_NEXT) { if (RoadExamItem[ROAD_EXAM_ITEM_CAR_START] == ROAD_EXAM_ITEM_NOT_EXEC) { CarStartInit(); totalRunDistance = ReadOdo(); RoadExamStatus = ROAD_EXAM_ITEM_CAR_START; } else { if (RoadExamItem[ROAD_EXAM_ITEM_STRAIGHT] == ROAD_EXAM_ITEM_NOT_EXEC) { if (straight > 170 && road_end > 170) { } } if (RoadExamItem[ROAD_EXAM_ITEM_OP_GEAR] == ROAD_EXAM_ITEM_NOT_EXEC) { if (straight > 150 && road_end > 150) { StartOperateGearExam(); RoadExamStatus = ROAD_EXAM_ITEM_OP_GEAR; } } if (RoadExamItem[ROAD_EXAM_ITEM_CHANGE_LANE] == ROAD_EXAM_ITEM_NOT_EXEC) { } if (RoadExamItem[ROAD_EXAM_ITEM_OVER_TAKE] == ROAD_EXAM_ITEM_NOT_EXEC) { } } } else if (RoadExamStatus == ROAD_EXAM_FREE_RUN) { if (ReadOdo() - freeRunDistance > 300) { RoadExamStatus = ROAD_EXAM_READY_NEXT; } } else { bool testing = false; switch (RoadExamStatus) { case ROAD_EXAM_ITEM_CAR_START: testing = TestCarStart(car, speed, moveDirect, rtkTime); break; case ROAD_EXAM_ITEM_OP_GEAR: testing = TestOperateGear(rtkTime); break; default:break; } if (!testing) { RoadExamItem[RoadExamStatus] = ROAD_EXAM_ITEM_EXECED; RoadExamStatus = ROAD_EXAM_FREE_RUN; freeRunDistance = ReadOdo(); } } } void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime) @@ -1755,7 +1655,7 @@ turnTimeCnt += TimeGetDiff(rtkTime, &prevDetectTurnTime); int wise = isTurn((int) car->yaw, startTurnYaw, TURN_THRESHOLD); DEBUG("转动角度 %d", wise); if (ABS(wise) > 60) { if (ABS(wise) > CROSSING_TURN_THRESHOLD) { // 确认转弯行为，检测开始刚转弯时转向灯情况 turnCnt = -1;
@@ -7,6 +7,7 @@ #include "../driver_test.h" #define GENERAL_MAP 100 #define DRIVE_STRAIGHT_MAP 101 #define OP_GEAER_MAP 102 @@ -25,6 +26,16 @@ int sec; int msec; }; typedef struct { int name; int value; struct RtkTime time; } car_sensor_value_t; car_sensor_value_t ReadCarSensorValue(int name); 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); lib/src/main/cpp/test_items2/stop_car.cpp
@@ -46,9 +46,9 @@ checkRoadDistance = false; if (!tts.empty()) { examTtsSeq = PlayTTS(tts.c_str()); examTtsSeq = PlayTTS(tts.c_str(), NULL); } else { examTtsSeq = PlayTTS("请靠边停车"); examTtsSeq = PlayTTS("请靠边停车", NULL); } AppTimer_delete(PlayTTSTimeout);
