From 682b17ff66dff23e03c6a57de276ea0c3e670c0e Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期一, 23 三月 2020 18:44:47 +0800
Subject: [PATCH] 起步和直线
---
lib/src/main/cpp/test_items2/drive_straight.h | 14 ++
lib/src/main/cpp/driver_test.cpp | 29 ++++
lib/src/main/cpp/native-lib.h | 2
lib/src/main/cpp/test_common/car_sensor.h | 5
lib/src/main/cpp/test_items2/dummy_light.cpp | 90 +++++++++---
lib/src/main/cpp/CMakeLists.txt | 1
lib/src/main/cpp/driver_test.h | 6
lib/src/main/cpp/test_items2/drive_straight.cpp | 80 +++++++++++
lib/src/main/cpp/master/comm_if.cpp | 19 ++
lib/src/main/cpp/native-lib.cpp | 25 +++
lib/src/main/cpp/test_items2/through_something.cpp | 2
lib/src/main/cpp/test_items2/road_exam.cpp | 127 +++++++++++++++--
lib/src/main/cpp/test_items2/dummy_light.h | 1
lib/src/main/cpp/test_common/car_sensor.cpp | 2
14 files changed, 355 insertions(+), 48 deletions(-)
diff --git a/lib/src/main/cpp/CMakeLists.txt b/lib/src/main/cpp/CMakeLists.txt
index 2d0498b..98ea0db 100644
--- a/lib/src/main/cpp/CMakeLists.txt
+++ b/lib/src/main/cpp/CMakeLists.txt
@@ -40,6 +40,7 @@
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
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index fae8d3b..46e0c3d 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -257,6 +257,15 @@
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();
@@ -282,6 +291,7 @@
vector<vector<int>> &redAreas,
vector<vector<int>> &greenLines,
vector<vector<int>> &triggerLines,
+ vector<vector<int>> &roadEdgeLines,
vector<int> area,
vector<int> stopLine)
{
@@ -371,7 +381,24 @@
}
} 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();
diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h
index b590054..83bb257 100644
--- a/lib/src/main/cpp/driver_test.h
+++ b/lib/src/main/cpp/driver_test.h
@@ -107,6 +107,9 @@
int triggerLineNum;
trigger_line_t *triggerLine;
+ int roadEdgeLineNum; // 閬撹矾杈圭嚎锛屽彧鏈夌洿绾块┚椹躲�侀潬杈瑰仠杞︽墠鏈�
+ Polygon *roadEdgeLine;
+
Polygon area; // 瀛愰」鐩殑鍖哄煙
Line stopLine; // 璇稿浜鸿閬撱�佽矾鍙ｇ瓑鑰冪偣鐨勫仠姝㈢嚎
int flagStop; // 鍒拌揪寮�濮嬬嚎鍓嶏紝鏄惁闇�瑕佸仠杞�
@@ -125,7 +128,8 @@
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,
diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp
index fddeb5c..182c687 100644
--- a/lib/src/main/cpp/master/comm_if.cpp
+++ b/lib/src/main/cpp/master/comm_if.cpp
@@ -526,9 +526,12 @@
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();
@@ -595,6 +598,20 @@
}
}
+ 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"];
@@ -633,7 +650,7 @@
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);
}
}
}
diff --git a/lib/src/main/cpp/native-lib.cpp b/lib/src/main/cpp/native-lib.cpp
index e19a097..2769bd6 100644
--- a/lib/src/main/cpp/native-lib.cpp
+++ b/lib/src/main/cpp/native-lib.cpp
@@ -29,6 +29,10 @@
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,
@@ -195,7 +199,18 @@
}
}
-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);
@@ -206,7 +221,7 @@
// Attach涓荤嚎绋�
if (sg_jvm->AttachCurrentThread(&env, NULL) != JNI_OK) {
LOGE("%s: AttachCurrentThread() failed", __FUNCTION__);
- return;
+ return -1;
}
} else {
ready_in_java_env = true;
@@ -214,6 +229,8 @@
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);
@@ -225,6 +242,8 @@
LOGE("%s: DetachCurrentThread() failed", __FUNCTION__);
}
}
+
+ return id;
}
extern "C"
@@ -248,6 +267,8 @@
AppTimer_add(SendBootIndicate, 500);
InitVirtualDevice(VIRTUAL_RTK_IP, VIRTUAL_RTK_PORT);
+
+ pthread_mutex_init(&tts_mutex, NULL);
}
static void SendBootIndicate(union sigval sig) {
diff --git a/lib/src/main/cpp/native-lib.h b/lib/src/main/cpp/native-lib.h
index 77011b3..60b05cd 100644
--- a/lib/src/main/cpp/native-lib.h
+++ b/lib/src/main/cpp/native-lib.h
@@ -18,6 +18,6 @@
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
diff --git a/lib/src/main/cpp/test_common/car_sensor.cpp b/lib/src/main/cpp/test_common/car_sensor.cpp
index d053ad9..a7a4692 100644
--- a/lib/src/main/cpp/test_common/car_sensor.cpp
+++ b/lib/src/main/cpp/test_common/car_sensor.cpp
@@ -84,6 +84,8 @@
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) {
diff --git a/lib/src/main/cpp/test_common/car_sensor.h b/lib/src/main/cpp/test_common/car_sensor.h
index b3a1157..b6e65af 100644
--- a/lib/src/main/cpp/test_common/car_sensor.h
+++ b/lib/src/main/cpp/test_common/car_sensor.h
@@ -22,6 +22,7 @@
BREAK,
HAND_BREAK,
SECOND_BREAK,
+ DOOR,
CAR_STATUS_END //////////////
};
@@ -30,6 +31,7 @@
EJECT_SEATBELT = 0,
ENGINE_START_INACTIVE = 0,
BREAK_INACTIVE = 0,
+ DOOR_OPEN = 0,
HAZARD_LIGHTS,
LEFT_TURN_LIGHT,
RIGHT_TURN_LIGHT,
@@ -47,7 +49,8 @@
GEAR_3,
GEAR_4,
GEAR_5,
- BREAK_ACTIVE
+ BREAK_ACTIVE,
+ DOOR_CLOSE
};
void CarSensorInit(void);
diff --git a/lib/src/main/cpp/test_items2/drive_straight.cpp b/lib/src/main/cpp/test_items2/drive_straight.cpp
new file mode 100644
index 0000000..9286e0f
--- /dev/null
+++ b/lib/src/main/cpp/test_items2/drive_straight.cpp
@@ -0,0 +1,80 @@
+//
+// 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;
+}
diff --git a/lib/src/main/cpp/test_items2/drive_straight.h b/lib/src/main/cpp/test_items2/drive_straight.h
new file mode 100644
index 0000000..fa6e3b3
--- /dev/null
+++ b/lib/src/main/cpp/test_items2/drive_straight.h
@@ -0,0 +1,14 @@
+//
+// 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
diff --git a/lib/src/main/cpp/test_items2/dummy_light.cpp b/lib/src/main/cpp/test_items2/dummy_light.cpp
index 83d3ee1..3f7298d 100644
--- a/lib/src/main/cpp/test_items2/dummy_light.cpp
+++ b/lib/src/main/cpp/test_items2/dummy_light.cpp
@@ -11,12 +11,22 @@
#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);
@@ -26,10 +36,13 @@
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));
}
@@ -37,31 +50,28 @@
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:
@@ -119,17 +129,53 @@
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;
}
}
diff --git a/lib/src/main/cpp/test_items2/dummy_light.h b/lib/src/main/cpp/test_items2/dummy_light.h
index 8f481d2..6edb749 100644
--- a/lib/src/main/cpp/test_items2/dummy_light.h
+++ b/lib/src/main/cpp/test_items2/dummy_light.h
@@ -24,6 +24,7 @@
struct dummy_light_exam {
int item;
+ int itemStatus;
char tts[512];
};
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index ae82561..b158dd5 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -12,6 +12,7 @@
#include "../native-lib.h"
#include "through_something.h"
#include "../master/comm_if.h"
+#include "drive_straight.h"
#include <vector>
#include <list>
@@ -23,6 +24,12 @@
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;
@@ -40,7 +47,7 @@
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;
@@ -61,8 +68,12 @@
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);
@@ -73,8 +84,9 @@
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);
@@ -107,10 +119,69 @@
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) {
@@ -266,7 +337,7 @@
// 妫�鏌ユ槸鍚︽寔缁浆鍚�
char turnDirect = CheckCarTurn(CarModelList);
if (turnDirect == 'L') {
- PlayTTS("宸�1", 5692);
+// PlayTTS("宸�1");
if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
if (!reportTurnSignalError) {
DEBUG("娌℃墦宸﹁浆鐏�");
@@ -284,7 +355,7 @@
}
}
} else if (turnDirect == 'R') {
- PlayTTS("鍙�1", 5692);
+// PlayTTS("鍙�1");
if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
if (!reportTurnSignalError) {
DEBUG("娌℃墦鍙宠浆鐏�");
@@ -386,7 +457,7 @@
// 妫�鏌ュ彉閬撳墠锛屾槸鍚︽彁鍓嶈浆鍚戠伅
if (inter == 1) {
- PlayTTS("宸�2", 5698);
+// PlayTTS("宸�2");
// 鍚戝乏渚у彉閬�
DEBUG("鍚戝乏渚у彉閬�");
if (turnSignalStatusWhenCrashGreenLine != LEFT_TURN_LIGHT) {
@@ -395,7 +466,7 @@
AddExamFault(13, rtkTime);
}
} else {
- PlayTTS("鍙�2", 5698);
+// PlayTTS("鍙�2");
// 鍚戝彸渚у彉閬�
DEBUG("鍚戝彸渚у彉閬�");
if (turnSignalStatusWhenCrashGreenLine != RIGHT_TURN_LIGHT) {
@@ -410,20 +481,32 @@
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);
- StartThroughExam(currExamMapIndex, RoadMapList);
+
+ 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;
- currExamMapIndex = ExecuteThroughExam(currExamMapIndex, RoadMapList, car,
- CarModelList, speed, moveDirect, rtkTime);
+ 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);
@@ -521,10 +604,9 @@
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) {
@@ -532,6 +614,8 @@
} else {
deltaAng = ABS(currYaw - prevYaw);
}
+
+ ang = deltaAng;
// DEBUG("瑙掑害宸�� %d", deltaAng);
@@ -545,12 +629,12 @@
}
}
- 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;
@@ -571,10 +655,15 @@
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;
@@ -790,3 +879,5 @@
}
return -1;
}
+
+
diff --git a/lib/src/main/cpp/test_items2/through_something.cpp b/lib/src/main/cpp/test_items2/through_something.cpp
index af55926..913333b 100644
--- a/lib/src/main/cpp/test_items2/through_something.cpp
+++ b/lib/src/main/cpp/test_items2/through_something.cpp
@@ -26,7 +26,7 @@
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");
}
--
Gitblit v1.8.0