From 27c78b3431a38878b8c8b1b81c79694cea4a2bcf Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期四, 23 七月 2020 10:07:51 +0800
Subject: [PATCH] 20200723
---
lib/src/main/cpp/test_items2/drive_straight.cpp | 9 +
lib/src/main/cpp/rtk_module/rtk.h | 1
lib/src/main/cpp/native-lib.cpp | 7 +
lib/src/main/cpp/test_items2/dummy_light.cpp | 20 +++--
lib/src/main/cpp/test_items2/road_exam.cpp | 69 +++++++++++++++--
lib/src/main/cpp/rtk_module/rtk.cpp | 112 +++++++++++++++++----------
6 files changed, 157 insertions(+), 61 deletions(-)
diff --git a/lib/src/main/cpp/native-lib.cpp b/lib/src/main/cpp/native-lib.cpp
index b9a2944..70aa448 100644
--- a/lib/src/main/cpp/native-lib.cpp
+++ b/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"
diff --git a/lib/src/main/cpp/rtk_module/rtk.cpp b/lib/src/main/cpp/rtk_module/rtk.cpp
index 5fc26f8..9c766f8 100644
--- a/lib/src/main/cpp/rtk_module/rtk.cpp
+++ b/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;
+ }
}
}
diff --git a/lib/src/main/cpp/rtk_module/rtk.h b/lib/src/main/cpp/rtk_module/rtk.h
index 1f9607b..82a5d54 100644
--- a/lib/src/main/cpp/rtk_module/rtk.h
+++ b/lib/src/main/cpp/rtk_module/rtk.h
@@ -44,5 +44,6 @@
void RebootModule(void);
void SetAYFactoryParam(int freq);
void GetGpsStatus(gpsStatus_t &data);
+void SetPjkPara(int centLon);
#endif //RTKDRIVERTEST_RTK_H
diff --git a/lib/src/main/cpp/test_items2/drive_straight.cpp b/lib/src/main/cpp/test_items2/drive_straight.cpp
index 4e006f3..6d505b9 100644
--- a/lib/src/main/cpp/test_items2/drive_straight.cpp
+++ b/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) {
diff --git a/lib/src/main/cpp/test_items2/dummy_light.cpp b/lib/src/main/cpp/test_items2/dummy_light.cpp
index 56aaa05..124a03f 100644
--- a/lib/src/main/cpp/test_items2/dummy_light.cpp
+++ b/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)
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index f7b028d..4b9a859 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/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);
--
Gitblit v1.8.0