From 25777013517d1bd398a98504826a417236706af2 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期二, 24 三月 2020 19:01:29 +0800
Subject: [PATCH] 车辆信号完善，路边停车完善。
---
lib/src/main/cpp/test_items2/stop_car.h | 15 +
lib/src/main/cpp/driver_test.cpp | 76 +++++++
lib/src/main/cpp/test_common/car_sensor.h | 13 +
lib/src/main/cpp/Geometry.cpp | 40 ++++
lib/src/main/cpp/test_items2/dummy_light.cpp | 30 ++
lib/src/main/cpp/test_items2/stop_car.cpp | 206 ++++++++++++++++++++
lib/src/main/cpp/CMakeLists.txt | 1
lib/src/main/cpp/Geometry.h | 3
lib/src/main/cpp/test_items2/drive_straight.cpp | 16 +
lib/src/main/cpp/test_items2/road_exam.h | 8
lib/src/main/cpp/native-lib.cpp | 2
lib/src/main/cpp/test_items2/road_exam.cpp | 26 +-
lib/src/main/cpp/test_common/car_sensor.cpp | 103 +++++++++
lib/src/main/cpp/rtk_platform/platform.cpp | 2
14 files changed, 507 insertions(+), 34 deletions(-)
diff --git a/lib/src/main/cpp/CMakeLists.txt b/lib/src/main/cpp/CMakeLists.txt
index 98ea0db..171aee1 100644
--- a/lib/src/main/cpp/CMakeLists.txt
+++ b/lib/src/main/cpp/CMakeLists.txt
@@ -41,6 +41,7 @@
test_items2/road_exam.cpp
test_items2/through_something.cpp
test_items2/drive_straight.cpp
+ test_items2/stop_car.cpp
rtk_module/rtk.cpp
rtk_module/virtual_rtk.cpp
diff --git a/lib/src/main/cpp/Geometry.cpp b/lib/src/main/cpp/Geometry.cpp
index 2693543..01e15cd 100644
--- a/lib/src/main/cpp/Geometry.cpp
+++ b/lib/src/main/cpp/Geometry.cpp
@@ -16,10 +16,16 @@
using namespace std;
const double EPSILON = 1e-6;
+const double EPSILON2 = 1e-3;
inline bool isEqual(double a, double b)
{
return (fabs(a - b) <= EPSILON);
+}
+
+inline bool isEqual2(double a, double b)
+{
+ return (fabs(a - b) <= EPSILON2);
}
inline double toRadians(double degree)
@@ -434,3 +440,37 @@
return -1;
}
}
+
+/***************************************************************
+ * 寰楀埌p3浜巔1,p2缁勬垚鐨勭洿绾夸笂鐨勫瀭鐐�
+ * @param p1
+ * @param p2
+ * @param p3
+ * @return
+ */
+PointF GetVerticalPoint(PointF p1, PointF p2, PointF p3)
+{
+ PointF p4;
+
+ if (isEqual2(p1.X, p2.X)) {
+ p4.Y = p3.Y;
+ p4.X = p1.X;
+ return p4;
+ }
+ if (isEqual2(p1.Y, p2.Y)) {
+ p4.X = p3.X;
+ p4.Y = p1.Y;
+ return p4;
+ }
+
+ double k = (p2.Y - p1.Y) / (p2.X - p1.X);
+ double b = p1.Y - k * p1.X;
+
+ double k2 = (p2.X - p1.X) / (p1.Y - p2.Y);
+ double b2 = p3.Y - p3.X * k2;
+
+ p4.X = (b2 - b) / (k - k2);
+ p4.Y = k2 * p4.X + b2;
+
+ return p4;
+}
diff --git a/lib/src/main/cpp/Geometry.h b/lib/src/main/cpp/Geometry.h
index d050c05..540cc87 100644
--- a/lib/src/main/cpp/Geometry.h
+++ b/lib/src/main/cpp/Geometry.h
@@ -36,6 +36,8 @@
inline double toRadians(double degree);
inline double toDegree(double radians);
inline bool isEqual(double a, double b);
+inline bool isEqual2(double a, double b);
+
void MakeLine(Line *line, const PointF *p1, const PointF *p2);
void MakePolygon(Polygon *polygon, std::initializer_list<PointF> point_set);
void CleanPolygon(Polygon *polygon);
@@ -55,5 +57,6 @@
bool PartInsidePolygon(const Polygon *t1, const Polygon *t2);
bool OutsidePolygon(const Polygon *t1, const Polygon *t2);
int IntersectionOfLine(PointF p1, PointF p2, PointF p3);
+PointF GetVerticalPoint(PointF p1, PointF p2, PointF p3);
#endif //GUI_GEOMETRY_H
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 46e0c3d..57a835f 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -551,10 +551,9 @@
if (start == 0) {
DEBUG("缁撴潫鑰冭瘯");
-// CurrExamMapIndex = -1;
-// ExamStart = false;
-// CommTestStart(false);
-// MA_SendExamStatus(0, 0);
+ CurrExamMapIndex = -1;
+ ExamStart = false;
+ MA_SendExamStatus(0, 0);
return;
}
@@ -728,6 +727,75 @@
static void ExecuteExam(const struct RtkTime* rtkTime)
{
+ {
+ static const char *NAME[] = {"OBD_SPEED",
+ "ENGINE_RPM",
+ "GEAR",
+ "TURN_SIGNAL_LAMP",
+ "DIPPED_BEAM_LAMP",
+ "FOG_LAMP",
+ "CLEARANCE_LAMP",
+ "FLASH_BEAM_LAMP",
+ "MAIN_BEAM_LAMP",
+ "SEATBELT",
+ "ENGINE_START",
+ "BREAK",
+ "HAND_BREAK",
+ "SECOND_BREAK",
+ "DOOR",
+ "SURROUND_CAR_1",
+ "SURROUND_CAR_2",
+ "SURROUND_CAR_3",
+ "SURROUND_CAR_4",
+ "CAR_STATUS_END"};
+
+ static const char *VALUE[] = {
+ "0",
+ "HAZARD_LIGHTS",
+ "LEFT_TURN_LIGHT",
+ "RIGHT_TURN_LIGHT",
+ "CLEARANCE_LIGHT",
+ "DIPPED_BEAM_LIGHT",
+ "MAIN_BEAM_LIGHT",
+ "FLASH_BEAM_LIGHT",
+ "FOG_LIGHT",
+ "INSERT_SEATBELT",
+ "ENGINE_START_ACTIVE",
+ "GEAR_N",
+ "GEAR_1",
+ "GEAR_2",
+ "GEAR_3",
+ "GEAR_4",
+ "GEAR_5",
+ "GEAR_R",
+ "BREAK_ACTIVE",
+ "DOOR_CLOSE",
+ "SURROUND_CAR_ACTIVE"
+ };
+
+
+
+ static int cs[CAR_STATUS_END] = {0};
+
+ int cs_temp[CAR_STATUS_END];
+
+ for (int i = 0; i < CAR_STATUS_END; ++i) {
+ cs_temp[i] = ReadCarStatus(i);
+
+// DEBUG("璇诲彇 %d <---- %d", i, cs_temp[i]);
+ }
+
+ for (int i = 0; i < CAR_STATUS_END; ++i) {
+ if (cs_temp[i] != cs[i]) {
+ DEBUG("杞﹁締鐘舵�� %s = %s", NAME[i], VALUE[ cs_temp[i] ]);
+
+ cs[i] = cs_temp[i];
+ }
+ }
+
+ }
+
+
if (ReadCarStatus(ENGINE_RPM) < ENGINE_MIN_ROTATE) {
if (engineRuning) {
engineRuning = false;
diff --git a/lib/src/main/cpp/native-lib.cpp b/lib/src/main/cpp/native-lib.cpp
index 2769bd6..34720a2 100644
--- a/lib/src/main/cpp/native-lib.cpp
+++ b/lib/src/main/cpp/native-lib.cpp
@@ -31,7 +31,7 @@
static pthread_mutex_t tts_mutex = PTHREAD_MUTEX_INITIALIZER;
-static int ttsSeq = 0;
+static int ttsSeq = 1;
static void SendBootIndicate(union sigval sig);
diff --git a/lib/src/main/cpp/rtk_platform/platform.cpp b/lib/src/main/cpp/rtk_platform/platform.cpp
index 4f9f0bb..48af7fa 100644
--- a/lib/src/main/cpp/rtk_platform/platform.cpp
+++ b/lib/src/main/cpp/rtk_platform/platform.cpp
@@ -22,6 +22,7 @@
#include "../utils/num.h"
#include "../driver_test.h"
#include "../test_common/car_sensor.h"
+#include "../test_items2/stop_car.h"
#define PARSE_BUFF_SIZE 4096
@@ -432,6 +433,7 @@
}
if (events & PLAY_TTS_DONE_EVT) {
DummyLightTTSDone(*((int *)data));
+ StopCarTTSDone(*((int *)data));
}
}
diff --git a/lib/src/main/cpp/test_common/car_sensor.cpp b/lib/src/main/cpp/test_common/car_sensor.cpp
index a7a4692..ea4c242 100644
--- a/lib/src/main/cpp/test_common/car_sensor.cpp
+++ b/lib/src/main/cpp/test_common/car_sensor.cpp
@@ -23,7 +23,6 @@
SENSOR_RIGHT_TURN_SIGNAL,
SENSOR_HANDBREAK,
SENSOR_BREAK,
- SENSOR_CLEARANCE_LIGHT,
SENSOR_DIPPED_BEAM_LIGHT,
SENSOR_MAIN_BEAM_LIGHT,
SENSOR_DOOR,
@@ -35,6 +34,13 @@
SENSOR_SHIFT_5,
SENSOR_SHIFT_R,
SENSOR_ENGINE_START,
+ SENSOR_BREAK2,
+ SENSOR_SURROUND_CAR_1,
+ SENSOR_SURROUND_CAR_2,
+ SENSOR_SURROUND_CAR_3,
+ SENSOR_SURROUND_CAR_4,
+ SENSOR_FOG_LIGHT,
+ SENSOR_CLEARANCE_LIGHT,
SENSOR_ENGINE_RPM,
SENSOR_SPEED
};
@@ -137,8 +143,6 @@
}
}
}
-
-
}
int ReadCarStatus(uint16_t id)
@@ -194,6 +198,8 @@
static void SensorChanged(int id, int value)
{
+ DEBUG("鐘舵�佹敼鍙� %d = %d", id, value);
+
switch (id) {
case SENSOR_LEFT_TURN_SIGNAL: {
left_turn_signal = value;
@@ -220,6 +226,14 @@
} else {
AppTimer_add(ConfirmTurnSigalLater, 200, RIGHT_TURN_LIGHT);
}
+ }
+ break;
+ }
+ case SENSOR_FOG_LIGHT: {
+ if (value == 0) {
+ WriteCarStatus(FOG_LAMP, OFF_LIGHT);
+ } else {
+ WriteCarStatus(FOG_LAMP, FOG_LIGHT);
}
break;
}
@@ -250,7 +264,7 @@
WriteCarStatus(FLASH_BEAM_LAMP, FLASH_BEAM_LIGHT);
AppTimer_delete(flashBeamLightClose);
- AppTimer_add(flashBeamLightClose, D_SEC(5));
+ AppTimer_add(flashBeamLightClose, D_SEC(2));
}
AppTimer_delete(confirmFlashBeamLightLater);
@@ -266,6 +280,87 @@
break;
}
case SENSOR_ENGINE_START: {
+ if (value == 0) {
+ WriteCarStatus(ENGINE_START, ENGINE_START_INACTIVE);
+ } else {
+ WriteCarStatus(ENGINE_START, ENGINE_START_ACTIVE);
+ }
+ break;
+ }
+ case SENSOR_BREAK: {
+ if (value == 0) {
+ WriteCarStatus(BREAK, BREAK_INACTIVE);
+ } else {
+ WriteCarStatus(BREAK, BREAK_ACTIVE);
+ }
+ break;
+ }
+ case SENSOR_BREAK2: {
+ if (value == 0) {
+ WriteCarStatus(SECOND_BREAK, BREAK_INACTIVE);
+ } else {
+ WriteCarStatus(SECOND_BREAK, BREAK_ACTIVE);
+ }
+ break;
+ }
+ case SENSOR_HANDBREAK: {
+ if (value == 0) {
+ WriteCarStatus(HAND_BREAK, BREAK_INACTIVE);
+ } else {
+ WriteCarStatus(HAND_BREAK, BREAK_ACTIVE);
+ }
+ break;
+ }
+ case SENSOR_DOOR: {
+ if (value == 0) {
+ WriteCarStatus(DOOR, DOOR_OPEN);
+ } else {
+ WriteCarStatus(DOOR, DOOR_CLOSE);
+ }
+ break;
+ }
+ case SENSOR_SURROUND_CAR_1: {
+ if (value == 0) {
+ WriteCarStatus(SURROUND_CAR_1, SURROUND_CAR_INACTIVE);
+ } else {
+ WriteCarStatus(SURROUND_CAR_1, SURROUND_CAR_ACTIVE);
+ }
+ break;
+ }
+ case SENSOR_SURROUND_CAR_2: {
+ if (value == 0) {
+ WriteCarStatus(SURROUND_CAR_2, SURROUND_CAR_INACTIVE);
+ } else {
+ WriteCarStatus(SURROUND_CAR_2, SURROUND_CAR_ACTIVE);
+ }
+ break;
+ }
+ case SENSOR_SURROUND_CAR_3: {
+ if (value == 0) {
+ WriteCarStatus(SURROUND_CAR_3, SURROUND_CAR_INACTIVE);
+ } else {
+ WriteCarStatus(SURROUND_CAR_3, SURROUND_CAR_ACTIVE);
+ }
+ break;
+ }
+ case SENSOR_SURROUND_CAR_4: {
+ if (value == 0) {
+ WriteCarStatus(SURROUND_CAR_4, SURROUND_CAR_INACTIVE);
+ } else {
+ WriteCarStatus(SURROUND_CAR_4, SURROUND_CAR_ACTIVE);
+ }
+ break;
+ }
+ case SENSOR_SHIFT_N:
+ case SENSOR_SHIFT_1:
+ case SENSOR_SHIFT_2:
+ case SENSOR_SHIFT_3:
+ case SENSOR_SHIFT_4:
+ case SENSOR_SHIFT_5:
+ case SENSOR_SHIFT_R: {
+ if (value != 0) {
+ WriteCarStatus(GEAR, id - SENSOR_SHIFT_N + GEAR_N);
+ }
break;
}
default:
diff --git a/lib/src/main/cpp/test_common/car_sensor.h b/lib/src/main/cpp/test_common/car_sensor.h
index b6e65af..20695f7 100644
--- a/lib/src/main/cpp/test_common/car_sensor.h
+++ b/lib/src/main/cpp/test_common/car_sensor.h
@@ -7,6 +7,7 @@
#include <cstdint>
+// Name
enum {
OBD_SPEED,
ENGINE_RPM,
@@ -23,15 +24,22 @@
HAND_BREAK,
SECOND_BREAK,
DOOR,
+ SURROUND_CAR_1,
+ SURROUND_CAR_2,
+ SURROUND_CAR_3,
+ SURROUND_CAR_4,
CAR_STATUS_END //////////////
};
+// Value
enum {
OFF_LIGHT = 0,
EJECT_SEATBELT = 0,
ENGINE_START_INACTIVE = 0,
BREAK_INACTIVE = 0,
DOOR_OPEN = 0,
+ SURROUND_CAR_INACTIVE = 0,
+
HAZARD_LIGHTS,
LEFT_TURN_LIGHT,
RIGHT_TURN_LIGHT,
@@ -43,14 +51,15 @@
INSERT_SEATBELT,
ENGINE_START_ACTIVE,
GEAR_N,
- SHIFT_R,
GEAR_1,
GEAR_2,
GEAR_3,
GEAR_4,
GEAR_5,
+ GEAR_R,
BREAK_ACTIVE,
- DOOR_CLOSE
+ DOOR_CLOSE,
+ SURROUND_CAR_ACTIVE
};
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
index 9286e0f..ee94653 100644
--- a/lib/src/main/cpp/test_items2/drive_straight.cpp
+++ b/lib/src/main/cpp/test_items2/drive_straight.cpp
@@ -11,6 +11,9 @@
#define DEBUG(fmt, args...) LOGD("<drive_straight> <%s>: " fmt, __func__, ##args)
+static const double CHECK_STAGE_DISTANCE = 100.0;
+static const double MAX_OFFSET_DISTANCE = 0.3;
+
static bool crossStartLine;
static bool reportOffsetOver;
static double edgeDistance;
@@ -49,20 +52,25 @@
p2.Y = RoadMapList[index].stopLine.Y2;
if (IntersectionOfLine(p1, p2, car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1) {
+ DEBUG("璺ㄨ繃鏍囪绾匡紝寮�濮嬬洿绾挎祴璇�");
crossStartLine = true;
startPoint = car->basePoint;
edgeDistance = DistanceOf(car->basePoint, road_edge);
}
} else {
- double runDistance;
- runDistance = DistanceOf(car->basePoint, startPoint);
+ double distanceToStart = DistanceOf(car->basePoint, startPoint);
- if (runDistance < 100.0) {
- if (!reportOffsetOver && fabs(DistanceOf(car->basePoint, road_edge) - edgeDistance) > 0.3) {
+ if (distanceToStart > CHECK_STAGE_DISTANCE) {
+ if (!reportOffsetOver && fabs(DistanceOf(car->basePoint, road_edge) - edgeDistance) > MAX_OFFSET_DISTANCE) {
DEBUG("鐩寸嚎鍋忕Щ澶т簬30鍘樼背");
// 鍋忕Щ澶т簬30鍘樼背锛屼笉鍚堟牸
AddExamFault(30, rtkTime);
reportOffsetOver = true;
+
+ //////////////////////////////////////////////
+ startPoint = car->basePoint;
+ edgeDistance = DistanceOf(car->basePoint, road_edge);
+ reportOffsetOver = false;
}
} else {
startPoint = car->basePoint;
diff --git a/lib/src/main/cpp/test_items2/dummy_light.cpp b/lib/src/main/cpp/test_items2/dummy_light.cpp
index 3f7298d..9c7fc71 100644
--- a/lib/src/main/cpp/test_items2/dummy_light.cpp
+++ b/lib/src/main/cpp/test_items2/dummy_light.cpp
@@ -25,6 +25,7 @@
static int checkCnt;
static bool turn_left_active, flash_beam_active;
+
static int examTtsSeq = 0;
static bool testing;
@@ -43,6 +44,8 @@
content[i].itemStatus = TTS_NOT_START;
}
testing = true;
+
+ AppTimer_delete(DummyLightCheckActive);
AppTimer_delete(ExamDummyLight);
AppTimer_add(ExamDummyLight, D_SEC(2));
}
@@ -71,9 +74,11 @@
static void DummyLightCheckActive(union sigval sig)
{
- DEBUG("DummyLightCheckActive item = %d", sig.sival_int);
+ int active = sig.sival_int;
+ AppTimer_delete(DummyLightCheckActive);
+ DEBUG("DummyLightCheckActive item = %d", active);
- switch (sig.sival_int) {
+ switch (active) {
case DRIVE_AT_NIGHT:
case TURN_ON_MAIN_BEAM_LAMP:
if (ReadCarStatus(MAIN_BEAM_LAMP) != MAIN_BEAM_LIGHT) {
@@ -95,8 +100,16 @@
case THROUGE_CROSSWALK:
case THROUGE_CURVE:
case THROUGE_CROSSROADS:
- if (ReadCarStatus(FLASH_BEAM_LAMP) != FLASH_BEAM_LIGHT) {
- AddExamFault(58, &currRtkTime);
+ if (++checkCnt < 5) {
+ if (ReadCarStatus(FLASH_BEAM_LAMP) == FLASH_BEAM_LIGHT) {
+ flash_beam_active = true;
+ }
+ AppTimer_add(DummyLightCheckActive, D_SEC(1), active);
+ return;
+ } else {
+ if (!flash_beam_active) {
+ AddExamFault(58, &currRtkTime);
+ }
}
break;
case OVERTAKE:
@@ -130,7 +143,7 @@
}
for (int i = 0; i < contentNum; ++i) {
- if (content[i].item == sig.sival_int) {
+ if (content[i].item == active) {
content[i].itemStatus = CHECK_OPERATE;
break;
}
@@ -161,8 +174,11 @@
checkCnt = 0;
turn_left_active = flash_beam_active = false;
AppTimer_add(DummyLightCheckActive, D_SEC(1), content[i].item);
- }
- else if (content[i].item >= 100)
+ } else if (content[i].item == THROUGE_CROSSWALK || content[i].item == THROUGE_CURVE || content[i].item == THROUGE_CROSSROADS) {
+ checkCnt = 0;
+ 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);
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index b158dd5..b280802 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -13,6 +13,7 @@
#include "through_something.h"
#include "../master/comm_if.h"
#include "drive_straight.h"
+#include "stop_car.h"
#include <vector>
#include <list>
@@ -55,13 +56,7 @@
static bool slideNormalDistance;
static bool occurSlide;
-static struct drive_timer {
- int hour;
- int min;
- int sec;
- int msec;
-} crashGreenRunTime, crashGreenCmpTime, crashGreenStartTime, turnSignalChangeTime;
-
+static struct drive_timer crashGreenRunTime, crashGreenCmpTime, crashGreenStartTime, turnSignalChangeTime;
static struct drive_timer gearErrorTimePoint;
static struct drive_timer gearNSlideTimePoint;
@@ -85,7 +80,7 @@
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, int &ang);
static char CheckCarTurn(LIST_CAR_MODEL &CarModelList);
static bool CrashRedLine(LIST_ROAD_MAP &RoadMapList, const car_model *car);
@@ -494,22 +489,29 @@
StartThroughExam(currExamMapIndex, RoadMapList);
} else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) {
StartDriveStraightExam(currExamMapIndex, RoadMapList);
+ } else if (RoadMapList[currExamMapIndex].type == STOP_CAR_MAP) {
+ StartStopCarExam(currExamMapIndex, RoadMapList);
}
}
} else if (startCar == START_CAR_DONE) {
int prevIdx = currExamMapIndex;
- if (currExamMapIndex >= THROUGH_INTERSECTION_MAP && currExamMapIndex <= TURN_AROUND_MAP) {
+ if (RoadMapList[currExamMapIndex].type >= THROUGH_INTERSECTION_MAP && currExamMapIndex <= TURN_AROUND_MAP) {
currExamMapIndex = ExecuteThroughExam(currExamMapIndex, RoadMapList, car,
CarModelList, speed, moveDirect, rtkTime);
}
- else if (currExamMapIndex == DRIVE_STRAIGHT_MAP) {
+ else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) {
currExamMapIndex = ExecuteDriveStraightExam(currExamMapIndex, RoadMapList, car,
CarModelList, speed, moveDirect, rtkTime);
+ } else if (RoadMapList[currExamMapIndex].type == STOP_CAR_MAP) {
+ currExamMapIndex = ExecuteStopCarExam(currExamMapIndex, RoadMapList, car,
+ CarModelList, speed, moveDirect, rtkTime);
}
if (currExamMapIndex == -1) {
- MA_EnterMap(RoadMapList[prevIdx].id, RoadMapList[prevIdx].type, 1);
+ DEBUG("绂诲紑鍖哄煙 index %d id %d type %d", prevIdx, RoadMapList[prevIdx].id, RoadMapList[prevIdx].type);
+
+ MA_EnterMap(RoadMapList[prevIdx].id, RoadMapList[prevIdx].type, 0);
}
}
}
@@ -596,7 +598,7 @@
return ret;
}
-static void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
+void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
{
tm.hour = rtkTime->hh;
tm.min = rtkTime->mm;
diff --git a/lib/src/main/cpp/test_items2/road_exam.h b/lib/src/main/cpp/test_items2/road_exam.h
index 2b9d29c..ec4f712 100644
--- a/lib/src/main/cpp/test_items2/road_exam.h
+++ b/lib/src/main/cpp/test_items2/road_exam.h
@@ -19,6 +19,14 @@
#define THROUGH_BUS_STATION_MAP 109
#define TURN_AROUND_MAP 110
+struct drive_timer {
+ int hour;
+ int min;
+ int sec;
+ int msec;
+};
+
+void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime);
void InitRoadExam(void);
void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);
bool ExitSonArea(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car);
diff --git a/lib/src/main/cpp/test_items2/stop_car.cpp b/lib/src/main/cpp/test_items2/stop_car.cpp
new file mode 100644
index 0000000..7630016
--- /dev/null
+++ b/lib/src/main/cpp/test_items2/stop_car.cpp
@@ -0,0 +1,206 @@
+//
+// Created by YY on 2020/3/24.
+//
+
+#include "stop_car.h"
+#include "../driver_test.h"
+#include "../native-lib.h"
+#include "../jni_log.h"
+#include "road_exam.h"
+#include "../utils/xconvert.h"
+#include "../common/apptimer.h"
+#include "../test_common/car_sensor.h"
+#include "../defs.h"
+
+#define DEBUG(fmt, args...) LOGD("<stop_car> <%s>: " fmt, __func__, ##args)
+
+static bool inactiveBreakHandbreakTogether, notCloseEngine, inactiveHandBreakAfterOpenDoor, occurOpenDoor, doorNotClose, checkRoadDistance;
+static int examTtsSeq = 0;
+static int ttsPlayEnd;
+static double moveDistance;
+static double prevSpeed;
+static struct drive_timer prevPointTime;
+static int prevMoveDirect;
+static uint32_t stopTimepoint = 0;
+static uint32_t openDoorTimepoint;
+
+const int ENGINE_MIN_ROTATE = 200;
+
+static const uint32_t STOP_CAR_TIME = D_SEC(2);
+static const uint32_t OPEN_DOOR_TIMEOUT = D_SEC(15);
+static const double DISTANCE_TO_ROAD_EDGE_1 = 0.5;
+static const double DISTANCE_TO_ROAD_EDGE_2 = 0.3;
+
+static void PlayTTSTimeout(union sigval sig);
+
+void StartStopCarExam(int index, LIST_ROAD_MAP &RoadMapList) {
+ if (index == -1)
+ return;
+
+ DEBUG("杩涘叆闈犺竟鍋滆溅鍦板浘 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type);
+
+ ttsPlayEnd = 0;
+ moveDistance = 0;
+ prevMoveDirect = 0;
+ inactiveBreakHandbreakTogether = false;
+ notCloseEngine = false;
+ inactiveHandBreakAfterOpenDoor = false;
+ occurOpenDoor = false;
+ doorNotClose = false;
+ checkRoadDistance = false;
+
+ if (!RoadMapList[index].tts.empty()) {
+ examTtsSeq = PlayTTS(RoadMapList[index].tts.c_str());
+ } else {
+ examTtsSeq = PlayTTS("璇烽潬杈瑰仠杞�");
+ }
+
+ AppTimer_delete(PlayTTSTimeout);
+ AppTimer_add(PlayTTSTimeout, D_SEC(8));
+}
+
+void StopCarTTSDone(int id)
+{
+ DEBUG("DummyLightTTSDone %d", id);
+ // 绛夎闊虫挱鎶ュ畬姣曞悗璁℃椂
+ if (id == examTtsSeq) {
+ ttsPlayEnd = 1;
+ }
+}
+
+int ExecuteStopCarExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
+ LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) {
+ if (ttsPlayEnd == 1) {
+ ttsPlayEnd = 2;
+ prevSpeed = speed;
+ Rtk2DriveTimer(prevPointTime, rtkTime);
+ }
+
+ if (ttsPlayEnd != 2)
+ return index;
+
+ uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
+ rtkTime->mss * 10,
+ prevPointTime.hour, prevPointTime.min,
+ prevPointTime.sec, prevPointTime.msec * 10);
+
+ moveDistance += (double)diff * (speed + prevSpeed) / 2;
+ prevSpeed = speed;
+ Rtk2DriveTimer(prevPointTime, rtkTime);
+
+ if (moveDistance > 150) {
+ // 150绫冲唴鏈仠杞︼紝涓嶅悎鏍�
+ DEBUG("鍋滆溅璺濈瓒呮爣");
+ AddExamFault(33, rtkTime);
+ return -1;
+ }
+
+ if (moveDirect != prevMoveDirect) {
+ if (moveDirect == 0) {
+ stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
+ DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
+ }
+ prevMoveDirect = moveDirect;
+ } else if (moveDirect == 0) {
+ uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
+
+ if (tp - stopTimepoint >= STOP_CAR_TIME && !checkRoadDistance) {
+ // 鍋滆溅瓒�2绉掞紝寮�濮嬪垽鏂�
+ DEBUG("妫�娴嬪拰璺竟鐨勮窛绂�");
+ PointF pc;
+
+ checkRoadDistance = true;
+
+ pc.X = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].X + car->carXY[car->right_rear_tire[TIRE_OUTSIDE]].X) / 2;
+ pc.Y = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].Y + car->carXY[car->right_rear_tire[TIRE_OUTSIDE]].Y) / 2;
+
+ // 妫�娴嬮亾璺竟缂樼嚎
+ for (int i = 0; i < RoadMapList[index].roadEdgeLineNum; ++i) {
+ PointF p1, p2, pv;
+
+ p1 = RoadMapList[index].roadEdgeLine[i].point[0];
+ for (int j = 1; j < RoadMapList[index].roadEdgeLine[i].num; ++j) {
+ p2 = RoadMapList[index].roadEdgeLine[i].point[j];
+ pv = GetVerticalPoint(p1, p2, pc);
+
+ if (isEqual2(pv.X, MIN(p1.X, p2.X)) || isEqual2(pv.X, MAX(p1.X, p2.X)) ||
+ (pv.X > MIN(p1.X, p2.X) && pv.X < MAX(p1.X, p2.X))) {
+ double dis2roadEdge = DistanceOf(pc, pv);
+
+ DEBUG("鍋滆溅璺濊矾杈硅窛绂� %f", dis2roadEdge);
+
+ if (dis2roadEdge > DISTANCE_TO_ROAD_EDGE_1) {
+ DEBUG("鍋滆溅瓒呭嚭璺竟0.5绫�");
+ // 鍋滆溅璺濈瓒呰繃50鍘樼背锛屼笉鍚堟牸
+ AddExamFault(36, rtkTime);
+ } else if (dis2roadEdge > DISTANCE_TO_ROAD_EDGE_2) {
+ DEBUG("鍋滆溅瓒呭嚭璺竟0.3绫�");
+ // 鍋滆溅璺濈瓒呰繃30鍘樼背锛屾墸10鍒�
+ AddExamFault(37, rtkTime);
+ }
+ }
+
+ p1 = p2;
+ }
+ }
+ }
+ }
+
+ if (!inactiveBreakHandbreakTogether && ReadCarStatus(BREAK) == BREAK_INACTIVE && ReadCarStatus(HAND_BREAK) == BREAK_INACTIVE) {
+ // 鎷夋墜鍒瑰墠锛屾澗鑴氬埞锛屾墸10鍒�
+ DEBUG("鎷夋墜鍒瑰墠锛屾澗鑴氬埞");
+ AddExamFault(39, rtkTime);
+ inactiveBreakHandbreakTogether = true;
+ }
+
+ if (!notCloseEngine && ReadCarStatus(ENGINE_RPM) < ENGINE_MIN_ROTATE && ReadCarStatus(DOOR) == DOOR_OPEN) {
+ // 涓嬭溅鍓嶏紝涓嶇唲鐏紝鎵�5鍒�
+ DEBUG("涓嬭溅鍓嶏紝涓嶇唲鐏�");
+ AddExamFault(40, rtkTime);
+
+ notCloseEngine = true;
+ }
+
+ if (!inactiveHandBreakAfterOpenDoor && ReadCarStatus(DOOR) == DOOR_OPEN && ReadCarStatus(HAND_BREAK) == BREAK_INACTIVE) {
+ // 寮�闂ㄥ墠锛屾湭鎷夋墜鍒癸紝 鎵�10鍒�
+ DEBUG("寮�闂ㄥ墠锛屾湭鎷夋墜鍒�");
+ AddExamFault(38, rtkTime);
+
+ inactiveHandBreakAfterOpenDoor = true;
+ }
+
+ if (ReadCarStatus(DOOR) == DOOR_OPEN) {
+ if (!occurOpenDoor) {
+ occurOpenDoor = true;
+ openDoorTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
+ } else {
+ uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
+
+ if (!doorNotClose && tp - openDoorTimepoint >= OPEN_DOOR_TIMEOUT) {
+ // 寮�闂ㄦ椂闂磋秴杩�15绉掞紝涓嶅悎鏍�
+ DEBUG("寮�闂ㄦ椂闂磋秴杩�15绉�");
+ AddExamFault(35, rtkTime);
+ doorNotClose = true;
+ }
+ }
+ } else if (occurOpenDoor) {
+ return -1;
+ }
+
+ if (occurOpenDoor) {
+ uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
+
+ if (tp - openDoorTimepoint >= OPEN_DOOR_TIMEOUT + D_SEC(5)) {
+ return -1;
+ }
+ }
+
+ return index;
+}
+
+static void PlayTTSTimeout(union sigval sig)
+{
+ AppTimer_delete(PlayTTSTimeout);
+
+ ttsPlayEnd = 1;
+}
diff --git a/lib/src/main/cpp/test_items2/stop_car.h b/lib/src/main/cpp/test_items2/stop_car.h
new file mode 100644
index 0000000..fe2b92a
--- /dev/null
+++ b/lib/src/main/cpp/test_items2/stop_car.h
@@ -0,0 +1,15 @@
+//
+// Created by YY on 2020/3/24.
+//
+
+#ifndef MYAPPLICATION2_STOP_CAR_H
+#define MYAPPLICATION2_STOP_CAR_H
+
+#include "../driver_test.h"
+
+void StartStopCarExam(int index, LIST_ROAD_MAP &RoadMapList);
+void StopCarTTSDone(int id);
+int ExecuteStopCarExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
+ LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);
+
+#endif //MYAPPLICATION2_STOP_CAR_H
--
Gitblit v1.8.0