From dc2a01d4764efd33a23afcaf4f1d7543dc35c4fa Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期三, 19 二月 2020 15:32:57 +0800
Subject: [PATCH] 上坡地图修正
---
lib/src/main/cpp/rtk_platform/parse_net.cpp | 10
lib/src/main/cpp/driver_test.cpp | 161 ++++++---
lib/src/main/cpp/test_items/park_bottom.h | 5
lib/src/main/cpp/test_items/stop_and_start.cpp | 10
lib/src/main/cpp/test_items/comm_test.h | 40 ++
lib/src/main/cpp/rtk_module/rtk.cpp | 18
lib/src/main/cpp/driver_test.h | 6
lib/src/main/cpp/master/comm_if.h | 6
lib/src/main/cpp/test_items/park_bottom.cpp | 471 +++++++++++----------------
lib/src/main/cpp/test_items/turn_a90.cpp | 16
lib/src/main/cpp/test_items/park_edge.cpp | 22
lib/src/main/cpp/rtk_platform/platform.h | 1
lib/src/main/cpp/mcu/mcu_if.h | 1
lib/src/main/cpp/master/comm_if.cpp | 23 +
lib/src/main/cpp/test_items/comm_test.cpp | 156 +++++++-
lib/src/main/cpp/mcu/mcu_if.cpp | 30 +
lib/src/main/cpp/rtk_platform/platform.cpp | 43 +-
17 files changed, 600 insertions(+), 419 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index ca588b9..58f861b 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -26,6 +26,7 @@
#include "test_items/stop_and_start.h"
#include "master/comm_if.h"
#include "utils/xconvert.h"
+#include "test_items/comm_test.h"
#define DEBUG(fmt, args...) LOGD("<driver_test> <%s>: " fmt, __func__, ##args)
@@ -87,9 +88,14 @@
} SensorConfig[32];
static int SensorNum = 0;
+static int SensorValidLevel;
+
#define MOV_AVG_SIZE 1
#define RTK_BUFFER_SIZE 100
#define CAR_MODEL_CACHE_SIZE 10
+
+static pthread_mutex_t rtk_clock_mutex = PTHREAD_MUTEX_INITIALIZER;
+struct RtkTime rtkClock;
static rtk_info *RtkBuffer = NULL;
static int RtkBufferNum = 0, RtkBufferIn = 0;
@@ -106,14 +112,20 @@
void DriverTestInit(void)
{
+ pthread_mutex_init(&rtk_clock_mutex, NULL);
+ memset(&rtkClock, 0, sizeof(rtkClock));
+
TestStart = false;
memset(&MapList, 0, sizeof(MapList));
MapNum = 0;
CarModel = NULL;
CarModelPrev = NULL;
+ CommTestInit();
SensorNum = 0;
memset(SensorConfig, 0, sizeof(SensorConfig));
+
+ SensorValidLevel = 0;
RtkBuffer = (rtk_info *) malloc(RTK_BUFFER_SIZE * sizeof(rtk_info));
RtkBufferNum = RtkBufferIn = 0;
@@ -299,22 +311,43 @@
void SetSensorCfg(int (*sensor)[3], int sensorNum)
{
- DEBUG("SetSensorCfg sensorNum %d", sensorNum);
+ DEBUG("鍔犲叆浼犳劅鍣ㄩ厤缃� sensorNum %d", sensorNum);
+ SensorValidLevel = 0;
SensorNum = sensorNum;
for (int i = 0; i < sensorNum; ++i) {
SensorConfig[i].gpioId = sensor[i][0];
SensorConfig[i].funId = sensor[i][1];
SensorConfig[i].validLvl = sensor[i][2];
+
+ if (sensor[i][2] != 0) {
+ SensorValidLevel |= BV(i);
+ }
}
}
-void GetFuncGpio(int func, int &gpio, int &lvl)
+int GetSensorValidLevel(void)
{
+ return SensorValidLevel;
+}
+
+void GetSensorCfg(int gpio, int &func, bool &lvl)
+{
+ for (int i = 0; i < SensorNum; ++i) {
+ if (SensorConfig[i].gpioId == gpio) {
+ func = SensorConfig[i].funId;
+ lvl = SensorConfig[i].validLvl == 0 ? false : true;
+ }
+ }
+}
+
+void FindSensorCfg(int func, int &gpio, bool &lvl)
+{
+ gpio = -1;
for (int i = 0; i < SensorNum; ++i) {
if (SensorConfig[i].funId == func) {
gpio = SensorConfig[i].gpioId;
- lvl = SensorConfig[i].validLvl;
+ lvl = SensorConfig[i].validLvl == 0 ? false : true;
}
}
}
@@ -326,6 +359,7 @@
DEBUG("StartDriverExam %d", start);
if (start == 0) {
TestStart = false;
+ CommTestStart(false);
MA_SendExamStatus(0, 0);
return;
}
@@ -344,13 +378,31 @@
examFaultIndex = 0;
TestStart = true;
+ CommTestStart(true);
}
MA_SendExamStatus(1, 0);
}
}
+void GetRtkClock(struct RtkTime *s)
+{
+ pthread_mutex_lock(&rtk_clock_mutex);
+ *s = rtkClock;
+ pthread_mutex_unlock(&rtk_clock_mutex);
+}
+
void UpdateRTKInfo(const rtk_info *s)
{
+ pthread_mutex_lock(&rtk_clock_mutex);
+ rtkClock.YY = s->YY;
+ rtkClock.MM = s->MM;
+ rtkClock.DD = s->DD;
+ rtkClock.hh = s->hh;
+ rtkClock.mm = s->mm;
+ rtkClock.ss = s->ss;
+ rtkClock.mss = s->dss;
+ pthread_mutex_unlock(&rtk_clock_mutex);
+
if (s->qf == 3) {
RtkBuffer[RtkBufferIn] = *s;
RtkBufferIn = (RtkBufferIn + 1) % RTK_BUFFER_SIZE;
@@ -372,7 +424,7 @@
RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);
brief.qf = RtkBuffer[index].qf;
- brief.map_id = 866;//GetMapId(CurrExamMapIndex, MapList, MapNum);
+ brief.map_id = 863;//GetMapId(CurrExamMapIndex, MapList, MapNum);
brief.move = move;
brief.speed = speed * 3.6;
brief.heading = RtkBuffer[index].heading;
@@ -431,7 +483,7 @@
DEBUG("杩涘叆鍊掕溅鍏ュ簱鍦哄湴");
MA_SendDebugInfo("杩涘叆鍊掕溅鍏ュ簱鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
- StartParkBottom();
+ StartParkBottom(move, &rtkTime);
CurrExamStatus = 0;
break;
case MAP_TYPE_STOP_START:
@@ -465,7 +517,7 @@
int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);
switch (mtype) {
case MAP_TYPE_PARK_BUTTOM:
- CurrExamStatus = TestParkBottom(ErrorList, &MapList[CurrExamMapIndex].map,
+ CurrExamStatus = TestParkBottom(&MapList[CurrExamMapIndex].map,
CarModel, CarModelPrev, speed, move, &rtkTime);
break;
case MAP_TYPE_STOP_START:
@@ -712,44 +764,46 @@
{
// 杞︾殑鏈�鍓嶇偣鏄惁杩涘叆鍦板浘
for (int i = 0; i < mapNum && car != NULL; ++i) {
- /*if (mapList[i].type == MAP_TYPE_STOP_START) {
- // 鏋勯�犺櫄鎷熺殑宸︿笂瑙掔偣
- double x9, y9, xo, yo;
-
- bool enter = false;
-
- xo = (mapList[i].map.point[0].X + mapList[i].map.point[7].X) / 2;
- yo = (mapList[i].map.point[0].Y + mapList[i].map.point[7].Y) / 2;
-
- x9 = 2*xo - mapList[i].map.point[8].X;
- y9 = 2*yo - mapList[i].map.point[8].Y;
-
- Polygon map;
-
- map.num = 4;
- map.point = (PointF *) malloc(map.num * sizeof(PointF));
-
- map.point[0] = mapList[i].map.point[0];
- map.point[1] = mapList[i].map.point[8];
- map.point[2] = mapList[i].map.point[7];
- map.point[3].X = x9;
- map.point[3].Y = y9;
-
- if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &map) == GM_Containment) {
- Line enterLine1;
-
- MakeLine(&enterLine1, &(map.point[0]), &(map.point[3]));
-
- if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
- enter = true;
- }
-
- free(map.point);
-
- if (enter) return i;
- } else if (mapList[i].type == MAP_TYPE_CURVE) {
-
- } else if (mapList[i].type == MAP_TYPE_PARK_BUTTOM || mapList[i].type == MAP_TYPE_PART_EDGE) {
+// if (mapList[i].type == MAP_TYPE_STOP_START) {
+// // 鏋勯�犺櫄鎷熺殑宸︿笂瑙掔偣
+// double x9, y9, xo, yo;
+//
+// bool enter = false;
+//
+// xo = (mapList[i].map.point[0].X + mapList[i].map.point[7].X) / 2;
+// yo = (mapList[i].map.point[0].Y + mapList[i].map.point[7].Y) / 2;
+//
+// x9 = 2*xo - mapList[i].map.point[8].X;
+// y9 = 2*yo - mapList[i].map.point[8].Y;
+//
+// Polygon map;
+//
+// map.num = 4;
+// map.point = (PointF *) malloc(map.num * sizeof(PointF));
+//
+// map.point[0] = mapList[i].map.point[0];
+// map.point[1] = mapList[i].map.point[8];
+// map.point[2] = mapList[i].map.point[7];
+// map.point[3].X = x9;
+// map.point[3].Y = y9;
+//
+// if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &map) == GM_Containment) {
+// Line enterLine1;
+//
+// MakeLine(&enterLine1, &(map.point[0]), &(map.point[3]));
+//
+// if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
+// enter = true;
+// }
+//
+// free(map.point);
+//
+// if (enter) return i;
+// }
+// if (mapList[i].type == MAP_TYPE_CURVE) {
+//
+// }
+ if (mapList[i].type == MAP_TYPE_PARK_BUTTOM /*|| mapList[i].type == MAP_TYPE_PART_EDGE*/) {
if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
Line enterLine1, enterLine2;
@@ -760,16 +814,17 @@
DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
return i;
}
- } else */if (mapList[i].type == MAP_TYPE_TURN_90) {
- if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
- Line enterLine1;
-
- MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
-
- if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
- return i;
- }
}
+// if (mapList[i].type == MAP_TYPE_TURN_90) {
+// if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
+// Line enterLine1;
+//
+// MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
+//
+// if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
+// return i;
+// }
+// }
}
return -1;
}
diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h
index 339469c..1e66de9 100644
--- a/lib/src/main/cpp/driver_test.h
+++ b/lib/src/main/cpp/driver_test.h
@@ -82,8 +82,12 @@
int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
int *body, int bodyNum, double (*point)[2], int pointNum);
void SetSensorCfg(int (*sensor)[3], int sensorNum);
-void GetFuncGpio(int func, int &gpio, int &lvl);
+int GetSensorValidLevel(void);
+void GetSensorCfg(int gpio, int &func, bool &lvl);
+void FindSensorCfg(int func, int &gpio, bool &lvl);
+
void StartDriverExam(int start);
+void GetRtkClock(struct RtkTime *s);
void UpdateRTKInfo(const rtk_info *s);
void AddExamFault(int wrong, const struct RtkTime *rtkTime);
car_model_cache_t *GetCarModelCache(int node);
diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp
index 688ffe2..5e2d033 100644
--- a/lib/src/main/cpp/master/comm_if.cpp
+++ b/lib/src/main/cpp/master/comm_if.cpp
@@ -49,6 +49,8 @@
#define ID_SM_RTCM_IND 0x000F
#define ID_SM_DEBUG_INFO 0x0010
#define ID_MS_FILE 0x8100
+#define ID_MS_READ_CARD 0x800F
+#define ID_SM_PUT_CARD 0x000F
#define MA_OUT_GPS_BRIEF 0x0001
#define MA_OUT_RTK_BRIEF 0x0002
@@ -135,7 +137,7 @@
void MA_ReadSensor(void)
{
-
+ ReadCard();
}
void MA_SendExamStatus(int start, int errorCode)
@@ -153,6 +155,21 @@
writer.EndObject();
SendMsgToMainProc(ID_SM_EXAM_STATUS, sb.GetString());
+}
+
+void MA_SendCardBrief(const struct cardBrief *brief)
+{
+ StringBuffer sb;
+ Writer<StringBuffer> writer(sb);
+
+ writer.StartObject();
+ writer.Key("result");
+ writer.Int(brief->result);
+ writer.Key("serialno");
+ writer.String(brief->card);
+ writer.EndObject();
+
+ SendMsgToMainProc(ID_SM_PUT_CARD, sb.GetString());
}
void MA_SendMcuBrief(const struct mcuBrief *brief)
@@ -688,6 +705,10 @@
}
break;
}
+ case ID_MS_READ_CARD : {
+ ReadCard();
+ break;
+ }
default:break;
}
}
diff --git a/lib/src/main/cpp/master/comm_if.h b/lib/src/main/cpp/master/comm_if.h
index deae21e..d6bbe84 100644
--- a/lib/src/main/cpp/master/comm_if.h
+++ b/lib/src/main/cpp/master/comm_if.h
@@ -17,6 +17,11 @@
char sn[20];
};
+struct cardBrief {
+ int result;
+ char card[32];
+};
+
struct gpsBrief {
char utc[32];
int sat_num;
@@ -70,6 +75,7 @@
void MA_ReadCar(void);
void MA_ReadSensor(void);
void MA_SendExamStatus(int start, int errorCode);
+void MA_SendCardBrief(const struct cardBrief *brief);
void MA_SendMcuBrief(const struct mcuBrief *brief);
void MA_SendGpsBrief(const struct gpsBrief *brief);
void MA_SendRtkBrief(const struct rtkBrief *brief);
diff --git a/lib/src/main/cpp/mcu/mcu_if.cpp b/lib/src/main/cpp/mcu/mcu_if.cpp
index 9821685..d9bb96a 100644
--- a/lib/src/main/cpp/mcu/mcu_if.cpp
+++ b/lib/src/main/cpp/mcu/mcu_if.cpp
@@ -72,6 +72,7 @@
static void SendDfuFile(int fileLen, int sentLen, int blockLen, const uint8_t *data);
static void GoNextDfuLater(union sigval sig);
static void GoNextDfu(void);
+static void ReadCardTimeout(union sigval sig);
void ParseMcuInit(void)
{
@@ -369,6 +370,10 @@
break;
case ID_MC_RFCARD_RSP:
DEBUG("ID_MC_RFCARD_RSP");
+ AppTimer_delete(ReadCardTimeout);
+
+ if (lenth > 0)
+ PlatformStatusChanged(CARD_UPDATE_EVT, data, lenth);
break;
default:
break;
@@ -484,3 +489,28 @@
GoNextDfu();
}
}
+
+static int readCartCnt = 0;
+
+static void ReadCardTimeout(union sigval sig) {
+ AppTimer_delete(ReadCardTimeout);
+
+ readCartCnt++;
+
+ if (readCartCnt < 2) {
+ AppTimer_add(ReadCardTimeout, D_SEC(3));
+ SendMcuCommand(ID_CM_READ_RFCARD, NULL, 0);
+ } else {
+ uint8_t data[8] = {0};
+ PlatformStatusChanged(CARD_UPDATE_EVT, data, sizeof(data));
+ }
+}
+
+void ReadCard(void)
+{
+ readCartCnt = 0;
+
+ AppTimer_delete(ReadCardTimeout);
+ AppTimer_add(ReadCardTimeout, D_SEC(3));
+ SendMcuCommand(ID_CM_READ_RFCARD, NULL, 0);
+}
diff --git a/lib/src/main/cpp/mcu/mcu_if.h b/lib/src/main/cpp/mcu/mcu_if.h
index 89c07c0..b4cd426 100644
--- a/lib/src/main/cpp/mcu/mcu_if.h
+++ b/lib/src/main/cpp/mcu/mcu_if.h
@@ -15,5 +15,6 @@
void UploadDfuFileEnd(void);
void UploadDfuFile(const uint8_t *file, int length);
+void ReadCard(void);
#endif //RTKDRIVERTEST_MCU_IF_H
diff --git a/lib/src/main/cpp/rtk_module/rtk.cpp b/lib/src/main/cpp/rtk_module/rtk.cpp
index f753214..b19a6e8 100644
--- a/lib/src/main/cpp/rtk_module/rtk.cpp
+++ b/lib/src/main/cpp/rtk_module/rtk.cpp
@@ -202,17 +202,17 @@
}*/
if (RxBufLen > 0) {
-// const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
-// if(ptr != RxBuf) {
-// memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf));
-// RxBufLen -= ptr - RxBuf;
-// } else if(RxBufLen == PARSE_BUFF_SIZE) { //濉弧浜嗭紝涓旀病鏈変竴涓猏r锛岄兘鎶涘純
-// DEBUG("Parse GPS error");
-// RxBufLen = 0;
-// }
+ const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
+ if(ptr != RxBuf) {
+ memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf));
+ RxBufLen -= ptr - RxBuf;
+ } else if(RxBufLen == PARSE_BUFF_SIZE) { //濉弧浜嗭紝涓旀病鏈変竴涓猏r锛岄兘鎶涘純
+ DEBUG("Parse GPS error");
+ RxBufLen = 0;
+ }
- RxBufLen = 0; /////////////////////////////
+// RxBufLen = 0; //PC妯℃嫙鐢ㄦ椂
}
}
if (res == 0) {
diff --git a/lib/src/main/cpp/rtk_platform/parse_net.cpp b/lib/src/main/cpp/rtk_platform/parse_net.cpp
index 352886d..09e2e1a 100644
--- a/lib/src/main/cpp/rtk_platform/parse_net.cpp
+++ b/lib/src/main/cpp/rtk_platform/parse_net.cpp
@@ -21,6 +21,8 @@
#define DEBUG(fmt, args...) LOGD("<parse_net> <%s>: " fmt, __func__, ##args)
+//#define ENABLE_DEBUG_PROTOCOL
+
using namespace std;
#define PKT_HEAD_CHAR 0x7E
@@ -718,7 +720,7 @@
}
#ifdef ENABLE_DEBUG_PROTOCOL
- /*{
+ {
static char buff[16384];
buff[0] = 0;
@@ -731,15 +733,15 @@
sprintf(buff + strlen(buff), "%02X ", item->data[i]);
if (strlen(buff) > 800) {
- DEBUG("%s -> %s...", item->access == DATA_ACCESS_MCU ? "UART" : "TCP" , buff);
+ DEBUG("DATAOUT: %s -> %s...", item->access == DATA_ACCESS_MCU ? "UART" : "TCP" , buff);
buff[0] = 0;
}
}
if (strlen(buff) > 0) {
- DEBUG("%s -> %s", item->access == DATA_ACCESS_MCU ? "UART" : "TCP", buff);
+ DEBUG("DATAOUT: %s -> %s", item->access == DATA_ACCESS_MCU ? "UART" : "TCP", buff);
}
- }*/
+ }
#endif
// 绯荤粺灞傜‘璁ゅ彂閫佹垚鍔�
item->curr_cnt += 1;
diff --git a/lib/src/main/cpp/rtk_platform/platform.cpp b/lib/src/main/cpp/rtk_platform/platform.cpp
index bd801f8..a5b4082 100644
--- a/lib/src/main/cpp/rtk_platform/platform.cpp
+++ b/lib/src/main/cpp/rtk_platform/platform.cpp
@@ -21,6 +21,7 @@
#include "../utils/xconvert.h"
#include "../utils/num.h"
#include "../driver_test.h"
+#include "../test_items/comm_test.h"
#define PARSE_BUFF_SIZE 4096
@@ -184,26 +185,6 @@
pthread_attr_init(&attr);
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);//detached
pthread_create(&pid, &attr, StatusListenThread, NULL);
-
-
- AddEvnet(100, (uint8_t *)"1234567890", 11);
- AddEvnet(101, (uint8_t *)"abcd", 5);
- AddEvnet(102, (uint8_t *)"xyz", 4);
- AddEvnet(104, NULL, 0);
- AddEvnet(105, (uint8_t *)"uio", 4);
-
- while (true) {
- struct event_t *evp = FetchEvent();
-
- if (evp != NULL) {
- DEBUG("寰楀埌event %d %d", evp->id, evp->length);
-
- if (evp->data != NULL)
- DEBUG("鏁版嵁 %s", evp->data);
- RemoveEvent();
- } else
- break;
- }
}
static void ReqRtkPlatformConfigTimeout(union sigval sig)
@@ -413,6 +394,28 @@
memcpy(brief.sn, data+40, 16);
MA_SendMcuBrief(&brief);
+
+ UpdateSensor(brief.gpio, brief.speed, brief.engine);
+ }
+ if (events & CARD_UPDATE_EVT) {
+ DEBUG("CARD_UPDATE_EVT length %d", length);
+
+ int ret = -1;
+
+ for (int i = 0; i < length; ++i) {
+ if (data[i] != 0) {
+ ret = 0;
+ break;
+ }
+ }
+
+ struct cardBrief brief;
+
+ brief.result = ret;
+
+ ConvertHex2String(brief.card, data, length);
+
+ MA_SendCardBrief(&brief);
}
}
diff --git a/lib/src/main/cpp/rtk_platform/platform.h b/lib/src/main/cpp/rtk_platform/platform.h
index 05898a5..5ad51b7 100644
--- a/lib/src/main/cpp/rtk_platform/platform.h
+++ b/lib/src/main/cpp/rtk_platform/platform.h
@@ -14,6 +14,7 @@
#define RTK_UPDATE_EVT 0x0010
#define GPS_UPDATE_EVT 0x0020
#define MCU_UPDATE_EVT 0x0040
+#define CARD_UPDATE_EVT 0x0080
typedef struct {
char domain_name[32];
diff --git a/lib/src/main/cpp/test_items/comm_test.cpp b/lib/src/main/cpp/test_items/comm_test.cpp
index 3ea8099..3ca6c44 100644
--- a/lib/src/main/cpp/test_items/comm_test.cpp
+++ b/lib/src/main/cpp/test_items/comm_test.cpp
@@ -5,20 +5,37 @@
#include "comm_test.h"
#include "../driver_test.h"
#include "../defs.h"
+#include "../common/apptimer.h"
static bool seatbeltInsert;
static bool engineStart;
const int ENGINE_MIN_ROTATE = 200;
+static bool commTest = false;
static uint16_t gpioStore = 0;
-static uint16_t engineStore = 0;
-static void SensorChange(int index, bool value);
+static void SensorChange(int gpio, bool value);
+static void EngineStartHold(union sigval sig);
-void CommTestStart(void)
+void CommTestInit(void)
{
- gpioStore = engineStore = 0;
+ gpioStore = 0;
+ engineStart = true;
+ commTest = false;
+}
+
+void CommTestStart(bool start)
+{
+ commTest = start;
+ if (start) {
+ // 妫�鏌ュ畨鍏ㄥ甫
+ if (CheckSensorX(SEATBELT) == 0) {
+ struct RtkTime rt;
+ GetRtkClock(&rt);
+ AddExamFault(1, &rt);
+ }
+ }
}
void UpdateSensor(uint16_t gpio, uint16_t speed, uint16_t engine)
@@ -27,37 +44,128 @@
uint16_t chg = gpioStore^gpio;
- if (chg == 0)
- return;
+ gpioStore = gpio;
- for (int i = 0; i < 16; ++i) {
- if (chg & BV(i)) {
- SensorChange(i, (bool)(gpio & BV(i)));
+ if (commTest) {
+ for (int i = 0; i < 16; ++i) {
+ if (chg & BV(i)) {
+ SensorChange(i, (bool) (gpio & BV(i)));
+ }
}
}
- gpioStore = gpio;
-
-
-
- // 瀹夊叏甯�
-
- // 鎸′綅
-
- // 鍚姩鎸囩ず
-
- // 鐔勭伀鐩戞帶
+ if (engine < ENGINE_MIN_ROTATE) {
+ if (engineStart) {
+ if (commTest) {
+ // 鐔勭伀1娆★紝鎵�10鍒�
+ struct RtkTime rt;
+ GetRtkClock(&rt);
+ AddExamFault(5, &rt);
+ }
+ engineStart = false;
+ }
+ } else {
+ engineStart = true;
+ }
}
-static void SensorChange(int index, bool value)
+int CheckSensorX(int func)
+{
+ int gpio;
+ bool level;
+ int validLevel = GetSensorValidLevel();
+
+ // 浼犳劅鍣ㄦ棤鏁堬紝璁や负閫氳繃
+ switch (func) {
+ case SEATBELT:
+ FindSensorCfg(SENSOR_SEATBELT, gpio, level);
+ if (gpio >= 0 && (gpioStore & BV(gpio)) != (validLevel & BV(gpio)) ) {
+ return 0;
+ }
+ return 1;
+ case LEFT_TURN_SIGNAL:
+ FindSensorCfg(SENSOR_LEFT_TURN_SIGNAL, gpio, level);
+ if (gpio == -1 || (gpioStore & BV(gpio)) == (validLevel & BV(gpio)) ) {
+ return 1;
+ }
+ return 0;
+ case RIGHT_TURN_SIGNAL:
+ FindSensorCfg(SENSOR_RIGHT_TURN_SIGNAL, gpio, level);
+ if (gpio == -1 || (gpioStore & BV(gpio)) == (validLevel & BV(gpio)) ) {
+ return 1;
+ }
+ return 0;
+ case HANDBREAK:
+ FindSensorCfg(SENSOR_HANDBREAK, gpio, level);
+ if (gpio == -1 || (gpioStore & BV(gpio)) == (validLevel & BV(gpio)) ) {
+ return 1;
+ }
+ return 0;
+ case SHIFT:
+ FindSensorCfg(SENSOR_SHIFT_N, gpio, level);
+ if (gpio == -1 || (gpioStore & BV(gpio)) == (validLevel & BV(gpio))) {
+ return 'N';
+ }
+ return 0;
+ default:
+ break;
+ }
+
+ return -1;
+}
+
+static void EngineStartHold(union sigval sig) {
+ AppTimer_delete(EngineStartHold);
+
+ // 涓嶅強鏃舵澗寮�鍚姩寮�鍏筹紝鎵�10鍒�
+ struct RtkTime rt;
+ GetRtkClock(&rt);
+ AddExamFault(4, &rt);
+}
+
+static void SensorChange(int gpio, bool value)
{
int func;
+ bool level;
+
+ GetSensorCfg(gpio, func, level);
switch (func) {
case SENSOR_SEATBELT:
- break;
- case SENSOR_TURNRIGHT:
- break;
+ if (level != value) {
+ // 涓嶄娇鐢ㄥ畨鍏ㄥ甫锛屼笉鍚堟牸
+ seatbeltInsert = false;
+ struct RtkTime rt;
+ GetRtkClock(&rt);
+ AddExamFault(1, &rt);
+ } else {
+ seatbeltInsert = true;
+ }
+ break;
+ case SENSOR_RIGHT_TURN_SIGNAL:
+ break;
+ case SENSOR_LEFT_TURN_SIGNAL:
+ break;
+ case SENSOR_HANDBREAK:
+ break;
+ case SENSOR_ENGINE_START:
+ AppTimer_delete(EngineStartHold);
+ if (level == value) {
+ AppTimer_add(EngineStartHold, D_SEC(2));
+
+ // 妫�鏌ユ槸鍚﹀湪绌烘尅
+ if (CheckSensorX(SHIFT) != 'N') {
+ // 涓嶆槸绌烘尅鐐圭伀锛屼笉鍚堟牸
+ struct RtkTime rt;
+ GetRtkClock(&rt);
+ AddExamFault(3, &rt);
+ }
+ } else {
+
+ }
+ break;
+ default:
+ break;
}
}
diff --git a/lib/src/main/cpp/test_items/comm_test.h b/lib/src/main/cpp/test_items/comm_test.h
index a1f7a5c..568ee32 100644
--- a/lib/src/main/cpp/test_items/comm_test.h
+++ b/lib/src/main/cpp/test_items/comm_test.h
@@ -5,10 +5,42 @@
#ifndef MYAPPLICATION2_COMM_TEST_H
#define MYAPPLICATION2_COMM_TEST_H
-#define SENSOR_SEATBELT 0
-#define SENSOR_TURNRIGHT 1
-#define SENSOR_TURNLEFT 2
-#define SENSOR_HANDBREAK 3
+#include <cstdint>
+enum {
+ SENSOR_SEATBELT,
+ SENSOR_LEFT_TURN_SIGNAL,
+ SENSOR_RIGHT_TURN_SIGNAL,
+ SENSOR_HANDBREAK,
+ SENSOR_BREAK,
+ SENSOR_LIGHT,
+ SENSOR_FAR_LIGHT,
+ SENSOR_DOOR,
+ SENSOR_SHIFT_N,
+ SENSOR_SHIFT_1,
+ SENSOR_SHIFT_2,
+ SENSOR_SHIFT_3,
+ SENSOR_SHIFT_4,
+ SENSOR_SHIFT_5,
+ SENSOR_SHIFT_R,
+ SENSOR_ENGINE_START
+};
+
+enum {
+ SEATBELT,
+ LEFT_TURN_SIGNAL,
+ RIGHT_TURN_SIGNAL,
+ HANDBREAK,
+ SHIFT,
+ LIGHT,
+ FAR_LIGHT,
+ DOOR,
+ ENGINE_START
+};
+
+void CommTestInit(void);
+void CommTestStart(bool start);
+void UpdateSensor(uint16_t gpio, uint16_t speed, uint16_t engine);
+int CheckSensorX(int func);
#endif //MYAPPLICATION2_COMM_TEST_H
diff --git a/lib/src/main/cpp/test_items/park_bottom.cpp b/lib/src/main/cpp/test_items/park_bottom.cpp
index 2b87406..79b0678 100644
--- a/lib/src/main/cpp/test_items/park_bottom.cpp
+++ b/lib/src/main/cpp/test_items/park_bottom.cpp
@@ -8,6 +8,7 @@
#include "../native-lib.h"
#include "../jni_log.h"
#include "../driver_test.h"
+#include "../utils/xconvert.h"
#include <vector>
#include <cstdlib>
@@ -24,317 +25,211 @@
THIRD_TOUCH_CTRL_LINE
};
-const int PARK_TIMEOUT = 210;
+const int PARK_TIMEOUT = D_SEC(210);
-static bool PBTesting = false;
-static bool trigLeaveTestAreaDetect = false;
-static bool leaveTestArea = false;
-static bool stopCar2S = false;
+static bool testing = false;
-static int currTarget;
-static bool leftTireCrossLeftLine, leftTireCrossRightLine, rightTireCrossLeftLine, rightTireCrossRightLine;
-static char first_ctrl_line_id;
-static bool carStopEvent; // 涓�斿仠杞︽爣璁�
-static bool carParkSuccess; // 鏄惁鍋滃湪搴撲綅
-static bool parkTimeout;
+const uint32_t STOP_CAR_TIME = D_SEC(2);
+static uint32_t stopTimepoint;
+static int prevMoveDirect, storeMoveDirectBeforeStop;
+static bool occurCrashRedLine;
+static bool checkPartStatus;
+static uint32_t firstReverseTimepoint;
+static bool reportExamTimeout, reportParkFail;
+static bool crossCtrlLineSw;
+static int parkCount;
+static char carray[3];
+static int darray[3];
+static int parkStatus[3];
-static void StopCarTimeout(union sigval sig);
-static void LeaveTestAreaLongtime(union sigval sig);
-static void CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car);
+static char CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car);
static bool EnterParking(const Polygon *map, const car_model *car);
-static void ParkBottomTimeout(union sigval sig);
static bool CrashRedLine(const Polygon *map, const car_model *car);
+static bool ExitParkArea(const Polygon *map, const car_model *car);
-void StartParkBottom(void)
+void StartParkBottom(int moveDirect, const struct RtkTime *rtkTime)
{
- stopCar2S = false;
- trigLeaveTestAreaDetect = false;
- leaveTestArea = false;
- PBTesting = true;
- parkTimeout = false;
- first_ctrl_line_id = 0;
- currTarget = FIRST_TOUCH_CTRL_LINE;
- leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;
-
DEBUG("StartParkBottom");
+ testing = true;
+ memset(carray, 0, sizeof(carray));
+ memset(darray, 0, sizeof(darray));
+ memset(parkStatus, 0, sizeof(parkStatus));
+ prevMoveDirect = moveDirect;
+
+ checkPartStatus = false;
+ firstReverseTimepoint = 0;
+ reportExamTimeout = false;
+ parkCount = 0;
+ crossCtrlLineSw = false;
+ reportParkFail = false;
+ occurCrashRedLine = false;
}
-void StopParkBottom(void)
+int TestParkBottom(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
- PBTesting = false;
- AppTimer_delete(StopCarTimeout);
- AppTimer_delete(ParkBottomTimeout);
- AppTimer_delete(LeaveTestAreaLongtime);
- currTarget = NONE;
+ char crossCtrlLine = 0;
+ uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- DEBUG("ParkBottom End");
-}
-
-int TestParkBottom(vector<int>&err, const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int run_status, const struct RtkTime *rtkTime)
-{
- int status = 0;
-
- if (!PBTesting)
- return -2;
-
- DEBUG("TestParkBottom speed %f dir %d", speed, run_status);
-
- if (currTarget > FIRST_TOUCH_CTRL_LINE) {
- // 鏄惁瓒呮椂
- if (parkTimeout) {
- // 涓嶅悎鏍硷細鍔ㄤ綔瓒呮椂
- DEBUG("涓嶅悎鏍硷細鍔ㄤ綔瓒呮椂");
- err.push_back(10);
- AddExamFault(10, rtkTime);
- status = -1;
+ if (ExitParkArea(map, car)) {
+ DEBUG("绂诲紑鍦哄湴");
+ // 绂诲紑鍦哄湴
+ testing = false;
+ if ((parkStatus[0] != 1 || parkStatus[1] != 1) && !reportParkFail) {
+ // 鍊掑簱涓嶅叆锛屼笉鍚堟牸
+ reportParkFail = true;
+ AddExamFault(8, rtkTime);
+ DEBUG("鍊掑簱涓嶅叆");
}
- // 鏄惁鍘嬬嚎
- if (CrashRedLine(map, car)) {
- // 涓嶅悎鏍硷細杞﹁韩鍑虹嚎
- DEBUG("涓嶅悎鏍硷細杞﹁韩鍑虹嚎");
- err.push_back(7);
+ goto TEST_END;
+ }
+
+ if (CrashRedLine(map, car)) {
+ if (!occurCrashRedLine) {
+ occurCrashRedLine = true;
+ // 杞﹁韩鍑虹嚎锛屼笉鍚堟牸
AddExamFault(7, rtkTime);
- status = -1;
+ DEBUG("杞﹁疆鍘嬬嚎");
}
-
- if (trigLeaveTestAreaDetect) {
- if (IntersectionOf(car->carXY[ car->left_front_tire[TIRE_OUTSIDE] ], map) == GM_Containment &&
- IntersectionOf(car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ], map) == GM_Containment) {
- trigLeaveTestAreaDetect = false;
- AppTimer_delete(LeaveTestAreaLongtime);
- }
- }
+ } else {
+ occurCrashRedLine = false;
}
- if (currTarget == FIRST_TOUCH_CTRL_LINE ||
- currTarget == SECOND_TOUCH_CTRL_LINE ||
- currTarget == THIRD_TOUCH_CTRL_LINE) {
- if (run_status > 0) {
- if (stopCar2S && currTarget != FIRST_TOUCH_CTRL_LINE) {
- // 鎵�5鍒嗭細涓�斿仠杞﹁秴杩�2绉�
- DEBUG("鎵�5鍒嗭細涓�斿仠杞﹁秴杩�2绉�");
- err.push_back(11);
- AddExamFault(11, rtkTime);
- }
-
- if (!((leftTireCrossLeftLine && rightTireCrossLeftLine) ||
- (leftTireCrossRightLine && rightTireCrossRightLine))) {
- DEBUG("CrossCtrlLine");
- CrossCtrlLine(map, car, carPrev);
- } else if (currTarget == FIRST_TOUCH_CTRL_LINE || currTarget == SECOND_TOUCH_CTRL_LINE) {
- // 璺ㄨ繃鎺у埗绾垮悗锛岃溅杈嗘寔缁悜鍓嶈椹讹紝澶勭悊杩欎簺涔辨悶鎯呭喌
- // 鏁翠釜杞﹂兘绂诲紑娴嬭瘯鍖哄悗锛屽鏋滄寔缁�15绉掞紝杩樻病鍥炲埌娴嬭瘯鍖猴紝灏卞拷鐣ヨ娴嬭瘯鎴栬�呮窐姹�
- if (leaveTestArea) {
- if (currTarget == FIRST_TOUCH_CTRL_LINE) {
- status = -2;
- DEBUG("鏉ラ亾鍦虹帺鐨�");
- } else {
- // 涓嶅悎鏍硷細鏈寜瑙勫畾绾胯矾琛岄┒锛堢洿鎺ヨ窇鍑烘祴璇曞尯浜嗭級
- DEBUG("涓嶅悎鏍硷細鏈寜瑙勫畾绾胯矾琛岄┒锛堢洿鎺ヨ窇鍑烘祴璇曞尯浜嗭級");
- err.push_back(6);
- AddExamFault(6, rtkTime);
- status = -1;
- DEBUG("鐩存帴璺戝嚭娴嬭瘯鍖轰簡");
- }
- } else if (!trigLeaveTestAreaDetect) {
- Polygon car_body;
-
- car_body.num = car->bodyNum;
- car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
- for (int i = 0; i < car_body.num; ++i) {
- car_body.point[i] = car->carXY[car->body[i]];
- }
-
- if (IntersectionOf(map, &car_body) == GM_None) {
- trigLeaveTestAreaDetect = true;
- AppTimer_delete(LeaveTestAreaLongtime);
- AppTimer_add(LeaveTestAreaLongtime, D_SEC(15));
- DEBUG("寮�濮嬬鍦鸿鏃�");
- }
-
- free(car_body.point);
- }
- }
-
- if (currTarget == THIRD_TOUCH_CTRL_LINE) {
- char the_ctrl_line_crossed = 0;
-
- if (leftTireCrossLeftLine && rightTireCrossLeftLine) {
- the_ctrl_line_crossed = 'L';
- } else if (leftTireCrossRightLine && rightTireCrossRightLine) {
- the_ctrl_line_crossed = 'R';
- }
-
- if (the_ctrl_line_crossed != 0 && the_ctrl_line_crossed == first_ctrl_line_id) {
- // 椤圭洰瀹屾垚
- status = 1;
- } else if (the_ctrl_line_crossed != 0) {
- // 涓嶅悎鏍硷細鏈寜瑙勫畾绾胯矾琛岄┒锛堟湭鍥炲埌璧峰鐐癸級
- DEBUG("涓嶅悎鏍硷細鏈寜瑙勫畾绾胯矾琛岄┒锛堟湭鍥炲埌璧峰鐐癸級");
- err.push_back(6);
- AddExamFault(6, rtkTime);
- status = -1;
- }
- }
- if (carStopEvent)
- AppTimer_delete(StopCarTimeout);
- carStopEvent = false;
- stopCar2S = false;
- } else if (run_status < 0) {
- // 宸﹀彸鍊掑簱澶х翰骞舵湭瑕佹眰璋佸厛瀹屾垚锛屾晠浠ュ厛瓒婅繃鐨勬帶鍒剁嚎涓哄噯锛屼笅娆″緱瓒婅繃鍙﹀涓�鏉�
- char the_ctrl_line_crossed = 0;
-
- if (leftTireCrossLeftLine && rightTireCrossLeftLine) {
- the_ctrl_line_crossed = 'L';
- } else if (leftTireCrossRightLine && rightTireCrossRightLine) {
- the_ctrl_line_crossed = 'R';
- }
-
- if (first_ctrl_line_id > 0 && first_ctrl_line_id == the_ctrl_line_crossed) {
- // 涓嶅悎鏍硷細鏈寜瑙勫畾绾胯矾琛岄┒锛堣瘯鍥惧仛2娆″悓鏂瑰悜鐨勫�掑簱锛�
- DEBUG("涓嶅悎鏍硷細鏈寜瑙勫畾绾胯矾琛岄┒锛堣瘯鍥惧仛2娆″悓鏂瑰悜鐨勫�掑簱锛�");
- err.push_back(6);
+ crossCtrlLine = CrossCtrlLine(map, car, carPrev);
+ if (crossCtrlLine > 0 && !crossCtrlLineSw) {
+ crossCtrlLineSw = true;
+ if (parkCount == 0) {
+ carray[0] = crossCtrlLine;
+ } else if (parkCount == 1) {
+ if (carray[0] == crossCtrlLine) {
+ // 涓嶆寜瑙勫畾绾胯矾锛岄『搴忓舰寮忥紝涓嶅悎鏍�
AddExamFault(6, rtkTime);
- status = -1;
- } else if (the_ctrl_line_crossed > 0 && first_ctrl_line_id == 0) {
- first_ctrl_line_id = the_ctrl_line_crossed;
- // 椤圭洰姝ｅ紡寮�濮嬶紝210绉掑唴瀹屾垚
- AppTimer_delete(ParkBottomTimeout);
- AppTimer_add(ParkBottomTimeout, D_SEC(PARK_TIMEOUT));
- currTarget = FIRST_PARK;
- carParkSuccess = false;
- parkTimeout = false;
- leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;
-
- DEBUG("绗竴娆″�掑簱");
- } else if (the_ctrl_line_crossed > 0) {
- currTarget = SECOND_PARK;
- carParkSuccess = false;
- leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;
- DEBUG("绗簩娆″�掑簱");
- } else if (currTarget != THIRD_TOUCH_CTRL_LINE) {
- // 涓嶅悎鏍硷細鍊掕溅鍓嶏紝2鍓嶈疆娌￠┒杩囨帶鍒剁嚎
- DEBUG("涓嶅悎鏍硷細鍊掕溅鍓嶏紝2鍓嶈疆娌￠┒杩囨帶鍒剁嚎");
- err.push_back(9);
- AddExamFault(9, rtkTime);
- status = -1;
+ DEBUG("涓嶆寜瑙勫畾绾胯矾锛岄『搴忓舰寮�");
}
- if (carStopEvent)
- AppTimer_delete(StopCarTimeout);
- carStopEvent = false;
- stopCar2S = false;
- } else {
- if (!carStopEvent) {
- AppTimer_delete(StopCarTimeout);
- AppTimer_add(StopCarTimeout, D_SEC(2));
+ carray[1] = crossCtrlLine;
+ } else if (parkCount == 2) {
+ if (carray[0] != crossCtrlLine) {
+ // 涓嶆寜瑙勫畾绾胯矾锛岄『搴忓舰寮忥紝涓嶅悎鏍�
+ AddExamFault(6, rtkTime);
+ DEBUG("涓嶆寜瑙勫畾绾胯矾锛岄『搴忓舰寮�");
+ } else {
+ // 绂诲紑娴嬭瘯鍖猴紝鍋滄璁℃椂
+ DEBUG("绂诲紑娴嬭瘯鍖猴紝鍋滄璁℃椂");
+ testing = false;
+ goto TEST_END;
}
- carStopEvent = true;
+ carray[2] = crossCtrlLine;
}
- } else if (currTarget == FIRST_PARK || currTarget == SECOND_PARK) {
- if (run_status < 0) {
- if (stopCar2S) {
- // 鎵�5鍒嗭細涓�斿仠杞�
- DEBUG("鎵�5鍒嗭細涓�斿仠杞�");
- err.push_back(11);
- AddExamFault(11, rtkTime);
+ }
+
+ if (testing && darray[0] > 0 && tp - firstReverseTimepoint >= PARK_TIMEOUT) {
+ // 瀹屾垚瓒呮椂锛屼笉鍚堟牸
+ if (!reportExamTimeout) {
+ reportExamTimeout = true;
+ AddExamFault(10, rtkTime);
+ DEBUG("椤圭洰瓒呮椂");
+ }
+ }
+
+ if (moveDirect != prevMoveDirect) {
+ if (moveDirect == 0) {
+ stopTimepoint = tp;
+ storeMoveDirectBeforeStop = prevMoveDirect;
+ if (prevMoveDirect == -1) {
+ checkPartStatus = true;
}
- carStopEvent = false;
- stopCar2S = false;
- } else if (run_status == 0) {
- // 绔嬪嵆妫�鏌ユ槸鍚﹀仠杞﹀埌浣嶏紝涔熻鏄腑閫斿仠杞︼紝鍏堜笉绠★紝寰呭彂鐢熷墠杩涗簨浠跺悗锛屽啀鏂畾鏄惁鍋滆溅鍒颁綅
- if (!carStopEvent) {
- carStopEvent = true;
- carParkSuccess = EnterParking(map, car);
- AppTimer_delete(StopCarTimeout);
- AppTimer_add(StopCarTimeout, D_SEC(2));
- }
+ DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
} else {
- if (carStopEvent) {
- if (!carParkSuccess) {
- // 涓嶅悎鏍硷細鍊掑簱涓嶅叆
- DEBUG("涓嶅悎鏍硷細鍊掑簱涓嶅叆");
- err.push_back(8);
- AddExamFault(8, rtkTime);
- status = -1;
- } else if (currTarget == FIRST_PARK) {
- currTarget = SECOND_TOUCH_CTRL_LINE;
- DEBUG("杩囧彟涓�鏍规帶鍒剁嚎");
- } else {
- currTarget = THIRD_TOUCH_CTRL_LINE;
- DEBUG("鍐嶈繃绗竴鏍规帶鍒剁嚎");
+ DEBUG("缁х画琛岄┒ %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
+
+ DEBUG("鍋滆溅鏃堕棿 %ld", tp - stopTimepoint);
+
+ if (moveDirect == storeMoveDirectBeforeStop) {
+ // 鍚屾柟鍚戝啀鍚姩锛岀户缁垽鏂槸鍚﹀仠杞﹁秴鏃�
+ if (tp - stopTimepoint >= STOP_CAR_TIME) {
+ // 鍋滆溅瓒�2绉掞紝姣忔鎵�5鍒�
+ AddExamFault(11, rtkTime);
+ DEBUG("涓�斿仠杞�");
+ }
+ } else if (moveDirect == -1) {
+ // 鍒囨崲涓哄�掕溅
+ if (darray[parkCount] == 0) {
+ if (!crossCtrlLineSw) {
+ // 鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎
+ AddExamFault(9, rtkTime);
+ DEBUG("鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎");
+ }
+ darray[parkCount] = 1;
+ firstReverseTimepoint = tp;
+ }
+ } else {
+ // 鍒囨崲涓哄墠杩�
+ if (tp - stopTimepoint >= STOP_CAR_TIME) {
+ if (crossCtrlLineSw) {
+ if (checkPartStatus) {
+ if (EnterParking(map, car)) {
+ parkStatus[parkCount] = 1;
+ }
+ checkPartStatus = false;
+ }
+ if (parkStatus[parkCount] != 1) {
+ // 鍊掑簱涓嶅叆锛屼笉鍚堟牸
+ reportParkFail = true;
+ AddExamFault(8, rtkTime);
+ DEBUG("鍊掑簱涓嶅叆");
+ }
+ }
+
+ crossCtrlLineSw = false;
+
+ if (parkCount < 2)
+ parkCount++;
}
}
- carStopEvent = false;
- stopCar2S = false;
+ }
+ prevMoveDirect = moveDirect;
+ } else if (moveDirect == -1) {
+ if (darray[parkCount] == 0) {
+ // 鍒囨崲涓哄�掕溅
+ if (!crossCtrlLineSw) {
+ // 鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎
+ AddExamFault(9, rtkTime);
+ DEBUG("鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎");
+ }
+ darray[parkCount] = 1;
+ firstReverseTimepoint = tp;
+ }
+ } else if (moveDirect == 0 && crossCtrlLineSw) {
+ if (tp - stopTimepoint >= STOP_CAR_TIME && checkPartStatus) {
+ if (EnterParking(map, car)) {
+ parkStatus[parkCount] = 1;
+ }
+ checkPartStatus = false;
}
}
- if (status != 0) {
- StopParkBottom();
- }
-
- return status;
-}
-
-static void StopCarTimeout(union sigval sig) {
- AppTimer_delete(StopCarTimeout);
-
- stopCar2S = true;
-}
-
-static void LeaveTestAreaLongtime(union sigval sig) {
- AppTimer_delete(LeaveTestAreaLongtime);
- leaveTestArea = true;
- trigLeaveTestAreaDetect = false;
-}
-
-static void ParkBottomTimeout(union sigval sig) {
- AppTimer_delete(ParkBottomTimeout);
- parkTimeout = true;
+TEST_END:
+ return testing ? 0 : 1;
}
// 妫�娴�2鍓嶈疆鏄惁姝ｅ悜瓒婅繃宸﹀彸鎺у埗绾�
-static void CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car)
+static char CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car)
{
- Line leftCtrlLine, rightCtrlLine;
- Line track1;
- PointF p1, p2;
+ // 杩囧彸鎺у埗绾�
+ if ((IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1) &&
+ (IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->right_front_tire[TIRE_OUTSIDE]]) == -1) &&
+ (IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->axial[AXIAL_REAR]]) == 1)) {
+ return 'R';
+ }
- MakeLine(&leftCtrlLine, &(map->point[0]), &(map->point[1]));
- MakeLine(&rightCtrlLine, &(map->point[6]), &map->point[7]);
- // 宸﹀墠杞紝鍙栬疆瀹界殑涓偣
- p1.X = (car->carXY[car->left_front_tire[TIRE_OUTSIDE]].X + car->carXY[car->left_front_tire[TIRE_INSIDE]].X) / 2;
- p1.Y = (car->carXY[car->left_front_tire[TIRE_OUTSIDE]].Y + car->carXY[car->left_front_tire[TIRE_INSIDE]].Y) / 2;
- p2.X = (prev_car->carXY[car->left_front_tire[TIRE_OUTSIDE]].X + prev_car->carXY[car->left_front_tire[TIRE_INSIDE]].X) / 2;
- p2.Y = (prev_car->carXY[car->left_front_tire[TIRE_OUTSIDE]].Y + prev_car->carXY[car->left_front_tire[TIRE_INSIDE]].Y) / 2;
- MakeLine(&track1, &p1, &p2);
- if (IntersectionOf(track1, leftCtrlLine) == GM_Intersection &&
- IntersectionOfLine(map->point[0], map->point[1], p1) == -1) {
- leftTireCrossLeftLine = true;
- DEBUG("宸﹁疆璺ㄨ秺宸︽帶鍒剁嚎");
+ // 杩囧乏鎺у埗绾�
+ if ((IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == 1) &&
+ (IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->right_front_tire[TIRE_OUTSIDE]]) == 1) &&
+ (IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->axial[AXIAL_REAR]]) == -1)) {
+ return 'L';
}
- if (IntersectionOf(track1, rightCtrlLine) == GM_Intersection &&
- IntersectionOfLine(map->point[6], map->point[7], p1) == -1) {
- leftTireCrossRightLine = true;
- DEBUG("宸﹁疆璺ㄨ秺鍙虫帶鍒剁嚎");
- }
- // 鍙冲墠杞�
- p1.X = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].X + car->carXY[car->right_front_tire[TIRE_INSIDE]].X) / 2;
- p1.Y = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].Y + car->carXY[car->right_front_tire[TIRE_INSIDE]].Y) / 2;
- p2.X = (prev_car->carXY[car->right_front_tire[TIRE_OUTSIDE]].X + prev_car->carXY[car->right_front_tire[TIRE_INSIDE]].X) / 2;
- p2.Y = (prev_car->carXY[car->right_front_tire[TIRE_OUTSIDE]].Y + prev_car->carXY[car->right_front_tire[TIRE_INSIDE]].Y) / 2;
- MakeLine(&track1, &p1, &p2);
- if (IntersectionOf(track1, leftCtrlLine) == GM_Intersection &&
- IntersectionOfLine(map->point[0], map->point[1], p1) == -1) {
- rightTireCrossLeftLine = true;
- DEBUG("鍙宠疆璺ㄨ秺宸︽帶鍒剁嚎");
- }
- if (IntersectionOf(track1, rightCtrlLine) == GM_Intersection &&
- IntersectionOfLine(map->point[6], map->point[7], p1) == -1) {
- rightTireCrossRightLine = true;
- DEBUG("鍙宠疆璺ㄨ秺鍙虫帶鍒剁嚎");
- }
+
+ return 0;
}
static bool EnterParking(const Polygon *map, const car_model *car) {
@@ -357,6 +252,8 @@
CleanPolygon(&parking);
free(car_body.point);
+
+ DEBUG("妫�鏌ュ�掑簱鐘舵�� %s", succ ? "鎴愬姛" : "澶辫触");
return succ;
}
@@ -387,3 +284,25 @@
free(car_body.point);
return ret;
}
+
+static bool ExitParkArea(const Polygon *map, const car_model *car)
+{
+// 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
+ bool ret = false;
+
+ Polygon carBody;
+
+ carBody.num = car->bodyNum;
+ carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
+ for (int i = 0; i < carBody.num; ++i) {
+ carBody.point[i] = car->carXY[car->body[i]];
+ }
+
+ if (IntersectionOf(&carBody, map) == GM_None) {
+ ret = true;
+ }
+
+ free(carBody.point);
+
+ return ret;
+}
diff --git a/lib/src/main/cpp/test_items/park_bottom.h b/lib/src/main/cpp/test_items/park_bottom.h
index ebe4042..be36967 100644
--- a/lib/src/main/cpp/test_items/park_bottom.h
+++ b/lib/src/main/cpp/test_items/park_bottom.h
@@ -11,8 +11,7 @@
using namespace std;
-void StartParkBottom(void);
-void StopParkBottom(void);
-int TestParkBottom(vector<int>&err, const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int run_status, const struct RtkTime *rtkTime);
+void StartParkBottom(int moveStatus, const struct RtkTime *rtkTime);
+int TestParkBottom(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime);
#endif //RTKDRIVERTEST_PARK_BOTTOM_H
diff --git a/lib/src/main/cpp/test_items/park_edge.cpp b/lib/src/main/cpp/test_items/park_edge.cpp
index c5ed6ec..8656e46 100644
--- a/lib/src/main/cpp/test_items/park_edge.cpp
+++ b/lib/src/main/cpp/test_items/park_edge.cpp
@@ -27,6 +27,7 @@
static int parkStatus;
static bool occurMoveBack, parkSuccess;
static uint32_t moveBackTimePoint;
+static bool testing = false;
static bool CrashRedLine1(const Polygon *map, const car_model *car);
static bool CrashRedLine2(const Polygon *map, const car_model *car);
@@ -36,6 +37,10 @@
void StartParkEdge(int moveStatus, const struct RtkTime *rtkTime)
{
+ DEBUG("杩涘叆渚ф柟鍋滆溅鍦哄湴");
+
+ testing = true;
+
occurCrashRedLine1 = occurCrashRedLine2 = false; // 杩欎釜绉戠洰瑙勫畾鐗规畩鐐癸紝鍙戠敓涓�娆℃墸10鍒嗭紝鑰屼笉鐩存帴娣樻卑
reportExamTimeout = false;
@@ -47,14 +52,10 @@
occurMoveBack = true;
moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
}
-
- DEBUG("杩涘叆渚ф柟鍋滆溅鍦哄湴");
}
int TestParkEdge(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime)
{
- int status = 0;
-
if (CrashRedLine1(map, car)) {
if (!occurCrashRedLine1) {
// 杞﹁疆鍘嬭竟绾匡紝姣忔鎵�10鍒�
@@ -83,7 +84,8 @@
AddExamFault(21, rtkTime);
DEBUG("鐩存帴椹剁娴嬭瘯鍖�,绉诲簱涓嶅叆");
}
- status = 1;
+ testing = false;
+ goto TEST_END;
}
if (occurMoveBack) {
@@ -121,7 +123,10 @@
// 鍊掕溅鍒囨崲鍒板墠杩�
if (moveStatus == 1 && tp - stopTimepoint >= STOP_CAR_TIME) {
if (parkStatus == 0) {
-
+ if (EnterParking(map, car)) {
+ parkStatus = 1;
+ parkSuccess = true;
+ }
}
if (!parkSuccess) {
@@ -149,7 +154,6 @@
prevMoveStatus = moveStatus;
} else if (moveStatus == 0 && parkStatus == 0) {
uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- tp = tp * 1000 + rtkTime->mss * 10;
if (tp - stopTimepoint >= STOP_CAR_TIME) {
if (EnterParking(map, car)) {
@@ -160,8 +164,8 @@
}
}
}
-
- return status;
+TEST_END:
+ return testing ? 0 : 1;
}
// 杞﹁疆鏄惁鍘嬮亾璺竟绾�
diff --git a/lib/src/main/cpp/test_items/stop_and_start.cpp b/lib/src/main/cpp/test_items/stop_and_start.cpp
index 9c0c69e..8ce210b 100644
--- a/lib/src/main/cpp/test_items/stop_and_start.cpp
+++ b/lib/src/main/cpp/test_items/stop_and_start.cpp
@@ -26,7 +26,7 @@
const double EPSILON = 1e-3;
-static bool SASTesting = false;
+static bool testing = false;
static double slideDistance;
static PointF stopPoint;
@@ -49,7 +49,7 @@
void StartSAS(int moveDirect, const struct RtkTime *rtkTime)
{
DEBUG("杩涘叆鍧¤捣椤圭洰");
- SASTesting = true;
+ testing = true;
slideDistance = 0.0;
prevMoveDirect = moveDirect;
@@ -68,8 +68,6 @@
int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
- int status = 0;
-
if (CrashRedLine(map, car)) {
// 杞﹁疆鍘嬬嚎锛屼笉鍚堟牸
if (!occurCrashRedLine) {
@@ -83,7 +81,7 @@
if (ExitTestArea(map, car)) {
// 椹剁娴嬭瘯鍖�
- status = 1;
+ testing = false;
}
if (prevMoveDirect != moveDirect) {
@@ -173,7 +171,7 @@
}
}
- return status;
+ return testing ? 0 : 1;
}
// 杞﹁疆鏄惁鍘嬭竟绾�
diff --git a/lib/src/main/cpp/test_items/turn_a90.cpp b/lib/src/main/cpp/test_items/turn_a90.cpp
index f76ac16..cedcaf7 100644
--- a/lib/src/main/cpp/test_items/turn_a90.cpp
+++ b/lib/src/main/cpp/test_items/turn_a90.cpp
@@ -19,7 +19,7 @@
const uint32_t STOP_CAR_TIME = D_SEC(2);
-static bool TA90Testing;
+static bool testing;
static int enterAreaHeading;
static bool turnLeftFinished;
@@ -35,6 +35,7 @@
void StartTurnA90(int moveDirect, double heading, const struct RtkTime *rtkTime)
{
DEBUG("杩涘叆鐩磋杞集鍦哄湴");
+ testing = true;
enterAreaHeading = (int) heading;
prevMoveDirect = moveDirect;
if (moveDirect == 0) {
@@ -47,10 +48,11 @@
int TestTurnA90(const Polygon *map, const car_model *car, const car_model *carPrev, double heading, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
- int status = 0;
+ int az = (int) heading;
if (ExitTestArea(map, car)) {
- status = 1;
+ testing = false;
+ goto TEST_END;
}
if (CrashRedLine(map, car)) {
@@ -86,15 +88,11 @@
}
// 妫�鏌ヨ浆鍚戠姸鎬�
- int az = (int) heading;
-
if (ABS(az - enterAreaHeading) > 180) {
az = 360 - ABS(az-enterAreaHeading);
} else {
az = ABS(az - enterAreaHeading);
}
-
-
if (az >= 30) {
if (!turnLeftFinished) {
@@ -113,8 +111,8 @@
if (turnLeftFinished) {
}
-
- return status;
+TEST_END:
+ return testing?0:1;
}
// 杞﹁疆鏄惁鍘嬭竟绾�
--
Gitblit v1.8.0