| lib/src/main/cpp/native-lib.cpp | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 | |
| lib/src/main/cpp/rtk_module/rtk.cpp | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 | |
| lib/src/main/cpp/rtk_module/rtk.h | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 | |
| lib/src/main/cpp/test_items2/drive_straight.cpp | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 | |
| lib/src/main/cpp/test_items2/dummy_light.cpp | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 | |
| lib/src/main/cpp/test_items2/road_exam.cpp | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 |
lib/src/main/cpp/native-lib.cpp
@@ -272,8 +272,15 @@ } static void SendBootIndicate(union sigval sig) { static int n = 0; AppTimer_delete(SendBootIndicate); MA_NdkStart(); n++; if (n < 3) { AppTimer_add(SendBootIndicate, D_SEC(1)); } } extern "C" lib/src/main/cpp/rtk_module/rtk.cpp
@@ -38,7 +38,7 @@ static gpsStatus_t gpsStatus; static rtk_info CurrRTKInfo; static bool needSetPjk = false; static void CheckPjkParam(void); static void CheckPjkParamTimeout(union sigval sig); @@ -84,55 +84,58 @@ void handleRTKRebootComp(const struct nmea *s) { DEBUG("RTK Reboot complete!!"); SetAYFactoryParam(5); CheckPjkParam(); } void handlePJKParam(const struct nmea *s) { //PJK Parameter: A:6378137.000, 1/F:298.257223563, B0:0.000000deg, L0:120.000000, N0:0.000, E0:500000.000. //PJK Parameter: A:6378137.000, 1/F:298.257223563, B0:29.512453deg, L0:106.455336, N0:0.000, E0:0.000. bool setparam = true; const char DP1[] = "A:6378137.000"; const char DP2[] = "1/F:298.257223563"; const char DP3[] = "B0:29.512453deg"; const char DP4[] = "L0:106.455336"; const char DP5[] = "N0:0.000"; const char DP6[] = "E0:0.000"; // bool setparam = true; // // const char DP1[] = "A:6378137.000"; // const char DP2[] = "1/F:298.257223563"; // const char DP3[] = "B0:29.512453deg"; // const char DP4[] = "L0:106.455336"; // const char DP5[] = "N0:0.000"; // const char DP6[] = "E0:0.000"; AppTimer_delete(CheckPjkParamTimeout); for (int i = 0; i < s->nmea_num; ++i) { char out[64] = {0}; // for (int i = 0; i < s->nmea_num; ++i) { // char out[64] = {0}; // // memcpy(out, s->nmea_value[i].data, s->nmea_value[i].length); // // DEBUG("handlePJKParam = %s", out); // } // // if (s->nmea_num != 6) return; // // if (memcmp(s->nmea_value[0].data, DP1, strlen(DP1))) { // setparam = true; // } // if (memcmp(s->nmea_value[1].data, DP2, strlen(DP2))) { // setparam = true; // } // if (memcmp(s->nmea_value[2].data, DP3, strlen(DP3))) { // setparam = true; // } // if (memcmp(s->nmea_value[3].data, DP4, strlen(DP4))) { // setparam = true; // } // if (memcmp(s->nmea_value[4].data, DP5, strlen(DP5))) { // setparam = true; // } // if (memcmp(s->nmea_value[5].data, DP6, strlen(DP6))) { // setparam = true; // } // // if (setparam) { // SetAYFactoryParam(5); // } memcpy(out, s->nmea_value[i].data, s->nmea_value[i].length); DEBUG("handlePJKParam = %s", out); } if (s->nmea_num != 6) return; if (memcmp(s->nmea_value[0].data, DP1, strlen(DP1))) { setparam = true; } if (memcmp(s->nmea_value[1].data, DP2, strlen(DP2))) { setparam = true; } if (memcmp(s->nmea_value[2].data, DP3, strlen(DP3))) { setparam = true; } if (memcmp(s->nmea_value[3].data, DP4, strlen(DP4))) { setparam = true; } if (memcmp(s->nmea_value[4].data, DP5, strlen(DP5))) { setparam = true; } if (memcmp(s->nmea_value[5].data, DP6, strlen(DP6))) { setparam = true; } if (setparam) { SetAYFactoryParam(5); } SetAYFactoryParam(5); needSetPjk = true; } void SetAYFactoryParam(int freq) @@ -155,8 +158,19 @@ WriteSerialPort(RTK_MODULE_UART, cmd, strlen(cmd)); } WriteSerialPort(RTK_MODULE_UART, AY_PJKPARAM, strlen(AY_PJKPARAM)); // WriteSerialPort(RTK_MODULE_UART, AY_PJKPARAM, strlen(AY_PJKPARAM)); // WriteSerialPort(RTK_MODULE_UART, SAVECONFIG, strlen(SAVECONFIG)); } void SetPjkPara(int centLon) { char buff[64]; sprintf(buff, "set pjkpara 6378137 298.257223563 0 %d 0 500000\r\n", centLon); WriteSerialPort(RTK_MODULE_UART, buff, strlen(buff)); DEBUG("%s", buff); } void GetGpsStatus(gpsStatus_t &data) @@ -229,6 +243,8 @@ void handleGPGGA(const struct nmea *s) { static uint32_t qfCnt = 0; DEBUG("handleGPGGA num = %d", s->nmea_num); if (s->nmea_num >= 10) { gpsStatus.gps_status = str2int(s->nmea_value[5].data, s->nmea_value[5].length); @@ -263,6 +279,18 @@ gpsStatus.mm = mm; gpsStatus.ss = ss; gpsStatus.mss = mss; // 计算中央子午线 int qf = str2int(s->nmea_value[5].data, s->nmea_value[5].length); if (qf > 0) { qfCnt++; if (needSetPjk && qfCnt >= 3) { needSetPjk = false; SetPjkPara(((int) (lon1 / 3.0 + 0.5)) * 3); } } else { qfCnt = 0; } } }
@@ -44,5 +44,6 @@ void RebootModule(void); void SetAYFactoryParam(int freq); void GetGpsStatus(gpsStatus_t &data); void SetPjkPara(int centLon); #endif //RTKDRIVERTEST_RTK_H lib/src/main/cpp/test_items2/drive_straight.cpp
@@ -72,9 +72,9 @@ reportOffsetOver = true; ////////////////////////////////////////////// startPoint = car->basePoint; edgeDistance = dis2roadEdge; reportOffsetOver = false; // startPoint = car->basePoint; // edgeDistance = dis2roadEdge; // reportOffsetOver = false; } if (distanceToStart > CHECK_STAGE_DISTANCE) { @@ -99,6 +99,7 @@ if (id == examTtsSeq) { DEBUG("StopCarTTSDone %d", id); ttsPlayEnd = 1; AppTimer_delete(PlayTTSTimeout); } } @@ -141,9 +142,11 @@ FIND_VP_END: if (get_vp) { DEBUG("得到垂点 %d: %f, %f -- %f, %f", road.id, car->basePoint.X, car->basePoint.Y, vp.X, vp.Y); distance = DistanceOf(car->basePoint, vp); } else { // 没有找到匹配线端，按最小距离顶点计算 DEBUG("无垂点"); distance = 100; for (int i = 0; i < road.leftEdge.size(); ++i) { for (int j = 0; j < road.leftEdge[i].points.size(); ++j) { lib/src/main/cpp/test_items2/dummy_light.cpp
@@ -38,16 +38,20 @@ content = ptr; contentNum = num; currRtkTime = *rtkTime; if (content != NULL && num > 0) { currRtkTime = *rtkTime; for (int i = 0; i < contentNum; ++i) { content[i].itemStatus = TTS_NOT_START; for (int i = 0; i < contentNum; ++i) { content[i].itemStatus = TTS_NOT_START; } testing = true; AppTimer_delete(DummyLightCheckActive); AppTimer_delete(ExamDummyLight); AppTimer_add(ExamDummyLight, D_SEC(2)); } else { testing = false; } testing = true; AppTimer_delete(DummyLightCheckActive); AppTimer_delete(ExamDummyLight); AppTimer_add(ExamDummyLight, D_SEC(2)); } int ExecuteDummyLightExam(const struct RtkTime* rtkTime) lib/src/main/cpp/test_items2/road_exam.cpp
@@ -48,7 +48,7 @@ const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1; const double CHANGE_LANE_RANGE = 100.0; const double OVERTAKE_RANGE = 150.0; const double OVERTAKE_HOLD_RANGE = 30.0; // 在超车道行驶的一段距离 const int OVERTAKE_HOLD_TIME = D_SEC(11); // 在超车道行驶的一段时间 const double EXAM_RANGE = 3000.0; // 至少驾驶距离 static const double LASTEST_BREAK_POINT = 30.0; @@ -84,6 +84,7 @@ static struct drive_timer gearErrorTimePoint; static struct drive_timer gearNSlideTimePoint; static struct drive_timer startCarLeftTurnSignalTime; static struct drive_timer overTakeCmpTime; static int gearErrorTime; static int gearNSlideTime; @@ -95,7 +96,7 @@ 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; @@ -122,7 +123,7 @@ 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 uint32_t CHANGE_ROAD_MIN_INTERVAL = D_SEC(10); static const uint32_t CHANGE_LANE_MIN_INTERVAL = D_SEC(10); static const uint32_t CRASH_DOTTED_LINE_TIMEOUT = D_SEC(10); static const uint32_t TURN_SIGNAL_LAMP_ADVANCE = D_SEC(3); @@ -200,6 +201,7 @@ laneChanging = false; nextRoadId = -1; checkTurn = false; ClearAll(RoadMap); odoGraph = 0.0; @@ -584,6 +586,7 @@ free(area.point); } if (currExamMapIndex == FIND_POSITION) { DEBUG("搜索道路"); for (int i = 0; i < RoadMap.roads.size(); ++i) { Polygon area; int n = 0; @@ -633,13 +636,14 @@ AddExamFault(3, rtkTime); } nextRoadId = -1; checkTurn = false; break; } free(area.point); } if (currExamMapIndex < 0) { currExamMapIndex = INVALID_POSITION; currExamMapIndex = FIND_POSITION;//INVALID_POSITION; DEBUG("搜寻未果"); } } else if (currExamMapIndex == INVALID_POSITION) { @@ -701,7 +705,7 @@ crashGreenCmpTime.sec, crashGreenCmpTime.msec * 10); if (diff < CHANGE_ROAD_MIN_INTERVAL) { if (diff < CHANGE_LANE_MIN_INTERVAL) { DEBUG("===================== 连续变道 ============!!"); // 连续变道，不合格 AddExamFault(15, rtkTime); @@ -763,7 +767,8 @@ } else if (currRoadItem != NULL && currRoadItem->active == ROAD_ITEM_OVERTAKE) { if (CurrentLane.lane > lane.lane) { DEBUG("超车变道完成"); overtake = true; Rtk2DriveTimer(overTakeCmpTime, rtkTime); } else { DEBUG("右道超车，错误"); AddExamFault(3, rtkTime); @@ -802,6 +807,51 @@ if (CrashTheLine(RoadMap.roads[currExamMapIndex].stopLine, car, CarModelList)) { DEBUG("下一个目标路 id = %d", RoadMap.roads[currExamMapIndex].targetRoad); nextRoadId = RoadMap.roads[currExamMapIndex].targetRoad; checkTurn = true; } if (checkTurn) { // 检查是否持续转向 char turnDirect = CheckCarTurn(CarModelList); if (turnDirect == 'L') { // PlayTTS("左1"); if (currTurnSignalStatus != LEFT_TURN_LIGHT) { if (!reportTurnSignalError) { DEBUG("没打左转灯"); // 没打左转灯，不合格 reportTurnSignalError = true; ReportTurnSignalError(13, rtkTime); } } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10, turnSignalChangeTime.hour, turnSignalChangeTime.min, turnSignalChangeTime.sec, turnSignalChangeTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) { if (!reportTurnSignalError) { DEBUG("转向灯时间不足"); // 不足3秒，不合格 reportTurnSignalError = true; ReportTurnSignalError(14, rtkTime); } } } else if (turnDirect == 'R') { // PlayTTS("右1"); if (currTurnSignalStatus != RIGHT_TURN_LIGHT) { if (!reportTurnSignalError) { DEBUG("没打右转灯"); // 没打右转灯，不合格 reportTurnSignalError = true; ReportTurnSignalError(13, rtkTime); } } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10, turnSignalChangeTime.hour, turnSignalChangeTime.min, turnSignalChangeTime.sec, turnSignalChangeTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) { if (!reportTurnSignalError) { DEBUG("转向灯时间不足"); // 不足3秒，不合格 reportTurnSignalError = true; ReportTurnSignalError(14, rtkTime); } } } else { reportTurnSignalError = false; } } } @@ -882,7 +932,10 @@ DEBUG("超车距离超标"); AddExamFault(3, rtkTime); currRoadItem = NULL; } else if (overtake && DistanceOf(car->basePoint, roadItemStartPoint) > OVERTAKE_HOLD_RANGE) { } else if (overtake && TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss * 10, overTakeCmpTime.hour, overTakeCmpTime.min, overTakeCmpTime.sec, overTakeCmpTime.msec * 10) > OVERTAKE_HOLD_TIME) { DEBUG("回原车道"); PlayTTS("请返回原车道"); currRoadItem = NULL; @@ -1484,7 +1537,7 @@ crashGreenCmpTime.hour, crashGreenCmpTime.min, crashGreenCmpTime.sec, crashGreenCmpTime.msec * 10); if (diff < CHANGE_ROAD_MIN_INTERVAL) { if (diff < CHANGE_LANE_MIN_INTERVAL) { DEBUG("===================== 连续变道 ============!!"); // 连续变道，不合格 AddExamFault(15, rtkTime);
