From 7c5b25c8617807635e9f272ebe9e98304d07cf2f Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期五, 14 二月 2020 14:28:25 +0800
Subject: [PATCH] 增加单片机信息传递。
---
lib/src/main/cpp/driver_test.cpp | 44 +++++-
lib/src/main/cpp/test_items/stop_and_start.cpp | 28 ++--
lib/src/main/cpp/test_items/comm_test.h | 14 ++
lib/src/main/cpp/CMakeLists.txt | 1
lib/src/main/cpp/driver_test.h | 3
lib/src/main/cpp/master/comm_if.h | 10 +
lib/src/main/cpp/test_items/turn_a90.cpp | 29 ++--
lib/src/main/cpp/test_items/park_edge.cpp | 18 +-
lib/src/main/cpp/rtk_platform/platform.h | 1
lib/src/main/cpp/master/comm_if.cpp | 35 +++++
lib/src/main/cpp/test_items/comm_test.cpp | 63 ++++++++++
lib/src/main/cpp/utils/xconvert.h | 2
lib/src/main/cpp/utils/xconvert.cpp | 23 +++
lib/src/main/cpp/mcu/mcu_if.cpp | 9
lib/src/main/cpp/rtk_platform/platform.cpp | 24 ++++
15 files changed, 248 insertions(+), 56 deletions(-)
diff --git a/lib/src/main/cpp/CMakeLists.txt b/lib/src/main/cpp/CMakeLists.txt
index 759cbf1..044fbc1 100644
--- a/lib/src/main/cpp/CMakeLists.txt
+++ b/lib/src/main/cpp/CMakeLists.txt
@@ -33,6 +33,7 @@
test_items/stop_and_start.cpp
test_items/driving_curve.cpp
test_items/turn_a90.cpp
+ test_items/comm_test.cpp
rtk_module/rtk.cpp
rtk_module/virtual_rtk.cpp
master/comm_if.cpp
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 64c63c9..ca588b9 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -80,6 +80,13 @@
static car_model *CarModel = NULL;
static car_model *CarModelPrev = NULL;
+static struct sensor_cfg {
+ int gpioId;
+ int funId;
+ int validLvl;
+} SensorConfig[32];
+static int SensorNum = 0;
+
#define MOV_AVG_SIZE 1
#define RTK_BUFFER_SIZE 100
#define CAR_MODEL_CACHE_SIZE 10
@@ -104,6 +111,9 @@
MapNum = 0;
CarModel = NULL;
CarModelPrev = NULL;
+
+ SensorNum = 0;
+ memset(SensorConfig, 0, sizeof(SensorConfig));
RtkBuffer = (rtk_info *) malloc(RTK_BUFFER_SIZE * sizeof(rtk_info));
RtkBufferNum = RtkBufferIn = 0;
@@ -287,9 +297,26 @@
DEBUG("SetCarMeasurePoint Calc Over");
}
-void SetSensorCfg(int (*sensor)[2], int sensorNum)
+void SetSensorCfg(int (*sensor)[3], int sensorNum)
{
DEBUG("SetSensorCfg sensorNum %d", sensorNum);
+
+ 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];
+ }
+}
+
+void GetFuncGpio(int func, int &gpio, int &lvl)
+{
+ for (int i = 0; i < SensorNum; ++i) {
+ if (SensorConfig[i].funId == func) {
+ gpio = SensorConfig[i].gpioId;
+ lvl = SensorConfig[i].validLvl;
+ }
+ }
}
void StartDriverExam(int start)
@@ -345,7 +372,7 @@
RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);
brief.qf = RtkBuffer[index].qf;
- brief.map_id = 865;//GetMapId(CurrExamMapIndex, MapList, MapNum);
+ brief.map_id = 866;//GetMapId(CurrExamMapIndex, MapList, MapNum);
brief.move = move;
brief.speed = speed * 3.6;
brief.heading = RtkBuffer[index].heading;
@@ -469,15 +496,14 @@
}
/*************************************************
- * 2娆￠噰鏍风浉宸殑鏃堕棿
+ * 2娆￠噰鏍风浉宸殑鏃堕棿, a 鏈�杩戠殑锛宐 鍏堝墠鐨�
* @param a
* @param b
* @return ms
*/
static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b)
{
- 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));
+ return TimeGetDiff(a->hh, a->mm, a->ss, a->dss*10, b->hh, b->mm, b->ss, b->dss*10);
}
static bool UpdateCarCoord(double &spd, int &mov, int &idx)
@@ -686,7 +712,7 @@
{
// 杞︾殑鏈�鍓嶇偣鏄惁杩涘叆鍦板浘
for (int i = 0; i < mapNum && car != NULL; ++i) {
- if (mapList[i].type == MAP_TYPE_STOP_START) {
+ /*if (mapList[i].type == MAP_TYPE_STOP_START) {
// 鏋勯�犺櫄鎷熺殑宸︿笂瑙掔偣
double x9, y9, xo, yo;
@@ -721,7 +747,7 @@
free(map.point);
if (enter) return i;
- } /*else if (mapList[i].type == MAP_TYPE_CURVE) {
+ } 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 (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
@@ -734,7 +760,7 @@
DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
return i;
}
- } else if (mapList[i].type == MAP_TYPE_TURN_90) {
+ } else */if (mapList[i].type == MAP_TYPE_TURN_90) {
if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
Line enterLine1;
@@ -743,7 +769,7 @@
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 c1c2b58..339469c 100644
--- a/lib/src/main/cpp/driver_test.h
+++ b/lib/src/main/cpp/driver_test.h
@@ -81,7 +81,8 @@
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
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)[2], int sensorNum);
+void SetSensorCfg(int (*sensor)[3], int sensorNum);
+void GetFuncGpio(int func, int &gpio, int &lvl);
void StartDriverExam(int start);
void UpdateRTKInfo(const rtk_info *s);
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 fc9f457..688ffe2 100644
--- a/lib/src/main/cpp/master/comm_if.cpp
+++ b/lib/src/main/cpp/master/comm_if.cpp
@@ -36,7 +36,7 @@
#define ID_MS_MAP 0x8006
#define ID_SM_READ_CAR 0x0007
#define ID_MS_CAR 0x8007
-#define ID_SM_READ_SENSOR_CFG 0x0008
+#define ID_SM_MCU_BRIEF 0x0008
#define ID_MS_SENSOR_CFG 0x8008
#define ID_MS_START_EXAM 0x8009
#define ID_SM_EXAM_STATUS 0x0009
@@ -135,7 +135,7 @@
void MA_ReadSensor(void)
{
- SendMsgToMainProc(ID_SM_READ_SENSOR_CFG, NULL);
+
}
void MA_SendExamStatus(int start, int errorCode)
@@ -153,6 +153,31 @@
writer.EndObject();
SendMsgToMainProc(ID_SM_EXAM_STATUS, sb.GetString());
+}
+
+void MA_SendMcuBrief(const struct mcuBrief *brief)
+{
+ StringBuffer sb;
+ Writer<StringBuffer> writer(sb);
+
+ writer.StartObject();
+
+ writer.Key("version");
+ writer.String(brief->version);
+ writer.Key("selftest");
+ writer.Int(brief->selftest);
+ writer.Key("gpio");
+ writer.Int(brief->gpio);
+ writer.Key("speed");
+ writer.Int(brief->speed);
+ writer.Key("engine");
+ writer.Int(brief->engine);
+ writer.Key("sn");
+ writer.String(brief->sn);
+
+ writer.EndObject();
+
+ SendMsgToMainProc(ID_SM_MCU_BRIEF, sb.GetString());
}
void MA_SendGpsBrief(const struct gpsBrief *brief)
@@ -615,7 +640,7 @@
if (a.IsArray() && a.Size() > 0) {
int n = a.Size();
int i = 0;
- int (*sensor)[2] = (int (*)[2]) new int[n][2];
+ int (*sensor)[3] = (int (*)[3]) new int[n][3];
for (Value::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr) {
// a gpio mapping
@@ -627,6 +652,10 @@
const Value &s = (*itr)["func_id"];
sensor[i][1] = s.GetInt();
}
+ if (itr->HasMember("level")) {
+ const Value &s = (*itr)["level"];
+ sensor[i][2] = s.GetInt();
+ }
++i;
}
SetSensorCfg(sensor, n);
diff --git a/lib/src/main/cpp/master/comm_if.h b/lib/src/main/cpp/master/comm_if.h
index aaeae65..deae21e 100644
--- a/lib/src/main/cpp/master/comm_if.h
+++ b/lib/src/main/cpp/master/comm_if.h
@@ -8,6 +8,15 @@
#include <cstdint>
#include "../driver_test.h"
+struct mcuBrief {
+ char version[70];
+ int selftest;
+ int gpio;
+ int speed;
+ int engine;
+ char sn[20];
+};
+
struct gpsBrief {
char utc[32];
int sat_num;
@@ -61,6 +70,7 @@
void MA_ReadCar(void);
void MA_ReadSensor(void);
void MA_SendExamStatus(int start, int errorCode);
+void MA_SendMcuBrief(const struct mcuBrief *brief);
void MA_SendGpsBrief(const struct gpsBrief *brief);
void MA_SendRtkBrief(const struct rtkBrief *brief);
void MA_SendCarPosition(const struct carBrief *brief);
diff --git a/lib/src/main/cpp/mcu/mcu_if.cpp b/lib/src/main/cpp/mcu/mcu_if.cpp
index 44e0c9e..9821685 100644
--- a/lib/src/main/cpp/mcu/mcu_if.cpp
+++ b/lib/src/main/cpp/mcu/mcu_if.cpp
@@ -11,6 +11,7 @@
#include "../defs.h"
#include "../jni_log.h"
#include "../common/serial_port.h"
+#include "../rtk_platform/platform.h"
#define DEBUG(fmt, args...) LOGD("<mcu_if> <%s>: " fmt, __func__, ##args)
@@ -62,6 +63,7 @@
static int dfuTryCnt = 0;
const int DFU_MAX_TRY = 3;
const int DFU_FILE_BLOCK_SIZE = 896;
+
static void *UartThread1(void *p);
static void ParseMcuTimeout(union sigval sig);
@@ -357,12 +359,9 @@
break;
case ID_MC_CAR_INFO: {
DEBUG("ID_MC_CAR_INFO %d", lenth);
- char ver[64] = {0};
- memcpy(ver, data, 32);
-
- DEBUG("ID_MC_CAR_INFO %s", ver);
-
+ if (lenth > 0)
+ PlatformStatusChanged(MCU_UPDATE_EVT, data, lenth);
break;
}
case ID_MC_RTK_DATA:
diff --git a/lib/src/main/cpp/rtk_platform/platform.cpp b/lib/src/main/cpp/rtk_platform/platform.cpp
index 322708f..bd801f8 100644
--- a/lib/src/main/cpp/rtk_platform/platform.cpp
+++ b/lib/src/main/cpp/rtk_platform/platform.cpp
@@ -390,6 +390,30 @@
MA_SendRtkBrief(&brief);
UpdateRTKInfo(rtk);
}
+ if (events & MCU_UPDATE_EVT) {
+ DEBUG("MCU_UPDATE_EVT length %d", length);
+ // 0-31 version
+ // 32-33 selftest
+ // 34-35 gpio
+ // 36-37 speed
+ // 38-39 engine
+ // 40-55 sn
+ struct mcuBrief brief;
+
+ memset(&brief, 0, sizeof(brief));
+
+ int x = 0;
+ while(data[x] != 0 && x < 32) x++;
+
+ ConvertHex2String(brief.version, data, x);
+ brief.selftest = BUILD_UINT16(data[33], data[32]);
+ brief.gpio = BUILD_UINT16(data[35], data[34]);
+ brief.speed = BUILD_UINT16(data[37], data[36]);
+ brief.engine = BUILD_UINT16(data[39], data[38]);
+ memcpy(brief.sn, data+40, 16);
+
+ MA_SendMcuBrief(&brief);
+ }
}
static void *PlatformDataListenThread(void *p) {
diff --git a/lib/src/main/cpp/rtk_platform/platform.h b/lib/src/main/cpp/rtk_platform/platform.h
index 4008293..05898a5 100644
--- a/lib/src/main/cpp/rtk_platform/platform.h
+++ b/lib/src/main/cpp/rtk_platform/platform.h
@@ -13,6 +13,7 @@
#define PLATFORM_LOGIN_EVT 0x0008
#define RTK_UPDATE_EVT 0x0010
#define GPS_UPDATE_EVT 0x0020
+#define MCU_UPDATE_EVT 0x0040
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
new file mode 100644
index 0000000..3ea8099
--- /dev/null
+++ b/lib/src/main/cpp/test_items/comm_test.cpp
@@ -0,0 +1,63 @@
+//
+// Created by fctom on 2020/2/13.
+//
+
+#include "comm_test.h"
+#include "../driver_test.h"
+#include "../defs.h"
+
+static bool seatbeltInsert;
+static bool engineStart;
+
+const int ENGINE_MIN_ROTATE = 200;
+
+static uint16_t gpioStore = 0;
+static uint16_t engineStore = 0;
+
+static void SensorChange(int index, bool value);
+
+void CommTestStart(void)
+{
+ gpioStore = engineStore = 0;
+}
+
+void UpdateSensor(uint16_t gpio, uint16_t speed, uint16_t engine)
+{
+ int idx, lvl;
+
+ uint16_t chg = gpioStore^gpio;
+
+ if (chg == 0)
+ return;
+
+ for (int i = 0; i < 16; ++i) {
+ if (chg & BV(i)) {
+ SensorChange(i, (bool)(gpio & BV(i)));
+ }
+ }
+
+ gpioStore = gpio;
+
+
+
+ // 瀹夊叏甯�
+
+ // 鎸′綅
+
+ // 鍚姩鎸囩ず
+
+ // 鐔勭伀鐩戞帶
+}
+
+static void SensorChange(int index, bool value)
+{
+ int func;
+
+ switch (func) {
+ case SENSOR_SEATBELT:
+ break;
+ case SENSOR_TURNRIGHT:
+ break;
+
+ }
+}
diff --git a/lib/src/main/cpp/test_items/comm_test.h b/lib/src/main/cpp/test_items/comm_test.h
new file mode 100644
index 0000000..a1f7a5c
--- /dev/null
+++ b/lib/src/main/cpp/test_items/comm_test.h
@@ -0,0 +1,14 @@
+//
+// Created by fctom on 2020/2/13.
+//
+
+#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
+
+
+#endif //MYAPPLICATION2_COMM_TEST_H
diff --git a/lib/src/main/cpp/test_items/park_edge.cpp b/lib/src/main/cpp/test_items/park_edge.cpp
index 105bd16..c5ed6ec 100644
--- a/lib/src/main/cpp/test_items/park_edge.cpp
+++ b/lib/src/main/cpp/test_items/park_edge.cpp
@@ -17,11 +17,11 @@
using namespace std;
-const int PARK_TIMEOUT = 90;
+const int PARK_TIMEOUT = D_SEC(90);
const uint32_t STOP_CAR_TIME = D_SEC(2);
static bool reportExamTimeout;
-static uint64_t stopTimepoint = 0;
+static uint32_t stopTimepoint = 0;
static bool occurCrashRedLine1, occurCrashRedLine2;
static int prevMoveStatus, storeMoveStatusBeforeStop;
static int parkStatus;
@@ -45,7 +45,7 @@
occurMoveBack = false;
if (moveStatus == -1) {
occurMoveBack = true;
- moveBackTimePoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
+ moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
}
DEBUG("杩涘叆渚ф柟鍋滆溅鍦哄湴");
@@ -87,7 +87,7 @@
}
if (occurMoveBack) {
- uint32_t tp = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
+ uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
if (!reportExamTimeout && tp - moveBackTimePoint >= PARK_TIMEOUT) {
// 瓒呮椂90绉掞紝涓嶅悎鏍�
@@ -100,15 +100,13 @@
if (moveStatus != prevMoveStatus) {
if (moveStatus == 0) {
parkStatus = 0;
- stopTimepoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
- stopTimepoint = stopTimepoint * 1000 + rtkTime->mss*10;
+ stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
storeMoveStatusBeforeStop = prevMoveStatus;
DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
} else {
DEBUG("缁х画琛岄┒ %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
- uint64_t tp = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
- tp = tp * 1000 + rtkTime->mss * 10;
+ uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
DEBUG("鍋滆溅鏃堕棿 %ld", tp - stopTimepoint);
@@ -144,13 +142,13 @@
if (moveStatus == -1 && !occurMoveBack) {
DEBUG("寮�濮嬪�掕溅");
occurMoveBack = true;
- moveBackTimePoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
+ moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
}
}
prevMoveStatus = moveStatus;
} else if (moveStatus == 0 && parkStatus == 0) {
- uint64_t tp = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
+ 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) {
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 1398ee5..9c0c69e 100644
--- a/lib/src/main/cpp/test_items/stop_and_start.cpp
+++ b/lib/src/main/cpp/test_items/stop_and_start.cpp
@@ -16,12 +16,6 @@
using namespace std;
-enum
-{
- STOP_CAR,
- START_CAR
-};
-
const double STOP_DISTANCE_THRESHOLD_RED = 0.5;
const double EDGE_DISTANCE_THRESHOLD_RED = 0.5;
const double EDGE_DISTANCE_THRESHOLD_YELLOW = 0.3;
@@ -38,7 +32,7 @@
static PointF stopPoint;
static int prevMoveDirect;
-static uint64_t stopTimepoint = 0;
+static uint32_t stopTimepoint = 0;
static bool stopCar = false;
static uint32_t stopCarTime;
static bool occurCrashRedLine = false;
@@ -61,9 +55,7 @@
prevMoveDirect = moveDirect;
if (moveDirect == 0) {
- stopTimepoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh,
- rtkTime->mm, rtkTime->ss);
- stopTimepoint = stopTimepoint * 1000 + rtkTime->mss * 10;
+ stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
}
occurCrashRedLine = false;
@@ -96,18 +88,16 @@
if (prevMoveDirect != moveDirect) {
if (moveDirect == 0) {
- stopTimepoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
- stopTimepoint = stopTimepoint * 1000 + rtkTime->mss*10;
+ stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
}
prevMoveDirect = moveDirect;
} else if (moveDirect == 0) {
- uint64_t tp = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
- tp = tp * 1000 + rtkTime->mss * 10;
+ uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
if (tp - stopTimepoint >= STOP_CAR_TIME && !stopCar) {
// 杩欓噷鍒ゆ柇鍋滆溅鐘舵��
stopCar = true;
- stopCarTime = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
+ stopCarTime = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
stopPoint = car->carXY[car->body[0]];
@@ -161,9 +151,10 @@
}
}
+
if (!reportStartTimeout && (IntersectionOfLine(map->point[4], stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) != -1 ||
DistanceOf(stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) < 0.1)) {
- if (TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss) - stopCarTime > CAR_START_TIMEOUT) {
+ if (TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10) - stopCarTime > CAR_START_TIMEOUT) {
// 璧锋鏃堕棿瓒呰繃30绉掞紝涓嶅悎鏍�
AddExamFault(15, rtkTime);
DEBUG("璧锋鏃堕棿瓒呰繃30绉�");
@@ -217,6 +208,9 @@
int rel1 = IntersectionOfLine(map->point[4], map->point[3], car->carXY[car->body[0]]);
int rel2 = IntersectionOfLine(map->point[5], map->point[6], car->carXY[car->body[0]]);
+ DEBUG("%d %d %f, %f", car->body[0], car->axial[AXIAL_FRONT], car->carXY[car->body[0]].X, car->carXY[car->body[0]].Y);
+ DEBUG("rel1 %d rel2 %d", rel1, rel2);
+
if (rel1 == 1) {
Line line1;
@@ -231,6 +225,8 @@
dis = DistanceOf(car->carXY[car->body[0]], line2);
}
+ DEBUG("DistanceOfHead2Stopline dis %f", dis);
+
return dis;
}
diff --git a/lib/src/main/cpp/test_items/turn_a90.cpp b/lib/src/main/cpp/test_items/turn_a90.cpp
index d03e269..f76ac16 100644
--- a/lib/src/main/cpp/test_items/turn_a90.cpp
+++ b/lib/src/main/cpp/test_items/turn_a90.cpp
@@ -21,9 +21,9 @@
static bool TA90Testing;
-static int azimuth;
+static int enterAreaHeading;
static bool turnLeftFinished;
-static uint64_t stopTimepoint = 0;
+static uint32_t stopTimepoint = 0;
static bool reportStopCarTimeout;
static int prevMoveDirect;
@@ -34,11 +34,11 @@
void StartTurnA90(int moveDirect, double heading, const struct RtkTime *rtkTime)
{
- azimuth = (int) heading;
+ DEBUG("杩涘叆鐩磋杞集鍦哄湴");
+ enterAreaHeading = (int) heading;
prevMoveDirect = moveDirect;
if (moveDirect == 0) {
- stopTimepoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
- stopTimepoint = stopTimepoint * 1000 + rtkTime->mss*10;
+ stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
}
reportStopCarTimeout = false;
crashRedLine = false;
@@ -66,8 +66,7 @@
if (moveDirect != prevMoveDirect) {
if (moveDirect == 0) {
- stopTimepoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
- stopTimepoint = stopTimepoint * 1000 + rtkTime->mss*10;
+ stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
reportStopCarTimeout = false;
DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
@@ -76,8 +75,7 @@
}
prevMoveDirect = moveDirect;
} else if (moveDirect == 0) {
- uint64_t tp = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
- tp = tp * 1000 + rtkTime->mss * 10;
+ uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
if (tp - stopTimepoint >= STOP_CAR_TIME && !reportStopCarTimeout) {
// 鍋滆溅瓒�2绉掞紝姣忔鎵�5鍒�
@@ -90,14 +88,21 @@
// 妫�鏌ヨ浆鍚戠姸鎬�
int az = (int) heading;
- if (ABS(az - azimuth) > 180) {
- az = 360 - ABS(az-azimuth);
+ if (ABS(az - enterAreaHeading) > 180) {
+ az = 360 - ABS(az-enterAreaHeading);
} else {
- az = ABS(az - azimuth);
+ az = ABS(az - enterAreaHeading);
}
+
+
if (az >= 30) {
if (!turnLeftFinished) {
+ if((( ((int)heading) + 360 - enterAreaHeading) % 360) < 180) {
+ DEBUG("鍙宠浆");
+ } else {
+ DEBUG("宸﹁浆");
+ }
// 杞悜鐏湭寮�鍚紝鎵�10鍒�
AddExamFault(30, rtkTime);
DEBUG("杞悜鐏湭寮�鍚�");
diff --git a/lib/src/main/cpp/utils/xconvert.cpp b/lib/src/main/cpp/utils/xconvert.cpp
index 62aeca2..0232370 100644
--- a/lib/src/main/cpp/utils/xconvert.cpp
+++ b/lib/src/main/cpp/utils/xconvert.cpp
@@ -107,3 +107,26 @@
totalSeconds += daysThisYear * SECONDS_PER_DAY;
return totalSeconds;
}
+
+uint32_t TimeMakeComposite(int hour, int minute, int second, int msecond)
+{
+ return (hour*SECONDS_PER_HOUR + minute*SECONDS_PER_MINUTE + second) * 1000 + msecond;
+}
+
+/*********************************************************
+ * TIME1 - TIME2: msecond
+ * @param hour1
+ * @param minute1
+ * @param second1
+ * @param msecond1
+ * @param hour2
+ * @param minute2
+ * @param second2
+ * @param msecond2
+ * @return
+ */
+uint32_t TimeGetDiff(int hour1, int minute1, int second1, int msecond1, int hour2, int minute2, int second2, int msecond2)
+{
+ return (TimeMakeComposite(hour1, minute1, second1, msecond1) + SECONDS_PER_DAY * 1000 -
+ TimeMakeComposite(hour2, minute2, second2, msecond2)) % (SECONDS_PER_DAY * 1000);
+}
diff --git a/lib/src/main/cpp/utils/xconvert.h b/lib/src/main/cpp/utils/xconvert.h
index 7278c0c..cec7129 100644
--- a/lib/src/main/cpp/utils/xconvert.h
+++ b/lib/src/main/cpp/utils/xconvert.h
@@ -11,5 +11,7 @@
void ConvertHex2String(char *str, const uint8_t *hex, int length);
void ConvertString2Hex(uint8_t *hex, int length, const char *str);
uint32_t TimeMakeComposite(int year, int month, int day, int hour, int minute, int second);
+uint32_t TimeMakeComposite(int hour, int minute, int second, int msecond);
+uint32_t TimeGetDiff(int hour1, int minute1, int second1, int msecond1, int hour2, int minute2, int second2, int msecond2);
#endif //MYAPPLICATION2_XCONVERT_H
--
Gitblit v1.8.0