From 7c66884c800b0b48c2b3aac2ac29ff0b4529b95e Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期二, 21 一月 2020 15:00:12 +0800
Subject: [PATCH] updatecrrDOC1
---
lib/src/main/cpp/test_items/park_bottom.cpp | 39 +++----
lib/src/main/cpp/driver_test.cpp | 82 +++++++++-------
lib/src/main/cpp/master/comm_if.cpp | 20 ++-
lib/src/main/cpp/native-lib.cpp | 3
lib/src/main/cpp/CMakeLists.txt | 1
lib/src/main/cpp/rtk_module/rtk.cpp | 19 ++-
lib/src/main/cpp/rtk_module/virtual_rtk.cpp | 95 +++++++++++++++++++
lib/src/main/cpp/rtk_module/virtual_rtk.h | 10 ++
8 files changed, 198 insertions(+), 71 deletions(-)
diff --git a/lib/src/main/cpp/CMakeLists.txt b/lib/src/main/cpp/CMakeLists.txt
index a2228df..759cbf1 100644
--- a/lib/src/main/cpp/CMakeLists.txt
+++ b/lib/src/main/cpp/CMakeLists.txt
@@ -34,6 +34,7 @@
test_items/driving_curve.cpp
test_items/turn_a90.cpp
rtk_module/rtk.cpp
+ rtk_module/virtual_rtk.cpp
master/comm_if.cpp
utils/crc16.cpp
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 41b802d..d2d2d44 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -74,7 +74,7 @@
} MapList[MAP_LIST_SIZE];
static int MapNum = 0;
-static int CurrExamMapId = -1;
+static int CurrExamMapIndex = -1;
static int CurrExamStatus = 0; // 1 娴嬭瘯瀹屾垚 0 娴嬭瘯涓� -1 娴嬭瘯閿欒閫�鍑�
static car_model *CarModel = NULL;
@@ -94,7 +94,8 @@
static bool FrontTireEnterArea(const Polygon *car, const Polygon *map);
static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum);
static bool ExitMap(const car_model *car, int mapId, const struct map_list *mapList, int mapNum);
-static int GetMapType(int id, const struct map_list *mapList, int mapNum);
+static int GetMapId(int index, const struct map_list *mapList, int mapNum);
+static int GetMapType(int index, const struct map_list *mapList, int mapNum);
void DriverTestInit(void)
{
@@ -201,6 +202,7 @@
CarModelPrev = NULL;
}
+ // 浠呭垎閰峜arXY, body鍜宑arDesc鍧囨病鏈�
CarModelPrev = (car_model *)malloc(sizeof(car_model));
CarModelPrev->body = NULL;
CarModelPrev->carDesc = NULL;
@@ -337,7 +339,7 @@
RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);
brief.qf = RtkBuffer[index].qf;
- brief.map_id = 863;//CurrExamMapId;
+ brief.map_id = 863;//GetMapId(CurrExamMapIndex, MapList, MapNum);
brief.move = move;
brief.speed = speed * 3.6;
brief.heading = RtkBuffer[index].heading;
@@ -374,16 +376,16 @@
free(brief.point);
if (TestStart) {
- if (CurrExamMapId < 0) {
- CurrExamMapId = EnterMap(CarModel, MapList, MapNum);
- int mtype = GetMapType(CurrExamMapId, MapList, MapNum);
+ if (CurrExamMapIndex < 0) {
+ CurrExamMapIndex = EnterMap(CarModel, MapList, MapNum);
+ int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);
- DEBUG("CurrExamMapId %d mtype %d", CurrExamMapId, mtype);
+ DEBUG("CurrExamMapIndex %d mtype %d", GetMapId(CurrExamMapIndex, MapList, MapNum), mtype);
switch (mtype) {
case MAP_TYPE_PARK_BUTTOM:
DEBUG("杩涘叆鍊掕溅鍏ュ簱鍦哄湴");
- MA_SendDebugInfo("杩涘叆鍊掕溅鍏ュ簱鍦哄湴 %d", CurrExamMapId);
+ MA_SendDebugInfo("杩涘叆鍊掕溅鍏ュ簱鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
StartParkBottom();
CurrExamStatus = 0;
@@ -411,12 +413,12 @@
rtkTime.ss = RtkBuffer[index].ss;
rtkTime.mss = RtkBuffer[index].dss;
- if (CurrExamMapId >= 0) {
+ if (CurrExamMapIndex >= 0) {
if (CurrExamStatus == 0) {
- int mtype = GetMapType(CurrExamMapId, MapList, MapNum);
+ int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);
switch (mtype) {
case MAP_TYPE_PARK_BUTTOM:
- CurrExamStatus = TestParkBottom(ErrorList, &MapList[CurrExamMapId].map,
+ CurrExamStatus = TestParkBottom(ErrorList, &MapList[CurrExamMapIndex].map,
CarModel, CarModelPrev, speed, move, &rtkTime);
break;
case MAP_TYPE_STOP_START:
@@ -432,10 +434,10 @@
}
}
if (CurrExamStatus != 0) {
- if (ExitMap(CarModel, CurrExamMapId, MapList, MapNum)) {
- DEBUG("閫�鍑哄満鍦� %d", CurrExamMapId);
- MA_SendDebugInfo("閫�鍑哄満鍦� %d", CurrExamMapId);
- CurrExamMapId = -1;
+ if (ExitMap(CarModel, CurrExamMapIndex, MapList, MapNum)) {
+ DEBUG("閫�鍑哄満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
+ MA_SendDebugInfo("閫�鍑哄満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
+ CurrExamMapIndex = -1;
}
}
}
@@ -450,8 +452,8 @@
*/
static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b)
{
- return ABS(TimeMakeComposite(2000 + a->YY, a->MM, a->DD, a->hh, a->mm, a->ss) * 1000 + a->dss*10 -
- (TimeMakeComposite(2000 + b->YY, b->MM, b->DD, b->hh, b->mm, b->ss) * 1000 + b->dss*10));
+ return ABS((long)(TimeMakeComposite(2000 + a->YY, a->MM, a->DD, a->hh, a->mm, a->ss) * 1000 + a->dss*10) -
+ (long)(TimeMakeComposite(2000 + b->YY, b->MM, b->DD, b->hh, b->mm, b->ss) * 1000 + b->dss*10));
}
static bool UpdateCarCoord(double &spd, int &mov, int &idx)
@@ -663,42 +665,48 @@
if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 &&
DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
- return mapList[i].id;
+ return i;
}
}
}
return -1;
}
-static bool ExitMap(const car_model *car, int mapId, const struct map_list *mapList, int mapNum)
+static bool ExitMap(const car_model *car, int index, const struct map_list *mapList, int mapNum)
{
// 杞︾殑鏈�鍚庣偣鏄惁杩涘叆鍦板浘
- for (int i = 0; i < mapNum && car != NULL; ++i) {
- if (mapList[i].id == mapId) {
- if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
- if (IntersectionOf(car->carXY[ car->axial[AXIAL_REAR] ], &mapList[i].map) == GM_None) {
- Line enterLine1, enterLine2;
+ if (index < 0 || mapList == NULL || mapNum == 0) return true;
- MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
- MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
+ if (mapList[index].type == MAP_TYPE_PARK_BUTTOM) {
+ if (IntersectionOf(car->carXY[car->axial[AXIAL_REAR]], &mapList[index].map) == GM_None) {
+ Line enterLine1, enterLine2;
- if (DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine1) > 0.1 &&
- DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine2) > 0.1)
- return true;
- }
- }
- return false;
+ MakeLine(&enterLine1, &(mapList[index].map.point[0]), &(mapList[index].map.point[1]));
+ MakeLine(&enterLine2, &(mapList[index].map.point[6]), &(mapList[index].map.point[7]));
+
+ if (DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine1) > 0.1 &&
+ DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine2) > 0.1)
+ return true;
}
}
+
return true;
}
-static int GetMapType(int id, const struct map_list *mapList, int mapNum)
+static int GetMapId(int index, const struct map_list *mapList, int mapNum)
{
- for (int i = 0; i < mapNum && id >= 0; ++i) {
- if (mapList[i].id == id) return mapList[i].type;
- }
- return -1;
+ if (index < 0 || mapList == NULL || mapNum == 0)
+ return -1;
+
+ return mapList[index].id;
+}
+
+static int GetMapType(int index, const struct map_list *mapList, int mapNum)
+{
+ if (index < 0 || mapList == NULL || mapNum == 0)
+ return -1;
+
+ return mapList[index].type;
}
void AddExamFault(int wrong, const struct RtkTime *rtkTime)
diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp
index 798b389..f799243 100644
--- a/lib/src/main/cpp/master/comm_if.cpp
+++ b/lib/src/main/cpp/master/comm_if.cpp
@@ -48,7 +48,13 @@
#define ID_SM_RTCM_IND 0x000F
#define ID_SM_DEBUG_INFO 0x0010
-static int OnOff = 0xFFFF;
+#define MA_OUT_GPS_BRIEF 0x0001
+#define MA_OUT_RTK_BRIEF 0x0002
+#define MA_OUT_CAR_BRIEF 0x0004
+#define MA_OUT_RTCM_IND 0x0008
+#define MA_OUT_DBG_INFO 0x0010
+
+static int OnOff = 0;//0xFFFF;
void MA_NdkStart(void)
{
@@ -149,7 +155,7 @@
void MA_SendGpsBrief(const struct gpsBrief *brief)
{
- if (!(OnOff & BV(0)))
+ if (!(OnOff & MA_OUT_GPS_BRIEF))
return;
StringBuffer sb;
@@ -179,7 +185,7 @@
void MA_SendRtkBrief(const struct rtkBrief *brief)
{
- if (!(OnOff & BV(1)))
+ if (!(OnOff & MA_OUT_RTK_BRIEF))
return;
char a[2] = {brief->coord_x_dir, 0};
@@ -240,7 +246,7 @@
void MA_SendCarPosition(const struct carBrief *brief)
{
- if (!(OnOff & BV(2))) return;
+ if (!(OnOff & MA_OUT_CAR_BRIEF)) return;
StringBuffer sb;
Writer<StringBuffer> writer(sb);
@@ -317,7 +323,7 @@
void MA_SendRtcmInd(int length)
{
- if (!(OnOff & BV(3))) return;
+ if (!(OnOff & MA_OUT_RTCM_IND)) return;
StringBuffer sb;
Writer<StringBuffer> writer(sb);
@@ -332,7 +338,7 @@
void MA_SendDebugInfo(const char *str, ...)
{
- if (!(OnOff & BV(4)))
+ if (!(OnOff & MA_OUT_DBG_INFO))
return;
va_list argp;
@@ -358,6 +364,8 @@
{
switch (cmd) {
case ID_MS_RTK_PLAT_CFG: {
+ if (OnOff == 0) OnOff = MA_OUT_GPS_BRIEF + MA_OUT_RTK_BRIEF + MA_OUT_CAR_BRIEF;
+
rtk_platform_cfg_t cfg;
memset(&cfg, 0, sizeof(cfg));
diff --git a/lib/src/main/cpp/native-lib.cpp b/lib/src/main/cpp/native-lib.cpp
index e813fed..a605f17 100644
--- a/lib/src/main/cpp/native-lib.cpp
+++ b/lib/src/main/cpp/native-lib.cpp
@@ -15,6 +15,7 @@
#include "mcu/mcu_if.h"
#include "driver_test.h"
#include "master/comm_if.h"
+#include "rtk_module/virtual_rtk.h"
#define DEBUG(fmt, args...) LOGD("<native-lib> <%s>: " fmt, __func__, ##args)
@@ -243,6 +244,8 @@
InitPlatform(phone, RTK_PLATFORM_IP, RTK_PLATFORM_PORT);
AppTimer_add(SendBootIndicate, 500);
+
+ InitVirtualDevice("192.168.16.104", 9001);
}
static void SendBootIndicate(union sigval sig) {
diff --git a/lib/src/main/cpp/rtk_module/rtk.cpp b/lib/src/main/cpp/rtk_module/rtk.cpp
index 4d6ae49..f753214 100644
--- a/lib/src/main/cpp/rtk_module/rtk.cpp
+++ b/lib/src/main/cpp/rtk_module/rtk.cpp
@@ -202,14 +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; /////////////////////////////
}
}
if (res == 0) {
diff --git a/lib/src/main/cpp/rtk_module/virtual_rtk.cpp b/lib/src/main/cpp/rtk_module/virtual_rtk.cpp
new file mode 100644
index 0000000..da1baa7
--- /dev/null
+++ b/lib/src/main/cpp/rtk_module/virtual_rtk.cpp
@@ -0,0 +1,95 @@
+//
+// Created by YY on 2020/1/20.
+//
+
+#include <pthread.h>
+#include <cstdlib>
+#include <cstring>
+#include "virtual_rtk.h"
+#include "../jni_log.h"
+#include "../common/net.h"
+#include "../native-lib.h"
+#include "../common/apptimer.h"
+#include "../defs.h"
+#include "parse_gps.h"
+
+#define DEBUG(fmt, args...) LOGD("<virtual_device> <%s>: " fmt, __func__, ##args)
+
+struct vSocket {
+ char domain_name[32];
+ int port;
+};
+
+static void *VDataListenThread(void *p);
+
+void InitVirtualDevice(const char *domain_name, int port)
+{
+ DEBUG("InitVirtualDevice");
+
+ struct vSocket *ptr = (struct vSocket *)malloc(sizeof(struct vSocket));
+
+ strcpy(ptr->domain_name, domain_name);
+ ptr->port = port;
+
+ pthread_t pid;
+ pthread_attr_t attr;
+ pthread_attr_init(&attr);
+ pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);//detached
+ pthread_create(&pid, &attr, VDataListenThread, ptr);
+}
+
+#define PARSE_BUFF_SIZE 4096
+
+static void *VDataListenThread(void *p) {
+ struct vSocket *vs = (struct vSocket *)p;
+
+ uint8_t RxBuf[PARSE_BUFF_SIZE];
+
+ int fds_ret;
+ struct timeval tv;
+ fd_set rdfds;
+ fd_set exfds;
+
+ int fd = -1;
+ int RxBufLen = 0;
+
+ fd = ConnectTCP(vs->domain_name, vs->port);
+
+ while (fd > 0) {
+ tv.tv_sec = 5;
+ tv.tv_usec = 0;
+ FD_ZERO(&rdfds); //clean
+ FD_SET(fd, &rdfds); //set
+
+ fds_ret = select(fd + 1, &rdfds, NULL, NULL, &tv);
+
+ if (fds_ret < 0) {
+ break;
+ } else if(fds_ret == 0) {
+ //Occur failure(such as line disconnect)
+ } else if(FD_ISSET(fd, &rdfds)) {
+ int lx = ReadTCP(fd, RxBuf + RxBufLen, sizeof(RxBuf) - RxBufLen);
+
+ if (lx < 0) {
+ break;
+ } else if (lx > 0) {
+ RxBufLen += lx;
+ 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;
+ }
+ }
+ }
+ }
+
+ if (fd > 0) {
+ DisconnectTCP(fd);
+ }
+
+ free(vs);
+ pthread_exit(NULL);
+}
diff --git a/lib/src/main/cpp/rtk_module/virtual_rtk.h b/lib/src/main/cpp/rtk_module/virtual_rtk.h
new file mode 100644
index 0000000..783bfa8
--- /dev/null
+++ b/lib/src/main/cpp/rtk_module/virtual_rtk.h
@@ -0,0 +1,10 @@
+//
+// Created by YY on 2020/1/20.
+//
+
+#ifndef MYAPPLICATION2_VIRTUAL_RTK_H
+#define MYAPPLICATION2_VIRTUAL_RTK_H
+
+void InitVirtualDevice(const char *domain_name, int port);
+
+#endif //MYAPPLICATION2_VIRTUAL_RTK_H
diff --git a/lib/src/main/cpp/test_items/park_bottom.cpp b/lib/src/main/cpp/test_items/park_bottom.cpp
index e25b216..9c4d4ba 100644
--- a/lib/src/main/cpp/test_items/park_bottom.cpp
+++ b/lib/src/main/cpp/test_items/park_bottom.cpp
@@ -292,44 +292,43 @@
Line track1;
PointF p1, p2;
- MakeLine(&leftCtrlLine, &map->point[0], &map->point[1]);
- MakeLine(&rightCtrlLine, &map->point[6], &map->point[7]);
-
+ 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[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;
-
+ 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 &&
- IntersectionOf(p1, map) == GM_None) {
+ IntersectionOfLine(map->point[0], map->point[1], p1) == -1
+ /*IntersectionOf(p1, map) == GM_None*/) {
leftTireCrossLeftLine = true;
+ DEBUG("宸﹁疆璺ㄨ秺宸︽帶鍒剁嚎");
}
-
if (IntersectionOf(track1, rightCtrlLine) == GM_Intersection &&
- IntersectionOf(p1, map) == GM_None) {
+ IntersectionOfLine(map->point[6], map->point[7], p1) == -1
+ /*IntersectionOf(p1, map) == GM_None*/) {
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[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;
-
+ 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 &&
- IntersectionOf(p1, map) == GM_None) {
+ IntersectionOfLine(map->point[0], map->point[1], p1) == -1
+ /*IntersectionOf(p1, map) == GM_None*/) {
rightTireCrossLeftLine = true;
+ DEBUG("鍙宠疆璺ㄨ秺宸︽帶鍒剁嚎");
}
if (IntersectionOf(track1, rightCtrlLine) == GM_Intersection &&
- IntersectionOf(p1, map) == GM_None) {
+ IntersectionOfLine(map->point[6], map->point[7], p1) == -1
+ /*IntersectionOf(p1, map) == GM_None*/) {
rightTireCrossRightLine = true;
+ DEBUG("鍙宠疆璺ㄨ秺鍙虫帶鍒剁嚎");
}
}
@@ -373,7 +372,7 @@
}
for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
- MakeLine(&red_line, &map->point[red_lines[i][0]], &map->point[red_lines[i][1]]);
+ MakeLine(&red_line, &(map->point[red_lines[i][0]]), &(map->point[red_lines[i][1]]));
if (IntersectionOf(red_line, &car_body) != GM_None) {
ret = true;
break;
--
Gitblit v1.8.0