From eced3d013f06b623a49cb9deaba42218c4e37bb8 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期三, 15 一月 2020 10:56:05 +0800
Subject: [PATCH] update
---
lib/src/main/cpp/master/comm_if.h | 5
lib/src/main/cpp/test_items/park_bottom.cpp | 68 +++++------
lib/src/main/cpp/driver_test.cpp | 146 +++++++++++++++++------
lib/src/main/cpp/master/comm_if.cpp | 59 +++++++++
lib/src/main/cpp/test_items/park_bottom.h | 2
app/src/main/assets/map.json | 10 -
Documents/模拟驾考APP内部通讯.docx | 0
lib/src/main/cpp/rtk_module/rtk.cpp | 9 +
lib/src/main/cpp/driver_test.h | 36 +++++
lib/src/main/cpp/rtk_platform/platform.cpp | 15 +
10 files changed, 253 insertions(+), 97 deletions(-)
diff --git "a/Documents/\346\250\241\346\213\237\351\251\276\350\200\203APP\345\206\205\351\203\250\351\200\232\350\256\257.docx" "b/Documents/\346\250\241\346\213\237\351\251\276\350\200\203APP\345\206\205\351\203\250\351\200\232\350\256\257.docx"
index 63a4ab7..e28ccc5 100644
--- "a/Documents/\346\250\241\346\213\237\351\251\276\350\200\203APP\345\206\205\351\203\250\351\200\232\350\256\257.docx"
+++ "b/Documents/\346\250\241\346\213\237\351\251\276\350\200\203APP\345\206\205\351\203\250\351\200\232\350\256\257.docx"
Binary files differ
diff --git a/app/src/main/assets/map.json b/app/src/main/assets/map.json
index 36abf91..b503b40 100644
--- a/app/src/main/assets/map.json
+++ b/app/src/main/assets/map.json
@@ -3,10 +3,7 @@
"item": 1,
"point": [{
"x-y": [-11.9669, 28.013, -11.5114, 27.137, -10.8069, 27.5039, -10.2969, 26.4212, -9.2102, 26.8894, -9.6513, 28.0027, -8.9758, 28.3797, -9.5057, 29.3232]
- },
- {
- "x-y": [-11.9669, 28.013, -11.5114, 27.137, -10.8069, 27.5039, -10.2969, 26.4212, -9.2102, 26.8894, -9.6513, 28.0027, -8.9758, 28.3797, -9.5057, 29.3232]
- }
+ }
]
},
{
@@ -14,10 +11,7 @@
"item": 2,
"point": [{
"x-y": [-11.9669, 28.013, -11.5114, 27.137, -10.8069, 27.5039, -10.2969, 26.4212, -9.2102, 26.8894, -9.6513, 28.0027, -8.9758, 28.3797, -9.5057, 29.3232]
- },
- {
- "x-y": [-11.9669, 28.013, -11.5114, 27.137, -10.8069, 27.5039, -10.2969, 26.4212, -9.2102, 26.8894, -9.6513, 28.0027, -8.9758, 28.3797, -9.5057, 29.3232]
- }
+ }
]
}
]
\ No newline at end of file
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 28b5bfc..48c820b 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -57,10 +57,8 @@
int errs = 0;
vector<int> ErrorList;
-
-static Polygon theParkEdgeMap; // 渚т綅鍋滆溅鍦板浘
-static Polygon theTurn90Map;
-static Polygon theSSMap;
+vector<ExamFault> ExamFaultList;
+static int examFaultIndex = 0;
static struct map_list {
int id;
@@ -71,35 +69,8 @@
static int MapNum = 0;
-
-#define CAR_COORD_STORE_SIZE 10
-
-struct car_coord_ {
- uint32_t uptime;
- double azimuth;
- PointF coord;
-};
-
-struct car_desc_ {
- double distance; // 璺濈涓诲ぉ绾跨殑璺濈
- double angle; // 浠庝腑杞寸嚎閫嗘椂閽堝舰鎴愮殑瑙掑害
-};
-
-struct car_desc_ *CarDesc = NULL;
-
-typedef struct {
- PointF basePoint;
- int axial[2];
- int left_front_tire[2];
- int right_front_tire[2];
- int left_rear_tire[2];
- int right_rear_tire[2];
- int pointNum;
- struct car_desc_ *carDesc;
- PointF *carXY;
-} car_model;
-
static car_model *CarModel = NULL;
+static car_model *CarModelPrev = NULL;
#define MOV_AVG_SIZE 1
#define RTK_BUFFER_SIZE 100
@@ -110,15 +81,17 @@
static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b);
static void ReadDriverExamPrimerTimeout(union sigval sig);
-static void UpdateCarBodyCoord(double azimuth, PointF main_ant, const car_model *carModel);
+static void UpdateCarBodyCoord(double azimuth, PointF main_ant, car_model *carModel);
static bool UpdateCarCoord(double &spd, int &mov, int &idx);
static bool FrontTireEnterArea(const Polygon *car, const Polygon *map);
void DriverTestInit(void)
{
+ TestStart = false;
memset(&MapList, 0, sizeof(MapList));
MapNum = 0;
CarModel = NULL;
+ CarModelPrev = NULL;
RtkBuffer = (rtk_info *) malloc(RTK_BUFFER_SIZE * sizeof(rtk_info));
RtkBufferNum = RtkBufferIn = 0;
@@ -140,6 +113,8 @@
void ClearMap(void)
{
+ if (TestStart) return;
+
for (int i = 0; i < MapNum; ++i) {
if (MapList[i].map.point != NULL)
free(MapList[i].map.point);
@@ -156,7 +131,7 @@
AppTimer_delete(ReadDriverExamPrimerTimeout);
- if (map == NULL || pointNum == 0)
+ if (map == NULL || pointNum == 0 || TestStart)
return;
MapList[MapNum].id = id;
@@ -187,13 +162,15 @@
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
- double (*point)[2], int pointNum)
+ int *body, int bodyNum,double (*point)[2], int pointNum)
{
DEBUG("SetCarMeasurePoint pointNum %d", pointNum);
- if (point == NULL || pointNum == 0) return;
+ if (point == NULL || pointNum == 0 || TestStart) return;
if (CarModel != NULL) {
+ if (CarModel->body != NULL)
+ free(CarModel->body);
if (CarModel->carDesc != NULL)
free(CarModel->carDesc);
if (CarModel->carXY != NULL)
@@ -201,6 +178,23 @@
free(CarModel);
CarModel = NULL;
}
+
+ if (CarModelPrev != NULL) {
+ if (CarModelPrev->body != NULL)
+ free(CarModelPrev->body);
+ if (CarModelPrev->carDesc != NULL)
+ free(CarModelPrev->carDesc);
+ if (CarModelPrev->carXY != NULL)
+ free(CarModelPrev->carXY);
+ free(CarModelPrev);
+ CarModelPrev = NULL;
+ }
+
+ CarModelPrev = (car_model *)malloc(sizeof(car_model));
+ CarModelPrev->body = NULL;
+ CarModelPrev->carDesc = NULL;
+ CarModelPrev->pointNum = 0;
+ CarModelPrev->carXY = (PointF *) malloc(sizeof(PointF) * pointNum);
CarModel = (car_model *)malloc(sizeof(car_model));
CarModel->basePoint.X = basePoint[0];
@@ -215,8 +209,22 @@
CarModel->left_rear_tire[1] = left_rear_tire[1];
CarModel->right_rear_tire[0] = right_rear_tire[0];
CarModel->right_rear_tire[1] = right_rear_tire[1];
- CarModel->pointNum = pointNum;
+ CarModel->bodyNum = bodyNum;
+ if (bodyNum == 0 || body == NULL) {
+ CarModel->bodyNum = 6;
+ CarModel->body = (int *) malloc(sizeof(int) * 6);
+ for (int i = 0; i < 6; ++i) {
+ CarModel->body[i] = i;
+ }
+ } else {
+ CarModel->body = (int *) malloc(sizeof(int) * CarModel->bodyNum);
+ for (int i = 0; i < CarModel->bodyNum; ++i) {
+ CarModel->body[i] = body[i];
+ }
+ }
+
+ CarModel->pointNum = pointNum;
CarModel->carDesc = (struct car_desc_ *)malloc(sizeof(struct car_desc_) * pointNum);
CarModel->carXY = (PointF *) malloc(sizeof(PointF) * pointNum);
@@ -264,8 +272,15 @@
err = true;
MA_SendExamStatus(0, -2);
}
- if (!err)
+ if (!err) {
+ if (!TestStart) {
+ ExamFaultList.clear();
+ examFaultIndex = 0;
+
+ TestStart = true;
+ }
MA_SendExamStatus(1, 0);
+ }
}
void UpdateRTKInfo(const rtk_info *s)
@@ -275,6 +290,8 @@
RtkBufferIn = (RtkBufferIn + 1) % RTK_BUFFER_SIZE;
if (RtkBufferNum < RTK_BUFFER_SIZE)
RtkBufferNum++;
+ } else {
+ return;
}
double speed;
@@ -289,9 +306,9 @@
RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);
brief.qf = RtkBuffer[index].qf;
- brief.map_id = -1;
+ brief.map_id = 863;
brief.move = move;
- brief.speed = speed;
+ brief.speed = speed * 3.6;
brief.heading = RtkBuffer[index].heading;
brief.main_ant[0] = RtkBuffer[index].x;
brief.main_ant[1] = RtkBuffer[index].y;
@@ -307,6 +324,12 @@
brief.right_rear_tire[0] = CarModel->right_rear_tire[0];
brief.right_rear_tire[1] = CarModel->right_rear_tire[1];
+ brief.bodyNum = CarModel->bodyNum;
+ brief.body = (int *) malloc(sizeof(int) * CarModel->bodyNum);
+ for (int i = 0; i < CarModel->bodyNum; ++i) {
+ brief.body[i] = CarModel->body[i];
+ }
+
brief.pointNum = CarModel->pointNum;
brief.point = (double *) malloc(CarModel->pointNum * 2 * sizeof(double));
for (int i = 0, j = 0; i < CarModel->pointNum; ++i) {
@@ -315,6 +338,13 @@
}
MA_SendCarPosition(&brief);
+
+ free(brief.body);
+ free(brief.point);
+
+ if (TestStart) {
+// TestParkBottom(ErrorList, 0, CarModel, CarModelPrev, speed, move);
+ }
}
}
@@ -345,6 +375,8 @@
// 濡傛灉涓�瀹氱殑鏃堕棿閮芥病鏈夋湁鏁堝畾浣嶏紝鍒犻櫎涔嬪墠鐨勫��
uint32_t tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2]);
+ DEBUG("tmDiff = %ld, p1 = %d p2 = %d dss = %d dss2 = %d", tmDiff, p1, p2, RtkBuffer[p1].dss, RtkBuffer[p2].dss);
+
if (tmDiff > D_SEC(5)) {
if (p1 != 0)
RtkBuffer[0] = RtkBuffer[p1];
@@ -357,7 +389,22 @@
main_ant_coord.X = RtkBuffer[p1].x;
main_ant_coord.Y = RtkBuffer[p1].y;
+ // 淇濆瓨涓婁竴涓溅韬疆寤撶偣
+ if (CarModelPrev->pointNum > 0) {
+ CarModelPrev->pointNum = CarModel->pointNum;
+ CarModelPrev->basePoint = CarModel->basePoint;
+ for (int i = 0; i < CarModel->pointNum; ++i) {
+ CarModelPrev->carXY[i] = CarModel->carXY[i];
+ }
+ }
UpdateCarBodyCoord(RtkBuffer[p1].heading, main_ant_coord, CarModel);
+ if (CarModelPrev->pointNum == 0) {
+ CarModelPrev->pointNum = CarModel->pointNum;
+ CarModelPrev->basePoint = CarModel->basePoint;
+ for (int i = 0; i < CarModel->pointNum; ++i) {
+ CarModelPrev->carXY[i] = CarModel->carXY[i];
+ }
+ }
// 璁＄畻閫熷害(绫�/绉�)銆佸墠杩涘悗閫�
double speed = sqrt(pow(RtkBuffer[p1].x - RtkBuffer[p2].x, 2) + pow(RtkBuffer[p1].y - RtkBuffer[p2].y, 2)) * 1000 /
@@ -505,6 +552,21 @@
}*/
}
+void AddExamFault(int wrong, const char *utc)
+{
+ if (!TestStart) return;
+
+ struct ExamFault fault;
+
+ fault.sn = examFaultIndex++;
+ strcpy(fault.utc, utc);
+ fault.wrong_id = wrong;
+
+ ExamFaultList.push_back(fault);
+
+ MA_SendExamWrong(ExamFaultList);
+}
+
car_model_cache_t *GetCarModelCache(int node)
{
return NULL;
@@ -515,8 +577,10 @@
* @param azimuth
* @param coord
*/
-static void UpdateCarBodyCoord(double azimuth, PointF main_ant, const car_model *carModel)
+static void UpdateCarBodyCoord(double azimuth, PointF main_ant, car_model *carModel)
{
+ carModel->basePoint = main_ant;
+
for (int i = 0; i < carModel->pointNum; ++i) {
double tx = main_ant.X + carModel->carDesc[i].distance*sin(toRadians(azimuth));
double ty = main_ant.Y + carModel->carDesc[i].distance*cos(toRadians(azimuth));
diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h
index 1efed4d..ddc0dab 100644
--- a/lib/src/main/cpp/driver_test.h
+++ b/lib/src/main/cpp/driver_test.h
@@ -8,8 +8,31 @@
#include "Geometry.h"
#include "rtk_module/rtk.h"
+#include <vector>
+
#define TIRE_OUTSIDE 0
#define TIRE_INSIDE 1
+
+using namespace std;
+
+struct car_desc_ {
+ double distance; // 璺濈涓诲ぉ绾跨殑璺濈
+ double angle; // 浠庝腑杞寸嚎閫嗘椂閽堝舰鎴愮殑瑙掑害
+};
+
+typedef struct {
+ PointF basePoint; // 鐜板満娴嬮噺鐨勪富澶╃嚎鍧愭爣
+ int axial[2];
+ int left_front_tire[2];
+ int right_front_tire[2];
+ int left_rear_tire[2];
+ int right_rear_tire[2];
+ int bodyNum;
+ int *body;
+ int pointNum;
+ struct car_desc_ *carDesc; // 缁忕幇鍦烘祴閲忕偣璁＄畻鍚庣殑鍊�
+ PointF *carXY; // 璁＄畻鍚庣殑杞﹁韩鍚勭偣鍧愭爣
+} car_model;
// 鎬绘暟
// 涓酱绾垮墠鐐�,id 0锛屼腑杞寸嚎鍚庣偣id
@@ -31,17 +54,26 @@
carModelDesc_t *desc;
} car_model_cache_t;
+struct ExamFault {
+ int sn;
+ char utc[32];
+ int wrong_id;
+};
+
+//vector<ExamFault> ExamFaultList;
+
void DriverTestInit(void);
void ReadDriverExamPrimer(void);
void ClearMap(void);
void AddMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2);
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
- double (*point)[2], int pointNum);
+ int *body, int bodyNum, double (*point)[2], int pointNum);
void SetSensorCfg(int (*sensor)[2], int sensorNum);
void StartDriverExam(int start);
-
void UpdateRTKInfo(const rtk_info *s);
+void AddExamFault(int wrong, const char *utc);
+
car_model_cache_t *GetCarModelCache(int node);
#endif //RTKDRIVERTEST_DRIVER_TEST_H
diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp
index 578b2e5..9f379ef 100644
--- a/lib/src/main/cpp/master/comm_if.cpp
+++ b/lib/src/main/cpp/master/comm_if.cpp
@@ -12,10 +12,14 @@
#include "../utils/xconvert.h"
#include "../driver_test.h"
#include "../defs.h"
+#include "../common/apptimer.h"
+#include "../jni_log.h"
#include <string>
#include <iostream>
#include <vector>
+
+#define DEBUG(fmt, args...) LOGD("<comm_if> <%s>: " fmt, __func__, ##args)
using namespace rapidjson;
using namespace std;
@@ -207,9 +211,36 @@
SendMsgToMainProc(ID_SM_RTK_BRIEF, sb.GetString());
}
-void MA_SendExamWrong(vector<int>&err)
+void MA_SendExamWrong(vector<ExamFault> &ExamFaultList)
{
+ StringBuffer sb;
+ Writer<StringBuffer> writer(sb);
+ writer.StartArray();
+
+ for (int i = 0; i < 3; ++i) {
+ writer.StartObject(); // Between StartObject()/EndObject(),
+ writer.Key("wrong_id"); // output a key,
+ writer.Int(i+1);
+ writer.EndObject();
+ }
+
+ for (vector<ExamFault>::iterator iter = ExamFaultList.begin(); iter != ExamFaultList.end(); ++iter) {
+ writer.StartObject();
+
+ writer.Key("sn");
+ writer.Int(iter->sn);
+ writer.Key("utc");
+ writer.String(iter->utc);
+ writer.Key("wrong_id");
+ writer.Int(iter->wrong_id);
+
+ writer.EndObject();
+ }
+
+ writer.EndArray();
+
+ SendMsgToMainProc(ID_SM_EXAM_BRIEF, sb.GetString());
}
void MA_SendCarPosition(const struct carBrief *brief)
@@ -270,6 +301,13 @@
writer.Int(brief->right_rear_tire[1]);
writer.EndArray();
+ writer.Key("body");
+ writer.StartArray();
+ for (int i = 0; i < brief->bodyNum; ++i) {
+ writer.Int(brief->body[i]);
+ }
+ writer.EndArray();
+
writer.Key("point");
writer.StartArray();
for (int i = 0; i < brief->pointNum; ++i) {
@@ -279,7 +317,9 @@
writer.EndArray();
writer.EndObject();
+ uint32_t a = AppTimer_GetTickCount();
SendMsgToMainProc(ID_SM_CAR, sb.GetString());
+ DEBUG("鑰楁椂 %ld", AppTimer_GetTickCount() - a);
}
void MA_MainProcMsgEntry(int cmd, const char *value)
@@ -421,6 +461,8 @@
if (!doc.HasParseError()) {
double basePoint[2];
int axial[2], left_front_tire[2], right_front_tire[2], left_rear_tire[2], right_rear_tire[2];
+ int bodyNum = 0;
+ int *body = NULL;
int pointNum = 0;
double (*point)[2] = NULL;
@@ -484,6 +526,18 @@
}
}
+ if (doc.HasMember("body")) {
+ const Value& s = doc["body"];
+ if (s.IsArray()) {
+ int i = 0;
+ bodyNum = s.Size();
+ body = new int[bodyNum];
+ for(Value::ConstValueIterator itr = s.Begin(); itr != s.End(); ++itr) {
+ body[i++] = itr->GetInt();
+ }
+ }
+ }
+
if (doc.HasMember("point")) {
const Value& s = doc["point"];
if (s.IsArray()) {
@@ -503,8 +557,9 @@
}
SetCarMeasurePoint(basePoint, axial, left_front_tire, right_front_tire,
- left_rear_tire, right_rear_tire, point, pointNum);
+ left_rear_tire, right_rear_tire, body, bodyNum, point, pointNum);
+ if (body != NULL) delete []body;
if (point != NULL) delete []point;
}
break;
diff --git a/lib/src/main/cpp/master/comm_if.h b/lib/src/main/cpp/master/comm_if.h
index ab839e6..36694fd 100644
--- a/lib/src/main/cpp/master/comm_if.h
+++ b/lib/src/main/cpp/master/comm_if.h
@@ -6,6 +6,7 @@
#define MYAPPLICATION2_COMM_IF_H
#include <cstdint>
+#include "../driver_test.h"
struct gpsBrief {
char utc[32];
@@ -42,6 +43,8 @@
int right_front_tire[2];
int left_rear_tire[2];
int right_rear_tire[2];
+ int *body;
+ int bodyNum;
double *point;
int pointNum;
};
@@ -60,6 +63,6 @@
void MA_SendGpsBrief(const struct gpsBrief *brief);
void MA_SendRtkBrief(const struct rtkBrief *brief);
void MA_SendCarPosition(const struct carBrief *brief);
-
+void MA_SendExamWrong(vector<ExamFault> &ExamFaultList);
#endif //MYAPPLICATION2_COMM_IF_H
diff --git a/lib/src/main/cpp/rtk_module/rtk.cpp b/lib/src/main/cpp/rtk_module/rtk.cpp
index 08adcc0..5204642 100644
--- a/lib/src/main/cpp/rtk_module/rtk.cpp
+++ b/lib/src/main/cpp/rtk_module/rtk.cpp
@@ -182,7 +182,7 @@
int ul = ReadSerialPort(RTK_MODULE_UART, (uint8_t *)RxBuf + RxBufLen, sizeof(RxBuf) - RxBufLen);
RxBufLen += ul;
- {
+ /*{
static char buffd[16384];
buffd[0] = 0;
@@ -199,7 +199,7 @@
}
if (strlen(buffd) > 0)
DEBUG("%s <- %s", "UART", buffd);
- }
+ }*/
if (RxBufLen > 0) {
const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
@@ -400,6 +400,11 @@
// CurrRTKInfo.heading = 60;
+ static double deg = 0;
+ CurrRTKInfo.heading = deg;
+
+ deg += 2.0;
+
if (CurrRTKInfo.hh == hh && CurrRTKInfo.mm == mm && CurrRTKInfo.ss == ss && CurrRTKInfo.dss == dss) {
PlatformStatusChanged(RTK_UPDATE_EVT, (uint8_t *)&CurrRTKInfo, sizeof(CurrRTKInfo));
// UpdateRTKInfo(&CurrRTKInfo);
diff --git a/lib/src/main/cpp/rtk_platform/platform.cpp b/lib/src/main/cpp/rtk_platform/platform.cpp
index 5da003c..8fd338c 100644
--- a/lib/src/main/cpp/rtk_platform/platform.cpp
+++ b/lib/src/main/cpp/rtk_platform/platform.cpp
@@ -152,10 +152,6 @@
void PlatformStatusChanged(uint32_t event, const uint8_t *data, int length)
{
- pthread_mutex_lock(&events_mutex);
- eventMask |= event;
- pthread_mutex_unlock(&events_mutex);
-
if (data != NULL && length > 0) {
int x = BitCount(event-1);
@@ -163,7 +159,13 @@
eventData[x] = malloc(length);
memcpy(eventData[x], data, length);
eventDataLength[x] = length;
+
+// DEBUG("ADD %d len %d", x, length);
}
+
+ pthread_mutex_lock(&events_mutex);
+ eventMask |= event;
+ pthread_mutex_unlock(&events_mutex);
sem_post(&sem_status_changed);
}
@@ -208,6 +210,9 @@
while (events > 0) {
int x = BitCount(((events - 1) ^ events) - 1);
+
+// DEBUG("FETCH %d len %d", x, eventDataLength[x]);
+
PlatformChangeEntry(BV(x), (uint8_t *)eventData[x], eventDataLength[x]);
if (eventData[x] != NULL) {
free(eventData[x]);
@@ -279,6 +284,7 @@
MA_RtkPlatformLogin(data[0]);
}
if (events & GPS_UPDATE_EVT) {
+ DEBUG("GPS_UPDATE_EVT length %d", length);
const gpsStatus_t *gps = (gpsStatus_t *)data;
struct gpsBrief brief;
@@ -294,6 +300,7 @@
RequestRtkDownload(gps, 1);
}
if (events & RTK_UPDATE_EVT) {
+ DEBUG("RTK_UPDATE_EVT length %d", length);
const rtk_info *rtk = (rtk_info *)data;
struct rtkBrief brief;
diff --git a/lib/src/main/cpp/test_items/park_bottom.cpp b/lib/src/main/cpp/test_items/park_bottom.cpp
index 6bb16fb..a9b71f3 100644
--- a/lib/src/main/cpp/test_items/park_bottom.cpp
+++ b/lib/src/main/cpp/test_items/park_bottom.cpp
@@ -40,10 +40,10 @@
static void StopCarTimeout(union sigval sig);
static void LeaveTestAreaLongtime(union sigval sig);
-static void CrossCtrlLine(const Polygon *map, const car_model_cache_t *car);
-static bool EnterParking(const Polygon *map, const car_model_cache_t *car);
+static void 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_cache_t *car);
+static bool CrashRedLine(const Polygon *map, const car_model *car);
void StartParkBottom(void)
{
@@ -56,7 +56,7 @@
currTarget = FIRST_TOUCH_CTRL_LINE;
leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;
- TextOsd(0, "ParkBottom");
+ DEBUG("StartParkBottom");
}
void StopParkBottom(void)
@@ -67,10 +67,10 @@
AppTimer_delete(LeaveTestAreaLongtime);
currTarget = NONE;
- TextOsd(0, "ParkBottom End");
+ DEBUG("ParkBottom End");
}
-int TestParkBottom(vector<int>&err, const Polygon *map, const car_model_cache_t *car, double speed, int run_status)
+int TestParkBottom(vector<int>&err, const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int run_status)
{
int status = 0;
@@ -84,6 +84,8 @@
if (parkTimeout) {
// 涓嶅悎鏍硷細鍔ㄤ綔瓒呮椂
err.push_back(10);
+
+
status = -1;
}
// 鏄惁鍘嬬嚎
@@ -94,8 +96,8 @@
}
if (trigLeaveTestAreaDetect) {
- if (IntersectionOf(car->points[car->desc->front_left_tire[TIRE_OUTSIDE]], map) == GM_Containment &&
- IntersectionOf(car->points[car->desc->front_right_tire[TIRE_OUTSIDE]], map) == GM_Containment) {
+ 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);
}
@@ -114,29 +116,27 @@
if (!((leftTireCrossLeftLine && rightTireCrossLeftLine) ||
(leftTireCrossRightLine && rightTireCrossRightLine))) {
DEBUG("CrossCtrlLine");
- CrossCtrlLine(map, car);
+ 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;
- TextOsd(0, "鏉ラ亾鍦虹帺鐨�");
DEBUG("鏉ラ亾鍦虹帺鐨�");
} else {
// 涓嶅悎鏍硷細鏈寜瑙勫畾绾胯矾琛岄┒锛堢洿鎺ヨ窇鍑烘祴璇曞尯浜嗭級
err.push_back(6);
status = -1;
- TextOsd(0, "鐩存帴璺戝嚭娴嬭瘯鍖轰簡");
DEBUG("鐩存帴璺戝嚭娴嬭瘯鍖轰簡");
}
} else if (!trigLeaveTestAreaDetect) {
Polygon car_body;
- car_body.num = car->desc->body_num;
+ 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->points[car->desc->body[i]];
+ car_body.point[i] = car->carXY[car->body[i]];
}
if (IntersectionOf(map, &car_body) == GM_None) {
@@ -196,12 +196,12 @@
parkTimeout = false;
leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;
- TextOsd(0, "绗竴娆″�掑簱");
+ DEBUG("绗竴娆″�掑簱");
} else if (the_ctrl_line_crossed > 0) {
currTarget = SECOND_PARK;
carParkSuccess = false;
leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;
- TextOsd(0, "绗簩娆″�掑簱");
+ DEBUG("绗簩娆″�掑簱");
} else if (currTarget != THIRD_TOUCH_CTRL_LINE) {
// 涓嶅悎鏍硷細鍊掕溅鍓嶏紝2鍓嶈疆娌￠┒杩囨帶鍒剁嚎
err.push_back(9);
@@ -243,10 +243,10 @@
status = -1;
} else if (currTarget == FIRST_PARK) {
currTarget = SECOND_TOUCH_CTRL_LINE;
- TextOsd(0, "杩囧彟涓�鏍规帶鍒剁嚎");
+ DEBUG("杩囧彟涓�鏍规帶鍒剁嚎");
} else {
currTarget = THIRD_TOUCH_CTRL_LINE;
- TextOsd(0, "鍐嶈繃绗竴鏍规帶鍒剁嚎");
+ DEBUG("鍐嶈繃绗竴鏍规帶鍒剁嚎");
}
}
carStopEvent = false;
@@ -279,25 +279,21 @@
}
// 妫�娴�2鍓嶈疆鏄惁姝ｅ悜瓒婅繃宸﹀彸鎺у埗绾�
-static void CrossCtrlLine(const Polygon *map, const car_model_cache_t *car)
+static void CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car)
{
Line leftCtrlLine, rightCtrlLine;
Line track1;
PointF p1, p2;
- car_model_cache_t *prev_car = GetCarModelCache(1);
- if (prev_car == NULL)
- return;
-
MakeLine(&leftCtrlLine, &map->point[0], &map->point[1]);
MakeLine(&rightCtrlLine, &map->point[6], &map->point[7]);
// 宸﹀墠杞紝鍙栬疆瀹界殑涓偣
- p1.X = (car->points[car->desc->front_left_tire[TIRE_OUTSIDE]].X + car->points[car->desc->front_left_tire[TIRE_INSIDE]].X) / 2;
- p1.Y = (car->points[car->desc->front_left_tire[TIRE_OUTSIDE]].Y + car->points[car->desc->front_left_tire[TIRE_INSIDE]].Y) / 2;
+ 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->points[prev_car->desc->front_left_tire[TIRE_OUTSIDE]].X + prev_car->points[prev_car->desc->front_left_tire[TIRE_INSIDE]].X) / 2;
- p2.Y = (prev_car->points[prev_car->desc->front_left_tire[TIRE_OUTSIDE]].Y + prev_car->points[prev_car->desc->front_left_tire[TIRE_INSIDE]].Y) / 2;
+ p2.X = (prev_car->carXY[prev_car->left_front_tire[TIRE_OUTSIDE]].X + prev_car->carXY[prev_car->left_front_tire[TIRE_INSIDE]].X) / 2;
+ p2.Y = (prev_car->carXY[prev_car->left_front_tire[TIRE_OUTSIDE]].Y + prev_car->carXY[prev_car->left_front_tire[TIRE_INSIDE]].Y) / 2;
MakeLine(&track1, &p1, &p2);
@@ -312,11 +308,11 @@
}
// 鍙冲墠杞�
- p1.X = (car->points[car->desc->front_right_tire[TIRE_OUTSIDE]].X + car->points[car->desc->front_right_tire[TIRE_INSIDE]].X) / 2;
- p1.Y = (car->points[car->desc->front_right_tire[TIRE_OUTSIDE]].Y + car->points[car->desc->front_right_tire[TIRE_INSIDE]].Y) / 2;
+ 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->points[prev_car->desc->front_right_tire[TIRE_OUTSIDE]].X + prev_car->points[prev_car->desc->front_right_tire[TIRE_INSIDE]].X) / 2;
- p2.Y = (prev_car->points[prev_car->desc->front_right_tire[TIRE_OUTSIDE]].Y + prev_car->points[prev_car->desc->front_right_tire[TIRE_INSIDE]].Y) / 2;
+ p2.X = (prev_car->carXY[prev_car->right_front_tire[TIRE_OUTSIDE]].X + prev_car->carXY[prev_car->right_front_tire[TIRE_INSIDE]].X) / 2;
+ p2.Y = (prev_car->carXY[prev_car->right_front_tire[TIRE_OUTSIDE]].Y + prev_car->carXY[prev_car->right_front_tire[TIRE_INSIDE]].Y) / 2;
MakeLine(&track1, &p1, &p2);
@@ -330,16 +326,16 @@
}
}
-static bool EnterParking(const Polygon *map, const car_model_cache_t *car) {
+static bool EnterParking(const Polygon *map, const car_model *car) {
bool succ = false;
Polygon parking;
Polygon car_body;
- car_body.num = car->desc->body_num;
+ 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->points[car->desc->body[i]];
+ car_body.point[i] = car->carXY[car->body[i]];
}
MakePolygon(&parking, {map->point[2], map->point[3], map->point[4], map->point[5]});
@@ -354,7 +350,7 @@
return succ;
}
-static bool CrashRedLine(const Polygon *map, const car_model_cache_t *car)
+static bool CrashRedLine(const Polygon *map, const car_model *car)
{
bool ret = false;
@@ -363,10 +359,10 @@
Polygon car_body;
- car_body.num = car->desc->body_num;
+ 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->points[car->desc->body[i]];
+ car_body.point[i] = car->carXY[car->body[i]];
}
for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
diff --git a/lib/src/main/cpp/test_items/park_bottom.h b/lib/src/main/cpp/test_items/park_bottom.h
index 7fc8606..a8ac1d4 100644
--- a/lib/src/main/cpp/test_items/park_bottom.h
+++ b/lib/src/main/cpp/test_items/park_bottom.h
@@ -13,6 +13,6 @@
void StartParkBottom(void);
void StopParkBottom(void);
-int TestParkBottom(vector<int>&err, const Polygon *map, const car_model_cache_t *car, double speed, int run_status);
+int TestParkBottom(vector<int>&err, const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int run_status);
#endif //RTKDRIVERTEST_PARK_BOTTOM_H
--
Gitblit v1.8.0