From a3c194e003b0cfb272e153ff11c510e02c5f72cd Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期一, 25 五月 2020 18:32:11 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items2/drive_straight.cpp | 34 ++++++++++-
lib/src/main/cpp/test_items2/drive_straight.h | 4 +
lib/src/main/cpp/test_items2/stop_car.h | 2
lib/src/main/cpp/native-lib.cpp | 2
lib/src/main/cpp/test_items2/road_exam.cpp | 102 ++++++++++++++++++++++++++++++----
lib/src/main/cpp/test_items2/stop_car.cpp | 6 +-
lib/src/main/cpp/driver_test.h | 12 ++--
lib/src/main/cpp/rtk_platform/platform.cpp | 2
8 files changed, 137 insertions(+), 27 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h
index 769a78d..14c6407 100644
--- a/lib/src/main/cpp/driver_test.h
+++ b/lib/src/main/cpp/driver_test.h
@@ -115,12 +115,11 @@
#define ROAD_ITEM_OPERATE_GEAR 4
#define ROAD_ITEM_START_CAR 5
-enum {
- ROAD_ACTIVE_FORWARD,
- ROAD_ACTIVE_TURN_LEFT,
- ROAD_ACTIVE_TURN_RIGHT,
- ROAD_ACTIVE_TURN_BACKWARD
-};
+
+#define ROAD_ACTIVE_FORWARD LANE_FORWARD
+#define ROAD_ACTIVE_TURN_LEFT LANE_LEFT
+#define ROAD_ACTIVE_TURN_RIGHT LANE_RIGHT
+#define ROAD_ACTIVE_TURN_BACKWARD LANE_BACKWARD
typedef struct {
int character; // 灞炴�э紙瀹炵嚎銆佽櫄绾匡紝鏈変簺鍙互鎺夊ご鐨勮矾娈碉級
@@ -174,6 +173,7 @@
int time; // 椤圭洰鏈�澶у畬鎴愭椂闂�
int distance; // 椤圭洰鏈�澶у畬鎴愯窛绂�
string tts;
+ bool cmp;
std::vector<PointF> points;
std::vector<PointF> leftPoints; // 瀵瑰簲鍒伴亾璺乏渚х殑鐐�
} trigger_line_t;
diff --git a/lib/src/main/cpp/native-lib.cpp b/lib/src/main/cpp/native-lib.cpp
index 206f071..b9a2944 100644
--- a/lib/src/main/cpp/native-lib.cpp
+++ b/lib/src/main/cpp/native-lib.cpp
@@ -264,7 +264,7 @@
ConfigRTKModule();
MA_Init();
InitPlatform(phone, RTK_PLATFORM_IP, RTK_PLATFORM_PORT);
- AppTimer_add(SendBootIndicate, 500);
+ AppTimer_add(SendBootIndicate, D_SEC(1));
InitVirtualDevice(VIRTUAL_RTK_IP, VIRTUAL_RTK_PORT);
diff --git a/lib/src/main/cpp/rtk_platform/platform.cpp b/lib/src/main/cpp/rtk_platform/platform.cpp
index be6f0c1..c47a98e 100644
--- a/lib/src/main/cpp/rtk_platform/platform.cpp
+++ b/lib/src/main/cpp/rtk_platform/platform.cpp
@@ -24,6 +24,7 @@
#include "../test_common/car_sensor.h"
#include "../test_items2/stop_car.h"
#include "../test_items2/operate_gear.h"
+#include "../test_items2/drive_straight.h"
#define PARSE_BUFF_SIZE 4096
@@ -440,6 +441,7 @@
DummyLightTTSDone(*((int *)data));
StopCarTTSDone(*((int *)data));
OperateGearTTSDone(*((int *)data));
+ DriveStraightTTSDone(*((int *)data));
}
}
diff --git a/lib/src/main/cpp/test_items2/drive_straight.cpp b/lib/src/main/cpp/test_items2/drive_straight.cpp
index 3b9f8d3..4e006f3 100644
--- a/lib/src/main/cpp/test_items2/drive_straight.cpp
+++ b/lib/src/main/cpp/test_items2/drive_straight.cpp
@@ -8,6 +8,7 @@
#include "../jni_log.h"
#include "road_exam.h"
#include "../master/comm_if.h"
+#include "../common/apptimer.h"
#include <cmath>
#define DEBUG(fmt, args...) LOGD("<drive_straight> <%s>: " fmt, __func__, ##args)
@@ -20,24 +21,30 @@
static bool reportOffsetOver;
static double edgeDistance;
static double distanceToStartSum;
+static int examTtsSeq;
static double CalcDistance2Edge(road_t &road, const car_model *car);
+static void PlayTTSTimeout(union sigval sig);
void StartDriveStraightExam(std::string tts) {
DEBUG("寮�濮嬬洿绾胯椹�");
+ ttsPlayEnd = 0;
if (!tts.empty()) {
- PlayTTS(tts.c_str());
+ examTtsSeq = PlayTTS(tts.c_str());
} else {
- DEBUG("娌℃湁TTS");
+ examTtsSeq = PlayTTS("璇蜂繚鎸佺洿绾胯椹�");
}
distanceToStartSum = 0;
reportOffsetOver = false;
+
+ AppTimer_delete(PlayTTSTimeout);
+ AppTimer_add(PlayTTSTimeout, D_SEC(5));
}
int ExecuteDriveStraightExam(road_t &road, const car_model *car,
- LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) {
+ const trigger_line_t *item, const struct RtkTime *rtkTime) {
static PointF startPoint;
double dis2roadEdge = 0;
@@ -86,6 +93,27 @@
return 1;
}
+void DriveStraightTTSDone(int id)
+{
+ // 绛夎闊虫挱鎶ュ畬姣曞悗璁℃椂
+ if (id == examTtsSeq) {
+ DEBUG("StopCarTTSDone %d", id);
+ ttsPlayEnd = 1;
+ }
+}
+
+void TerminateDriveStraightExam(void)
+{
+ AppTimer_delete(PlayTTSTimeout);
+}
+
+static void PlayTTSTimeout(union sigval sig)
+{
+ AppTimer_delete(PlayTTSTimeout);
+
+ ttsPlayEnd = 1;
+}
+
static double CalcDistance2Edge(road_t &road, const car_model *car)
{
PointF vp;
diff --git a/lib/src/main/cpp/test_items2/drive_straight.h b/lib/src/main/cpp/test_items2/drive_straight.h
index d4fd2ba..1187221 100644
--- a/lib/src/main/cpp/test_items2/drive_straight.h
+++ b/lib/src/main/cpp/test_items2/drive_straight.h
@@ -9,6 +9,8 @@
void StartDriveStraightExam(std::string tts);
int ExecuteDriveStraightExam(road_t &road, const car_model *car,
- LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);
+ const trigger_line_t *item, const struct RtkTime *rtkTime);
+void DriveStraightTTSDone(int id);
+void TerminateDriveStraightExam(void);
#endif //MYAPPLICATION2_DRIVE_STRAIGHT_H
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index d132808..f7b028d 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -34,6 +34,12 @@
START_CAR_DONE
};
+enum {
+ STOP_CAR_NOT_DO,
+ STOP_CAR_DOING,
+ STOP_CAR_DONE
+};
+
static const int INVALID_ROAD = -1;
static const int TURN_THRESHOLD = 1;
@@ -43,6 +49,7 @@
const double CHANGE_LANE_RANGE = 100.0;
const double OVERTAKE_RANGE = 150.0;
const double OVERTAKE_HOLD_RANGE = 30.0; // 鍦ㄨ秴杞﹂亾琛岄┒鐨勪竴娈佃窛绂�
+const double EXAM_RANGE = 3000.0; // 鑷冲皯椹鹃┒璺濈
static const double LASTEST_BREAK_POINT = 30.0;
static const double NEXT_ROAD_TIP = 100.0; // 鍒拌揪璺彛鍓嶆彁绀轰笅涓�涓�庝箞璧�
@@ -81,7 +88,7 @@
static int gearErrorTime;
static int gearNSlideTime;
-static int startCar;
+static int startCar, stopCar;
static int currExamMapIndex;
static trigger_line_t *currRoadItem;
static int nextRoadId;
@@ -104,6 +111,10 @@
int type; // 瀹炵嚎锛岃櫄绾�
} CurrentLane;
static bool laneChanging;
+static double odoGraph;
+static struct drive_timer odoTimer;
+static double odoPrevSpeed;
+static int odoCnt;
static const int MAX_ENGINE_RPM = 2500;
static const double START_CAR_MOVE_DISTANCE = 10.0;
@@ -140,6 +151,7 @@
static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList);
static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
static void ClearAll(road_exam_map &map);
+static bool AllCmp(road_exam_map &map);
static void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList,
double speed, int moveDirect, const struct RtkTime *rtkTime);
@@ -172,6 +184,8 @@
currExamMapIndex = FIND_POSITION;
startCar = START_CAR_NOT_DO;
+ stopCar = STOP_CAR_NOT_DO;
+
currRoadItem = NULL;
checkDoor = false;
@@ -187,6 +201,11 @@
nextRoadId = -1;
ClearAll(RoadMap);
+
+ odoGraph = 0.0;
+ odoCnt = 0;
+
+ // 鍒濆鍖栬�冮」
}
void TerminateRoadExam(void)
@@ -194,6 +213,7 @@
TerminateDummyLightExam();
TerminateStopCarExam();
TerminateOperateGearExam();
+ TerminateDriveStraightExam();
AppTimer_delete(TurnSignalError13ColdTimer);
AppTimer_delete(TurnSignalError14ColdTimer);
@@ -306,6 +326,25 @@
void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
+ // 琛岄┒璺濈锛屼笉鍖呭惈鍊掕溅
+ if (odoCnt == 0 && moveDirect == 1) {
+ odoPrevSpeed = speed;
+ odoCnt = 1;
+ Rtk2DriveTimer(odoTimer, rtkTime);
+ } else if (odoCnt == 1) {
+ uint32_t tm = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
+ odoTimer.hour, odoTimer.min, odoTimer.sec, odoTimer.msec*10);
+ if (tm >= D_SEC(1)) {
+ odoGraph += ((double)tm)*(odoPrevSpeed + speed)/2.0/1000.0;
+ if (moveDirect == 1) {
+ Rtk2DriveTimer(odoTimer, rtkTime);
+ odoPrevSpeed = speed;
+ } else {
+ odoCnt = 0;
+ }
+ }
+ }
+
// 瓒呴�熸娴�
if (moveDirect != 0 && speed > MAX_SPEED) {
if (!occurOverSpeed) {
@@ -636,7 +675,9 @@
crashRedLineNow = true;
}
+ // 妫�鏌ュ綋鍓嶈溅閬�
struct car_on_lane lane;
+
if (UpdateLane(lane, RoadMap.roads[currExamMapIndex], car)) {
if (lane.road == CurrentLane.road && lane.separate == CurrentLane.separate &&
@@ -737,7 +778,7 @@
}
}
- if (lane.road != CurrentLane.road || lane.separate != CurrentLane.separate) {
+ if (lane.road != CurrentLane.road || lane.separate != CurrentLane.separate || lane.direct != CurrentLane.direct) {
// 璺垨娈靛彉鏇达紝鎾ら攢鍙橀亾璺熻釜
DEBUG("============== 璺垨娈靛彉鏇� CURR %d, %d dir %d NEW %d, %d, dir %d", CurrentLane.road,
CurrentLane.separate, CurrentLane.direct,
@@ -818,7 +859,17 @@
if (startCar != START_CAR_DONE)
return;
- // 妫�娴嬬敱瑙﹀彂绾挎帶鍒剁殑椤圭洰
+ if (odoGraph > EXAM_RANGE && currRoadItem == NULL && AllCmp(RoadMap) && stopCar == STOP_CAR_NOT_DO) {
+ // 鍦ㄥ悎閫傛潯浠朵笅鍋滆溅缁撴潫鑰冭瘯
+ StartStopCarExam("");
+ stopCar = STOP_CAR_DOING;
+ } else if (stopCar == STOP_CAR_DOING) {
+ if (ExecuteStopCarExam(RoadMap.roads[currExamMapIndex], car, CarModelList, speed, moveDirect, rtkTime) < 0)
+ stopCar = STOP_CAR_DONE;
+ }
+
+
+ // 鎵ц鏌愪釜椤圭洰
if (currRoadItem != NULL) {
if (currRoadItem->active == ROAD_ITEM_CHANGE_LANE) {
if (DistanceOf(car->basePoint, roadItemStartPoint) > CHANGE_LANE_RANGE) {
@@ -837,11 +888,20 @@
currRoadItem = NULL;
}
} else if (currRoadItem->active == ROAD_ITEM_OPERATE_GEAR) {
- ExecuteOperateGearExam(rtkTime);
+ if (ExecuteOperateGearExam(rtkTime) < 0) {
+ currRoadItem = NULL;
+ }
+ } else if (currRoadItem->active == ROAD_ITEM_STRAIGHT) {
+ if (ExecuteDriveStraightExam(RoadMap.roads[currExamMapIndex], car, currRoadItem, rtkTime) < 0) {
+ currRoadItem = NULL;
+ }
}
- } else if (currExamMapIndex >= 0) {
- currRoadItem = EntryItem(currExamMapIndex, RoadMap, car, CarModelList);
- if (currRoadItem != NULL) {
+ } // 妫�娴嬬敱瑙﹀彂绾挎帶鍒剁殑椤圭洰
+ else if (currExamMapIndex >= 0) {
+ trigger_line_t *new_item = EntryItem(currExamMapIndex, RoadMap, car, CarModelList);
+
+ if (new_item != NULL && !new_item->cmp) {
+ currRoadItem = new_item;
if (!currRoadItem->tts.empty())
PlayTTS(currRoadItem->tts.c_str());
@@ -853,6 +913,8 @@
overtake = false;
} else if (currRoadItem->active == ROAD_ITEM_OPERATE_GEAR) {
StartOperateGearExam(rtkTime);
+ } else if (currRoadItem->active == ROAD_ITEM_STRAIGHT) {
+ StartDriveStraightExam(currRoadItem->tts);
}
}
}
@@ -903,7 +965,7 @@
MakeLine(&rightExtLine, &car->carXY[car->axial[AXIAL_FRONT]], &vp);
goto RIGHT_EXT_CMP;
} else {
- DEBUG("鍙充晶涓嶅瀭鐐� %d p1(%f,%f) p2(%f,%f) (%f,%f)", j, p1.X, p1.Y, p2.X, p2.Y, vp.X, vp.Y);
+// DEBUG("鍙充晶涓嶅瀭鐐� %d p1(%f,%f) p2(%f,%f) (%f,%f)", j, p1.X, p1.Y, p2.X, p2.Y, vp.X, vp.Y);
}
p1 = p2;
}
@@ -946,7 +1008,6 @@
orthogonalInSegment = true;
intersection = true;
-// DEBUG("鍒嗛亾绾� %d 宸︽浜�", j);
break;
} else if (IntersectionOf(rightExtLine, sep) == GM_Intersection) {
vector<int> stor(4);
@@ -960,7 +1021,6 @@
orthogonalInSegment = true;
intersection = true;
-// DEBUG("鍒嗛亾绾� %d 鍙虫浜�", j);
break;
}
p1 = p2;
@@ -983,12 +1043,15 @@
lane.road = road.id;
lane.separate = i;
lane.lane = itx->first;
+
+// DEBUG("left_char %d second %d", left_char, itx->second[1]);
+
if ((left_char == LINE_SOLID || left_char == LINE_HALF_SOLID_RIGHT) &&
(itx->second[1] == LINE_SOLID || itx->second[1] == LINE_HALF_SOLID_LEFT)) // 杞﹂亾宸﹀彸鍧囨槸瀹炵嚎
lane.direct = itx->second[2];
else
lane.direct = 0;
-// DEBUG("璺� %d 娈� %d 杞﹂亾 %d", lane.road, lane.separate, lane.lane);
+// DEBUG("璺� %d 娈� %d 杞﹂亾 %d 闄愬畾 %d", lane.road, lane.separate, lane.lane, lane.direct);
break;
} else {
right_direct = itx->second[3];
@@ -1000,12 +1063,15 @@
lane.road = road.id;
lane.separate = i;
lane.lane = orthogonal.size();
+
+// DEBUG("left_char %d right_direct %d", left_char, right_direct);
+
// 鏈�鍚庤溅閬撶殑鍙充晶闄愬畾
if (left_char == LINE_SOLID || left_char == LINE_HALF_SOLID_RIGHT)
lane.direct = right_direct;
else
lane.direct = 0;
-// DEBUG("璺� %d 娈� %d 杞﹂亾 %d", lane.road, lane.separate, lane.lane);
+// DEBUG("璺� %d 娈� %d 杞﹂亾 %d 闄愬畾 %d", lane.road, lane.separate, lane.lane, lane.direct);
}
out = lane;
return true;
@@ -1835,6 +1901,15 @@
return NULL;
}
+static bool AllCmp(road_exam_map &map)
+{
+ for (int i = 0; i < map.triggerLines.size(); ++i) {
+ if (!map.triggerLines[i].cmp)
+ return false;
+ }
+ return true;
+}
+
/************************************************************************
* 寮�濮嬫柊鐨勮�冭瘯鍚庯紝娓呴櫎鍦板浘鎵�鏈夌殑鍒硅溅銆佸仠杞﹁褰�
* @param map
@@ -1849,6 +1924,9 @@
for (int i = 0; i < map.specialAreas.size(); i++) {
map.specialAreas[i].overSpeed = map.specialAreas[i].activeBreak = false;
}
+ for (int i = 0; i < map.triggerLines.size(); ++i) {
+ map.triggerLines[i].cmp = false;
+ }
}
static void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList,
diff --git a/lib/src/main/cpp/test_items2/stop_car.cpp b/lib/src/main/cpp/test_items2/stop_car.cpp
index 514b2e4..c52bcd8 100644
--- a/lib/src/main/cpp/test_items2/stop_car.cpp
+++ b/lib/src/main/cpp/test_items2/stop_car.cpp
@@ -69,7 +69,7 @@
AppTimer_delete(PlayTTSTimeout);
}
-int ExecuteStopCarExam(int index, road_t &road, const car_model *car,
+int ExecuteStopCarExam(road_t &road, const car_model *car,
LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) {
if (ttsPlayEnd == 1) {
ttsPlayEnd = 2;
@@ -78,7 +78,7 @@
}
if (ttsPlayEnd != 2)
- return index;
+ return 1;
uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
rtkTime->mss * 10,
@@ -220,7 +220,7 @@
}
}
- return index;
+ return 1;
}
static void PlayTTSTimeout(union sigval sig)
diff --git a/lib/src/main/cpp/test_items2/stop_car.h b/lib/src/main/cpp/test_items2/stop_car.h
index 4b3f622..4519ef0 100644
--- a/lib/src/main/cpp/test_items2/stop_car.h
+++ b/lib/src/main/cpp/test_items2/stop_car.h
@@ -8,7 +8,7 @@
#include "../driver_test.h"
void StartStopCarExam(std::string tts);
-int ExecuteStopCarExam(int index, road_t &road, const car_model *car,
+int ExecuteStopCarExam(road_t &road, const car_model *car,
LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);
void TerminateStopCarExam(void);
void StopCarTTSDone(int id);
--
Gitblit v1.8.0