From 3ce7d9cbccabf7f94d8203a98796599cd9dd5411 Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期二, 11 二月 2020 17:14:27 +0800
Subject: [PATCH] 修改了上坡起步。
---
lib/src/main/cpp/master/comm_if.h | 1
lib/src/main/cpp/test_items/park_edge.cpp | 15 +
lib/src/main/cpp/mcu/mcu_if.h | 3
lib/src/main/cpp/master/comm_if.cpp | 14 +
lib/src/main/cpp/test_items/stop_and_start.cpp | 229 ++++++++++++++-----------
lib/src/main/cpp/native-lib.cpp | 13 +
lib/src/main/cpp/test_items/stop_and_start.h | 5
lib/src/main/cpp/mcu/mcu_if.cpp | 191 ++++++++++++++++++++
lib/src/main/java/com/anyun/exam/lib/RemoteService.java | 36 +++
9 files changed, 394 insertions(+), 113 deletions(-)
diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp
index f799243..fc9f457 100644
--- a/lib/src/main/cpp/master/comm_if.cpp
+++ b/lib/src/main/cpp/master/comm_if.cpp
@@ -14,6 +14,7 @@
#include "../defs.h"
#include "../common/apptimer.h"
#include "../jni_log.h"
+#include "../mcu/mcu_if.h"
#include <string>
#include <iostream>
@@ -47,6 +48,7 @@
#define ID_SM_CAR 0x000E
#define ID_SM_RTCM_IND 0x000F
#define ID_SM_DEBUG_INFO 0x0010
+#define ID_MS_FILE 0x8100
#define MA_OUT_GPS_BRIEF 0x0001
#define MA_OUT_RTK_BRIEF 0x0002
@@ -660,3 +662,15 @@
default:break;
}
}
+
+void MA_MainProcBinMsgEntry(int cmd, const uint8_t *value, int length)
+{
+ switch (cmd) {
+ case ID_MS_FILE: {
+ UploadDfuFile(value, length);
+ break;
+ }
+ default:
+ break;
+ }
+}
diff --git a/lib/src/main/cpp/master/comm_if.h b/lib/src/main/cpp/master/comm_if.h
index 34a6cae..aaeae65 100644
--- a/lib/src/main/cpp/master/comm_if.h
+++ b/lib/src/main/cpp/master/comm_if.h
@@ -50,6 +50,7 @@
};
void MA_MainProcMsgEntry(int cmd, const char *value);
+void MA_MainProcBinMsgEntry(int cmd, const uint8_t *value, int length);
void MA_NdkStart(void);
void MA_ReqRtkPlatformConfig(void);
diff --git a/lib/src/main/cpp/mcu/mcu_if.cpp b/lib/src/main/cpp/mcu/mcu_if.cpp
index 454eb73..44e0c9e 100644
--- a/lib/src/main/cpp/mcu/mcu_if.cpp
+++ b/lib/src/main/cpp/mcu/mcu_if.cpp
@@ -4,6 +4,7 @@
#include <cstring>
#include <pthread.h>
+#include <cstdlib>
#include "mcu_if.h"
#include "../common/apptimer.h"
#include "../utils/crc16.h"
@@ -29,8 +30,8 @@
#define ID_CM_APP_BOOT 0x0001
#define ID_MC_MCU_BOOT 0x8001
-#define ID_CM_INQ_MCU_VER 0x0002
-#define ID_MC_MCU_VER 0x8002
+#define ID_CM_DFU_UPLOAD 0x0002
+#define ID_MC_DFU_RSP 0x8002
#define ID_CM_RW_INFO 0x0003
#define ID_MC_RW_INFO_RSP 0x8003
#define ID_CM_MCU_DFU_REQ 0x0004
@@ -54,12 +55,27 @@
uint16_t crc16;
}McuPkt;
+static uint8_t *dfuFile = NULL;
+static uint8_t dfuFileBitmap[128];
+
+static int dfuFileLength = 0;
+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);
static void McuCommandEntry(uint16_t id, const uint8_t *data, int lenth);
+static void SendDfuFile(int fileLen, int sentLen, int blockLen, const uint8_t *data);
+static void GoNextDfuLater(union sigval sig);
+static void GoNextDfu(void);
+
void ParseMcuInit(void)
{
+ dfuFile = NULL;
+ dfuFileLength = 0;
+
parse_status = SYNC_HEAD_ONE;
AppTimer_delete(ParseMcuTimeout);
@@ -284,8 +300,54 @@
case ID_MC_MCU_BOOT:
DEBUG("MCU BOOT");
break;
- case ID_MC_MCU_VER:
- DEBUG("MCU VER");
+ case ID_MC_DFU_RSP:
+ DEBUG("ID_MC_DFU_RSP %d len %d", data[0], lenth);
+
+ if (data[0] == 0) {
+ // 绗竴鍖呬紶杈撴垚鍔�
+ dfuFileBitmap[0] |= 0x01;
+ } else if (data[0] == 10) {
+ // 鍗囩骇瀹屾垚
+ memset(dfuFileBitmap, 0xFF, sizeof(dfuFileBitmap));
+ } else if (data[0] == 11) {
+ // 鏀惧純浼犺緭
+ UploadDfuFileEnd();
+ } else if (data[0] == 12) {
+ // 鍏ㄩ儴閲嶄紶
+ memset(dfuFileBitmap, 0, sizeof(dfuFileBitmap));
+ } else if (data[0] == 13) {
+ // 閮ㄥ垎閲嶄紶锛屾湁鍚庣画瀛楁
+ DEBUG("BITMAP %02X %02X %02X %02X %02X", data[1], data[2], data[3], data[4], data[5]);
+
+ int total = dfuFileLength / DFU_FILE_BLOCK_SIZE + ((dfuFileLength % DFU_FILE_BLOCK_SIZE)?1:0);
+ int a = 0, b = 0;
+ for (int i = 1; i < lenth; ++i) {
+ for (int j = 0; j < 8; ++j) {
+ if ((data[i] & BV(j))) b++;
+ a++;
+ if (a == total) {
+ i = lenth;
+ break;
+ }
+ }
+ }
+
+ DEBUG("BITMAP total %d succ %d", total, b);
+
+ //memset(dfuFileBitmap, 0, sizeof(dfuFileBitmap));
+ memcpy(dfuFileBitmap, data + 1, lenth - 1);
+
+ if (total % 8) {
+ dfuFileBitmap[total/8] &= ~BV((total%8) - 1);
+ } else {
+ dfuFileBitmap[(total+7)/8 - 1] &= ~BV(7);
+ }
+// dfuFileBitmap[total/8] &= ~BV(total%8);
+ }
+
+ dfuTryCnt = 0;
+ AppTimer_delete(GoNextDfuLater);
+ GoNextDfu();
break;
case ID_MC_RW_INFO_RSP:
break;
@@ -293,9 +355,16 @@
break;
case ID_MC_MCU_DFU_DATA_RSP:
break;
- case ID_MC_CAR_INFO:
- DEBUG("ID_MC_CAR_INFO");
+ 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);
+
break;
+ }
case ID_MC_RTK_DATA:
DEBUG("ID_MC_RTK_DATA");
break;
@@ -306,3 +375,113 @@
break;
}
}
+
+static void SendDfuFile(int fileLen, int sentLen, int blockLen, const uint8_t *data) {
+ uint8_t buffer[1024];
+ int x = 0;
+
+ DEBUG("SendDfuFile fileLen %d sentLen %d blockLen %d", fileLen, sentLen, blockLen);
+
+ buffer[x++] = BREAK_UINT32(fileLen, 3);
+ buffer[x++] = BREAK_UINT32(fileLen, 2);
+ buffer[x++] = BREAK_UINT32(fileLen, 1);
+ buffer[x++] = BREAK_UINT32(fileLen, 0);
+
+ buffer[x++] = BREAK_UINT32(sentLen, 3);
+ buffer[x++] = BREAK_UINT32(sentLen, 2);
+ buffer[x++] = BREAK_UINT32(sentLen, 1);
+ buffer[x++] = BREAK_UINT32(sentLen, 0);
+
+ buffer[x++] = HI_UINT16(blockLen);
+ buffer[x++] = LO_UINT16(blockLen);
+
+ memcpy(buffer + x, data, blockLen);
+ x += blockLen;
+
+ SendMcuCommand(ID_CM_DFU_UPLOAD, buffer, x);
+}
+
+static void GoNextDfuLater(union sigval sig) {
+ AppTimer_delete(GoNextDfuLater);
+
+ GoNextDfu();
+}
+
+static void GoNextDfu(void)
+{
+ int dfuFileSent = 0, currDfuBlockLength = 0;
+
+ if (dfuFileLength == 0 || dfuFile == NULL)
+ return;
+
+ int row = 0, col = 0;
+
+ dfuFileSent = dfuFileLength;
+
+ for (row = 0; row < sizeof(dfuFileBitmap); ++row) {
+ for (col = 0; col < 8; ++col) {
+ if ((dfuFileBitmap[row] & BV(col)) == 0) {
+ DEBUG("ROW = %d COL = %d", row, col);
+ dfuFileSent = row * DFU_FILE_BLOCK_SIZE * 8 + col * DFU_FILE_BLOCK_SIZE;
+ goto GET_FILE_START;
+ }
+ }
+ }
+
+GET_FILE_START:
+ currDfuBlockLength = (dfuFileLength - dfuFileSent > DFU_FILE_BLOCK_SIZE) ? DFU_FILE_BLOCK_SIZE : (dfuFileLength - dfuFileSent);
+
+ if (dfuFileSent >= dfuFileLength || currDfuBlockLength == 0) {
+ UploadDfuFileEnd();
+ return;
+ }
+
+ SendDfuFile(dfuFileLength, dfuFileSent, currDfuBlockLength, dfuFile + dfuFileSent);
+
+ if (dfuFileSent > 0 && dfuFileSent + currDfuBlockLength < dfuFileLength) {
+ dfuFileBitmap[row] |= BV(col);
+ }
+
+ if (dfuFileSent == 0 || dfuFileSent + currDfuBlockLength == dfuFileLength) {
+ dfuTryCnt++;
+ if (dfuTryCnt > DFU_MAX_TRY) {
+ UploadDfuFileEnd();
+ return;
+ }
+ DEBUG("GoNextDfuLater 3 sec...");
+ AppTimer_add(GoNextDfuLater, D_SEC(3));
+ } else {
+ AppTimer_add(GoNextDfuLater, 10);
+ }
+}
+
+void UploadDfuFileEnd(void)
+{
+ DEBUG("UploadDfuFileEnd");
+
+ if (dfuFile != NULL)
+ free(dfuFile);
+ dfuFileLength = 0;
+
+ AppTimer_delete(GoNextDfuLater);
+}
+
+void UploadDfuFile(const uint8_t *file, int length)
+{
+ DEBUG("UploadDfuFile %ld", length);
+
+ if (length > 0) {
+ if (dfuFile != NULL)
+ free(dfuFile);
+
+ dfuTryCnt = 0;
+
+ dfuFile = (uint8_t *) malloc(length);
+ dfuFileLength = length;
+ memcpy(dfuFile, file, length);
+ memset(dfuFileBitmap, 0, sizeof(dfuFileBitmap));
+ AppTimer_delete(GoNextDfuLater);
+
+ GoNextDfu();
+ }
+}
diff --git a/lib/src/main/cpp/mcu/mcu_if.h b/lib/src/main/cpp/mcu/mcu_if.h
index a324a3d..89c07c0 100644
--- a/lib/src/main/cpp/mcu/mcu_if.h
+++ b/lib/src/main/cpp/mcu/mcu_if.h
@@ -13,4 +13,7 @@
void ParseMcu(const uint8_t *data, int length);
void SendMcuCommand(uint16_t id, const uint8_t *data, int length);
+void UploadDfuFileEnd(void);
+void UploadDfuFile(const uint8_t *file, int length);
+
#endif //RTKDRIVERTEST_MCU_IF_H
diff --git a/lib/src/main/cpp/native-lib.cpp b/lib/src/main/cpp/native-lib.cpp
index 760751c..0601d0b 100644
--- a/lib/src/main/cpp/native-lib.cpp
+++ b/lib/src/main/cpp/native-lib.cpp
@@ -266,3 +266,16 @@
MA_MainProcMsgEntry(cmd, NULL);
}
}
+
+extern "C"
+JNIEXPORT void JNICALL
+Java_com_anyun_exam_lib_RemoteService_MainProcBinMsgEntry(JNIEnv *env, jobject thiz, jint cmd,
+ jbyteArray data, jint length) {
+ // TODO: implement MainProcBinMsgEntry()
+ jbyte *c_dat = env->GetByteArrayElements(data, NULL);
+ int len = env->GetArrayLength(data);
+
+ MA_MainProcBinMsgEntry(cmd, (uint8_t *)c_dat, len);
+
+ env->ReleaseByteArrayElements(data, c_dat, NULL);
+}
diff --git a/lib/src/main/cpp/test_items/park_edge.cpp b/lib/src/main/cpp/test_items/park_edge.cpp
index c24b64c..87993ff 100644
--- a/lib/src/main/cpp/test_items/park_edge.cpp
+++ b/lib/src/main/cpp/test_items/park_edge.cpp
@@ -98,15 +98,18 @@
if (moveStatus != prevMoveStatus) {
if (moveStatus == 0) {
- DEBUG("鍋滆溅浜�");
parkStatus = 0;
stopTimepoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
stopTimepoint = stopTimepoint * 1000 + 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("缁х画琛岄┒");
+ 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;
+
+ DEBUG("鍋滆溅鏃堕棿 %ld", tp - stopTimepoint);
if (moveStatus == storeMoveStatusBeforeStop) {
// 鍚屾柟鍚戝啀鍚姩锛岀户缁垽鏂槸鍚﹀仠杞﹁秴鏃�
@@ -127,6 +130,13 @@
AddExamFault(21, rtkTime);
DEBUG("绉诲簱涓嶅叆");
status = -1;
+ }
+
+ // 鍦ㄨ繖閲屾鏌ヨ浆鍚戠伅鐘舵��
+ if (true) {
+ // 涓嶅紑杞悜鐏紝鎵�10鍒�
+ AddExamFault(25, rtkTime);
+ DEBUG("鏈紑鍚浆鍚戠伅");
}
}
}
@@ -232,7 +242,6 @@
CleanPolygon(&parking);
free(car_body.point);
- DEBUG("妫�鏌ュ仠杞﹀埌浣嶇粨鏉�");
return succ;
}
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 6b8fa0e..266a8e3 100644
--- a/lib/src/main/cpp/test_items/stop_and_start.cpp
+++ b/lib/src/main/cpp/test_items/stop_and_start.cpp
@@ -10,6 +10,9 @@
#include "../driver_test.h"
#include "../jni_log.h"
#include "../common/apptimer.h"
+#include "../utils/xconvert.h"
+
+#define DEBUG(fmt, args...) LOGD("<stop_and_start> <%s>: " fmt, __func__, ##args)
using namespace std;
@@ -24,138 +27,166 @@
const double EDGE_DISTANCE_THRESHOLD_YELLOW = 0.3;
const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;
-const int CAR_START_TIMEOUT = 30;
+const uint32_t CAR_START_TIMEOUT = D_SEC(30);
+const uint32_t STOP_CAR_TIME = D_SEC(2);
+
const double EPSILON = 1e-3;
static bool SASTesting = false;
static double slideDistance;
-static bool startCarTimeout;
-static int currTarget;
static PointF stopPoint;
-static int startCarConfirm; // 璧锋鏃讹紝鎸佺画鍓嶈繘涓�灏忔鎵嶇畻
-static void StartCarTimeout(union sigval sig);
-static bool CrashRedLine(const Polygon *map, const car_model_cache_t *car);
-static double DistanceOfHead2Stopline(const Polygon *map, const car_model_cache_t *car);
-static double DistanceOfTire2Edge(const Polygon *map, const car_model_cache_t *car);
-static bool ExitTestArea(const Polygon *map, const car_model_cache_t *car);
+static int prevMoveDirect;
+static uint64_t stopTimepoint = 0;
+static bool stopCar = false;
+static uint32_t stopCarTime;
+static bool occurCrashRedLine = false;
+static bool slideLongDistance = false;
+static bool slideNormalDistance = false;
+static bool reportSlideFault = false;
+static bool reportStartTimeout = false;
-void StartSAS(void)
+static bool CrashRedLine(const Polygon *map, const car_model *car);
+static double DistanceOfHead2Stopline(const Polygon *map, const car_model *car);
+static double DistanceOfTire2Edge(const Polygon *map, const car_model *car);
+static bool ExitTestArea(const Polygon *map, const car_model *car);
+
+void StartSAS(int moveDirect, const struct RtkTime *rtkTime)
{
+ DEBUG("杩涘叆鍧¤捣椤圭洰");
SASTesting = true;
slideDistance = 0.0;
- startCarTimeout = false;
- currTarget = STOP_CAR;
+
+ 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;
+ }
+
+ occurCrashRedLine = false;
+ stopCar = false;
+ slideLongDistance = false;
+ slideNormalDistance = false;
+ reportSlideFault = false;
+ reportStartTimeout = false;
}
-void StopSAS(void)
-{
- SASTesting = false;
- AppTimer_delete(StartCarTimeout);
-}
-
-int TestSAS(vector<int>&err, const Polygon *map, const car_model_cache_t *car, double speed, int run_status)
+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 (!SASTesting)
- return -2;
-
- if (currTarget >= STOP_CAR) {
- if (CrashRedLine(map, car)) {
- // 杞﹁疆鍘嬬嚎
- err.push_back(13);
- status = -1;
+ if (CrashRedLine(map, car)) {
+ // 杞﹁疆鍘嬬嚎锛屼笉鍚堟牸
+ if (!occurCrashRedLine) {
+ AddExamFault(13, rtkTime);
+ DEBUG("杞﹁疆鍘嬬嚎");
}
+ occurCrashRedLine = true;
+ } else {
+ occurCrashRedLine = false;
}
- if (currTarget == STOP_CAR) {
- if (run_status == 0) {
+ if (ExitTestArea(map, car)) {
+ // 椹剁娴嬭瘯鍖�
+ status = 1;
+ }
+
+ 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;
+ }
+ 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;
+
+ if (tp - stopTimepoint >= STOP_CAR_TIME && !stopCar) {
+ // 杩欓噷鍒ゆ柇鍋滆溅鐘舵��
+ stopCar = true;
+ stopCarTime = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
+
+ stopPoint = car->carXY[car->body[0]];
+
double dis1 = DistanceOfHead2Stopline(map, car);
double dis2 = DistanceOfTire2Edge(map, car);
if (dis1 > STOP_DISTANCE_THRESHOLD_RED) {
// 璺濈鍋滄绾垮墠鍚庤秴鍑�50鍘樼背
- err.push_back(12);
+ AddExamFault(12, rtkTime);
+ DEBUG("璺濈鍋滄绾垮墠鍚庤秴鍑�50鍘樼背锛屼笉鍚堟牸");
status = -1;
} else if (fabs(dis1) > EPSILON) {
- // 鍓嶄繚闄╂病鏈変綅浜庡仠姝㈠甫鍐咃紝浣嗘病鏈夎秴鍑�50鍘樼背
- err.push_back(17);
+ // 鍓嶄繚闄╂病鏈変綅浜庡仠姝㈠甫鍐咃紝浣嗘病鏈夎秴鍑�50鍘樼背锛屾墸10鍒�
+ AddExamFault(17, rtkTime);
+ DEBUG("鍓嶄繚闄╂病鏈変綅浜庡仠姝㈠甫鍐咃紝浣嗘病鏈夎秴鍑�50鍘樼背");
}
if (dis2 > EDGE_DISTANCE_THRESHOLD_RED) {
- // 璺濈杈圭嚎瓒呭嚭50鍘樼背
- err.push_back(14);
+ // 璺濈杈圭嚎瓒呭嚭50鍘樼背,涓嶅悎鏍�
+ AddExamFault(14, rtkTime);
+ DEBUG("璺濈杈圭嚎瓒呭嚭50鍘樼背");
status = -1;
} else if (dis2 > EDGE_DISTANCE_THRESHOLD_YELLOW) {
- // 璺濈杈圭嚎瓒呭嚭30鍘樼背
- err.push_back(18);
+ // 璺濈杈圭嚎瓒呭嚭30鍘樼背锛屼笉鍚堟牸
+ AddExamFault(18, rtkTime);
+ DEBUG("璺濈杈圭嚎瓒呭嚭30鍘樼背");
}
// 妫�鏌ユ槸鍚︽媺浣忔墜鍒�
-
- AppTimer_delete(StartCarTimeout);
- AppTimer_add(StartCarTimeout, D_SEC(CAR_START_TIMEOUT));
- slideDistance = 0.0;
- stopPoint = car->points[0];
- startCarConfirm = 0;
- currTarget = START_CAR;
- } else if (run_status > 0) {
- if (ExitTestArea(map, car)) {
- // 杞﹁締鐩存帴椹剁娴嬭瘯鍖猴紝鐩存帴娣樻卑
- err.push_back(12);
- status = -1;
+ if (true) {
+ AddExamFault(19, rtkTime);
+ DEBUG("娌℃媺鎵嬪埞");
}
}
- } else if (currTarget == START_CAR) {
- if (startCarTimeout) {
- startCarTimeout = false;
- //璧锋鏃堕棿瓒呰繃30绉�
- err.push_back(15);
- status = -1;
- }
+ }
- if (run_status > 0) {
- startCarConfirm++;
- if (startCarConfirm == 2) {
- AppTimer_delete(StartCarTimeout); // 璧锋瀹屾垚
+ // 鍒ゆ柇璧锋鍚庢粦鐘舵��
+ if (stopCar) {
+ if (IntersectionOfLine(map->point[4], stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) == 1) {
+ // 鍙戠敓鍚庢粦
+ slideDistance = DistanceOf(stopPoint, car->carXY[car->axial[AXIAL_FRONT]]);
+
+ if (slideLongDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) {
+ slideNormalDistance = true;
}
- if (slideDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) {
+ if (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED && !slideLongDistance && !reportSlideFault) {
+ // 鍚庢粦瓒呰繃30鍘樼背, 涓嶅悎鏍�
+ AddExamFault(16, rtkTime);
+ DEBUG("鍚庢粦瓒呰繃30鍘樼背");
+ slideLongDistance = true;
+ reportSlideFault = true;
+ }
+ }
+
+ 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) {
+ // 璧锋鏃堕棿瓒呰繃30绉掞紝涓嶅悎鏍�
+ AddExamFault(15, rtkTime);
+ DEBUG("璧锋鏃堕棿瓒呰繃30绉�");
+ reportStartTimeout = true;
+ }
+ }
+
+ if (IntersectionOfLine(map->point[5], map->point[6], car->carXY[car->axial[AXIAL_REAR]]) == -1) {
+ // 杞﹀熬椹惰繃鍋滄鏉�
+ if (slideNormalDistance && !slideLongDistance && !reportSlideFault) {
+ reportSlideFault = true;
// 鍚庢粦瓒呰繃10鍘樼背锛屼絾娌¤秴杩�30鍘樼背
- err.push_back(20);
- }
- } else if (run_status < 0) {
- // 鍚庢粦浜�
- slideDistance = DistanceOf(stopPoint, car->points[0]);
- if (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED) {
- // 鍚庢粦瓒呰繃30鍘樼背
- err.push_back(16);
- status = -1;
+ AddExamFault(20, rtkTime);
+ DEBUG("鍚庢粦瓒呰繃10鍘樼背锛屼絾娌¤秴杩�30鍘樼背");
}
}
-
- if (ExitTestArea(map, car)) {
- // 娴嬭瘯瀹屾垚浜�
- status = 1;
- }
}
-
- if (status != 0) {
- StopSAS();
- }
-
- return status;
-}
-
-static void StartCarTimeout(union sigval sig) {
- AppTimer_delete(StartCarTimeout);
- startCarTimeout = true;
}
// 杞﹁疆鏄惁鍘嬭竟绾�
-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;
@@ -164,8 +195,8 @@
Line frontAxle, rearAxle;
- MakeLine(&frontAxle, &car->points[car->desc->front_left_tire[TIRE_OUTSIDE]], &car->points[car->desc->front_right_tire[TIRE_OUTSIDE]]);
- MakeLine(&rearAxle, &car->points[car->desc->rear_left_tire[TIRE_OUTSIDE]], &car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]]);
+ MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
+ MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
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]]);
@@ -179,48 +210,48 @@
return ret;
}
-static double DistanceOfHead2Stopline(const Polygon *map, const car_model_cache_t *car)
+static double DistanceOfHead2Stopline(const Polygon *map, const car_model *car)
{
double dis = 0.0;
- int rel1 = IntersectionOfLine(map->point[4], map->point[3], car->points[0]);
- int rel2 = IntersectionOfLine(map->point[5], map->point[6], car->points[0]);
+ 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]]);
if (rel1 == 1) {
Line line1;
MakeLine(&line1, &map->point[4], &map->point[3]);
- dis = DistanceOf(car->points[0], line1);
+ dis = DistanceOf(car->carXY[car->body[0]], line1);
} else if (rel2 == -1) {
Line line2;
MakeLine(&line2, &map->point[5], &map->point[6]);
- dis = DistanceOf(car->points[0], line2);
+ dis = DistanceOf(car->carXY[car->body[0]], line2);
}
return dis;
}
-static double DistanceOfTire2Edge(const Polygon *map, const car_model_cache_t *car)
+static double DistanceOfTire2Edge(const Polygon *map, const car_model *car)
{
Line edge;
MakeLine(&edge, &map->point[0], &map->point[8]);
- double l1 = DistanceOf(car->points[car->desc->front_right_tire[TIRE_OUTSIDE]], edge);
+ double l1 = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], edge);
- double l2 = DistanceOf(car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]], edge);
+ double l2 = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], edge);
return (l1+l2)/2.0;
}
// 鏁翠釜杞﹁締閮借椹剁璇ユ祴璇曞尯鍩�
-static bool ExitTestArea(const Polygon *map, const car_model_cache_t *car)
+static bool ExitTestArea(const Polygon *map, const car_model *car)
{
- for (int i = 0; i < car->point_num; ++i) {
- if (IntersectionOfLine(map->point[8], map->point[7], car->points[i]) != -1)
+ for (int i = 0; i < car->bodyNum; ++i) {
+ if (IntersectionOfLine(map->point[8], map->point[7], car->carXY[car->body[i]]) != -1)
return false;
}
return true;
diff --git a/lib/src/main/cpp/test_items/stop_and_start.h b/lib/src/main/cpp/test_items/stop_and_start.h
index dcb90f3..b3f69cb 100644
--- a/lib/src/main/cpp/test_items/stop_and_start.h
+++ b/lib/src/main/cpp/test_items/stop_and_start.h
@@ -9,8 +9,7 @@
using namespace std;
-void StartSAS(void);
-void StopSAS(void);
-int TestSAS(vector<int>&err, const Polygon *map, const car_model_cache_t *car, double speed, int run_status);
+void StartSAS(int moveDirect, const struct RtkTime *rtkTime);
+int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime);
#endif //RTKDRIVERTEST_STOP_AND_START_H
diff --git a/lib/src/main/java/com/anyun/exam/lib/RemoteService.java b/lib/src/main/java/com/anyun/exam/lib/RemoteService.java
index 5cd8834..d6ba6fd 100644
--- a/lib/src/main/java/com/anyun/exam/lib/RemoteService.java
+++ b/lib/src/main/java/com/anyun/exam/lib/RemoteService.java
@@ -16,6 +16,7 @@
import androidx.annotation.Nullable;
+import java.io.UnsupportedEncodingException;
import java.util.concurrent.atomic.AtomicBoolean;
/**
@@ -52,8 +53,18 @@
@Override
public void SendCmd(int cmd, String value) throws RemoteException {
- Log.d(TAG, "Process " + Process.myPid() + " Thread " + Thread.currentThread().getId() + " RecvMsgFromMainProc cmd = " + String.format(" %04X ", cmd) + " length " + value.length() + " value " + value.trim());
- MainProcMsgEntry(cmd, value.trim());
+ if (cmd == 0x8100) {
+ try {
+ byte [] file = value.getBytes("ISO-8859-1");
+ Log.d(TAG, byte2hex(file));
+ MainProcBinMsgEntry(cmd, file, file.length);
+ } catch (UnsupportedEncodingException e) {
+ Log.d(TAG, "UnsupportedEncodingException");
+ }
+ } else {
+ Log.d(TAG, "Process " + Process.myPid() + " Thread " + Thread.currentThread().getId() + " RecvMsgFromMainProc cmd = " + String.format(" %04X ", cmd) + " length " + value.length() + " value " + value.trim());
+ MainProcMsgEntry(cmd, value.trim());
+ }
}
};
@Nullable
@@ -108,6 +119,9 @@
if (!mIsServiceDestroyed.get()){
onMessageArrived(cmd, value);
}
+
+
+
Log.d(TAG, "SendMsgToMainProc cmd = " + String.format("%04X", cmd) + " json = " + value);
}
@@ -136,6 +150,23 @@
}
}
+ private String byte2hex(byte [] buffer){
+ StringBuilder h = new StringBuilder();
+
+ Log.d(TAG, "byte buffer length = " + buffer.length);
+
+ for(int i = 0; i < buffer.length; i++){
+ String temp = Integer.toHexString(buffer[i] & 0xFF);
+ if(temp.length() == 1){
+ temp = "0" + temp;
+ }
+ h.append(temp);
+ h.append(" ");
+ }
+
+ return h.toString();
+ }
+
// Used to load the 'native-lib' library on application startup.
static {
System.loadLibrary("native-lib");
@@ -143,4 +174,5 @@
public native void startNative();
public native void MainProcMsgEntry(int cmd, String value);
+ public native void MainProcBinMsgEntry(int cmd, byte []data, int length);
}
--
Gitblit v1.8.0