From 6c7f9e67fdcb882725d2790279fe9d99fb343c06 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期四, 12 三月 2020 17:39:37 +0800
Subject: [PATCH] 更新传感器
---
lib/src/main/cpp/driver_test.cpp | 433 +++++++++++++++++++++++-------------------------------
1 files changed, 184 insertions(+), 249 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index d795059..80ccb06 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -26,8 +26,9 @@
#include "test_items/stop_and_start.h"
#include "master/comm_if.h"
#include "utils/xconvert.h"
-#include "test_items/comm_test.h"
+#include "test_common/car_sensor.h"
#include "mcu/mcu_if.h"
+#include "test_common/car_sensor.h"
#define DEBUG(fmt, args...) LOGD("<driver_test> <%s>: " fmt, __func__, ##args)
@@ -65,8 +66,9 @@
TEST_TYPE_ROAD_TRUE_LIGHT
};
-static bool TestStart = false;
-static int TestType;
+static bool ExamStart = false;
+static int ExamType;
+static bool reportSeatbeltEject;
static int CarInArea = 0;
int errs = 0;
@@ -91,17 +93,9 @@
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;
-
-static int SensorValidLevel;
-
static struct dummy_light_exam *DummyLightContent;
static int DummyLightContentSize;
+static bool engineRuning = false;
#define MOV_AVG_SIZE 1
#define RTK_BUFFER_SIZE 100
@@ -113,11 +107,13 @@
static rtk_info *RtkBuffer = NULL;
static int RtkBufferNum = 0, RtkBufferIn = 0;
+static void DetectEnterOrExitMap(void);
+static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime);
static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b);
static void ReadDriverExamPrimerTimeout(union sigval sig);
static void UpdateCarBodyCoord(double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel);
static bool UpdateCarCoord(double &spd, int &mov, int &idx);
-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 GetMapId(int index, const struct map_list *mapList, int mapNum);
@@ -128,17 +124,13 @@
pthread_mutex_init(&rtk_clock_mutex, NULL);
memset(&rtkClock, 0, sizeof(rtkClock));
- TestStart = false;
+ ExamStart = false;
memset(&MapList, 0, sizeof(MapList));
MapNum = 0;
CarModel = NULL;
CarModelPrev = NULL;
- CommTestInit();
- SensorNum = 0;
- memset(SensorConfig, 0, sizeof(SensorConfig));
-
- SensorValidLevel = 0;
+ CarSensorInit();
DummyLightContentSize = 0;
DummyLightContent = NULL;
@@ -165,7 +157,7 @@
void ClearMap(void)
{
- if (TestStart) return;
+ if (ExamStart) return;
for (int i = 0; i < MapNum; ++i) {
if (MapList[i].map.point != NULL)
@@ -183,7 +175,7 @@
AppTimer_delete(ReadDriverExamPrimerTimeout);
- if (map == NULL || pointNum == 0 || TestStart)
+ if (map == NULL || pointNum == 0 || ExamStart)
return;
MapList[MapNum].id = id;
@@ -219,7 +211,7 @@
{
DEBUG("鍔犲叆杞﹁締淇℃伅 pointNum %d", pointNum);
- if (point == NULL || pointNum == 0 || TestStart) return;
+ if (point == NULL || pointNum == 0 || ExamStart) return;
if (CarModel != NULL) {
if (CarModel->body != NULL)
@@ -329,54 +321,11 @@
DEBUG("SetCarMeasurePoint Calc Over");
}
-void SetSensorCfg(int (*sensor)[3], int 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);
- }
- }
-}
-
-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 == 0 ? false : true;
- }
- }
-}
-
void SetDummyLightExam(int n, struct dummy_light_exam *cfg)
{
- DEBUG("鑾峰彇妯℃嫙璺�冪伅鍏夋祴璇曢」鐩� N = %d %d", n, TestStart);
+ DEBUG("鑾峰彇妯℃嫙璺�冪伅鍏夋祴璇曢」鐩� N = %d %d", n, ExamStart);
-// if (TestStart) return;
+// if (ExamStart) return;
if (DummyLightContent != NULL) {
free(DummyLightContent);
@@ -401,7 +350,7 @@
if (start == 0) {
DEBUG("缁撴潫鑰冭瘯");
// CurrExamMapIndex = -1;
-// TestStart = false;
+// ExamStart = false;
// CommTestStart(false);
// MA_SendExamStatus(0, 0);
return;
@@ -421,14 +370,14 @@
}
if (!err) {
- if (!TestStart) {
+ if (!ExamStart) {
DEBUG("鍚姩鑰冭瘯");
ExamFaultList.clear();
examFaultIndex = 0;
- TestStart = true;
- TestType = type;
- CommTestStart(true);
+ ExamStart = true;
+ ExamType = type;
+ reportSeatbeltEject = false;
if (type == TEST_TYPE_ROAD_DUMMY_LIGHT) {
StartDummyLightExam(DummyLightContent, DummyLightContentSize);
@@ -535,107 +484,179 @@
rtkTime.ss = RtkBuffer[index].ss;
rtkTime.mss = RtkBuffer[index].dss;
- if (TestStart) {
- if (CurrExamMapIndex < 0) {
- if (CurrEnterMapIndex < 0) {
- CurrEnterMapIndex = EnterMap(CarModel, MapList, MapNum);
- if (CurrEnterMapIndex >= 0) {
- DEBUG("鍙戦�佽繘鍏ュ満鍦版姤鍛� %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
- MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 1);
+ DetectEnterOrExitMap();
- CurrExamMapIndex = CurrEnterMapIndex;
- CurrExamStatus = EXAM_AREA_START;
- }
- } else {
- if (ExitMap(CarModel, CurrEnterMapIndex, MapList, MapNum)) {
- DEBUG("鍙戦�佺寮�鍦哄湴鎶ュ憡 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
- MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 0);
- CurrEnterMapIndex = -1;
- }
- }
- }
- //ExitMap(const car_model *car, int index, const struct map_list *mapList, int mapNum)
+ if (ExamStart) {
+ ExecuteExam(speed, move, azimuth, &rtkTime);
}
+ }
+}
- if (CurrExamMapIndex >= 0) {
- int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);
+static void DetectEnterOrExitMap(void)
+{
+ if (ExamType == TEST_TYPE_AREA) {
- if (CurrExamStatus == EXAM_AREA_START) {
- DEBUG("CurrExamMapIndex %d mtype %d", GetMapId(CurrExamMapIndex, MapList, MapNum), mtype);
+ }
+ if (CurrExamMapIndex < 0) {
+ if (CurrEnterMapIndex < 0) {
+ CurrEnterMapIndex = EnterMap(CarModel, MapList, MapNum);
+ if (CurrEnterMapIndex >= 0) {
+ DEBUG("鍙戦�佽繘鍏ュ満鍦版姤鍛� %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
+ MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 1);
- switch (mtype) {
- case MAP_TYPE_PARK_BUTTOM:
- DEBUG("杩涘叆鍊掕溅鍏ュ簱鍦哄湴");
- MA_SendDebugInfo("杩涘叆鍊掕溅鍏ュ簱鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
+ CurrExamMapIndex = CurrEnterMapIndex;
+ CurrExamStatus = EXAM_AREA_START;
+ }
+ } else {
+ if (ExitMap(CarModel, CurrEnterMapIndex, MapList, MapNum)) {
+ DEBUG("鍙戦�佺寮�鍦哄湴鎶ュ憡 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
+ MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 0);
+ CurrEnterMapIndex = -1;
+ }
+ }
+ }
+}
- StartParkBottom(move, &rtkTime);
- CurrExamStatus = EXAM_AREA_RUN;
- break;
- case MAP_TYPE_STOP_START:
- DEBUG("杩涘叆涓婂潯璧锋鍦哄湴");
- MA_SendDebugInfo("杩涘叆涓婂潯璧锋鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
- StartSAS(move, &rtkTime);
- CurrExamStatus = EXAM_AREA_RUN;
- break;
- case MAP_TYPE_PART_EDGE:
- DEBUG("杩涘叆渚ф柟浣嶅仠杞﹀満鍦�");
- MA_SendDebugInfo("杩涘叆渚ф柟浣嶅仠杞﹀満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
- StartParkEdge(move, &rtkTime);
+static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime)
+{
+ if (move != 0) {
+ if (ReadSensor(SENSOR_SEATBELT) == 0 && !reportSeatbeltEject) {
+ DEBUG("涓嶇郴瀹夊叏甯�");
+ reportSeatbeltEject = true;
+ AddExamFault(1, rtkTime);
+ }
+ }
- CurrExamStatus = EXAM_AREA_RUN;
- break;
- case MAP_TYPE_CURVE:
- DEBUG("杩涘叆鏇茬嚎琛岄┒鍦哄湴");
- MA_SendDebugInfo("杩涘叆鏇茬嚎琛岄┒鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
- StartDrivingCurve(move, &rtkTime);
+ if (CurrExamMapIndex >= 0) {
+ int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);
- CurrExamStatus = EXAM_AREA_RUN;
- break;
- case MAP_TYPE_TURN_90:
- DEBUG("杩涘叆鐩磋杞集鍦哄湴");
- MA_SendDebugInfo("杩涘叆鐩磋杞集鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
- StartTurnA90(move, azimuth, &rtkTime);
- CurrExamStatus = EXAM_AREA_RUN;
- break;
- default:break;
- }
- } else if (CurrExamStatus == EXAM_AREA_RUN) {
- int testing = 0;
- switch (mtype) {
- case MAP_TYPE_PARK_BUTTOM:
- testing = TestParkBottom(&MapList[CurrExamMapIndex].map,
- CarModel, CarModelPrev, speed, move, &rtkTime);
- break;
- case MAP_TYPE_STOP_START:
- testing = TestSAS(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, &rtkTime);
- break;
- case MAP_TYPE_PART_EDGE:
- testing = TestParkEdge(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, &rtkTime);
- break;
- case MAP_TYPE_CURVE:
- testing = TestDrivingCurve(&MapList[CurrExamMapIndex].map, &MapList[CurrExamMapIndex].map2, CarModel, CarModelPrev, speed, move, &rtkTime);
- break;
- case MAP_TYPE_TURN_90:
- testing = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, azimuth, speed, move, &rtkTime);
- break;
- default:
- break;
- }
+ if (CurrExamStatus == EXAM_AREA_START) {
+ DEBUG("CurrExamMapIndex %d mtype %d", GetMapId(CurrExamMapIndex, MapList, MapNum), mtype);
- if (testing > 0) {
+ switch (mtype) {
+ case MAP_TYPE_PARK_BUTTOM:
+ DEBUG("杩涘叆鍊掕溅鍏ュ簱鍦哄湴");
+ MA_SendDebugInfo("杩涘叆鍊掕溅鍏ュ簱鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
+
+ StartParkBottom(move, rtkTime);
CurrExamStatus = EXAM_AREA_RUN;
- } else {
- CurrExamStatus = EXAM_AREA_END;
- }
+ break;
+ case MAP_TYPE_STOP_START:
+ DEBUG("杩涘叆涓婂潯璧锋鍦哄湴");
+ MA_SendDebugInfo("杩涘叆涓婂潯璧锋鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
+ StartSAS(move, rtkTime);
+ CurrExamStatus = EXAM_AREA_RUN;
+ break;
+ case MAP_TYPE_PART_EDGE:
+ DEBUG("杩涘叆渚ф柟浣嶅仠杞﹀満鍦�");
+ MA_SendDebugInfo("杩涘叆渚ф柟浣嶅仠杞﹀満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
+ StartParkEdge(move, rtkTime);
+
+ CurrExamStatus = EXAM_AREA_RUN;
+ break;
+ case MAP_TYPE_CURVE:
+ DEBUG("杩涘叆鏇茬嚎琛岄┒鍦哄湴");
+ MA_SendDebugInfo("杩涘叆鏇茬嚎琛岄┒鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
+ StartDrivingCurve(move, rtkTime);
+
+ CurrExamStatus = EXAM_AREA_RUN;
+ break;
+ case MAP_TYPE_TURN_90:
+ DEBUG("杩涘叆鐩磋杞集鍦哄湴");
+ MA_SendDebugInfo("杩涘叆鐩磋杞集鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
+ StartTurnA90(move, azimuth, rtkTime);
+ CurrExamStatus = EXAM_AREA_RUN;
+ break;
+ default:break;
+ }
+ } else if (CurrExamStatus == EXAM_AREA_RUN) {
+ int testing = 0;
+ switch (mtype) {
+ case MAP_TYPE_PARK_BUTTOM:
+ testing = TestParkBottom(&MapList[CurrExamMapIndex].map,
+ CarModel, CarModelPrev, speed, move, rtkTime);
+ break;
+ case MAP_TYPE_STOP_START:
+ testing = TestSAS(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, rtkTime);
+ break;
+ case MAP_TYPE_PART_EDGE:
+ testing = TestParkEdge(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, rtkTime);
+ break;
+ case MAP_TYPE_CURVE:
+ testing = TestDrivingCurve(&MapList[CurrExamMapIndex].map, &MapList[CurrExamMapIndex].map2, CarModel, CarModelPrev, speed, move, rtkTime);
+ break;
+ case MAP_TYPE_TURN_90:
+ testing = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, azimuth, speed, move, rtkTime);
+ break;
+ default:
+ break;
}
- if (CurrExamStatus != EXAM_AREA_RUN) {
- // 鏌愰」缁撴潫
- //DEBUG("閫�鍑哄満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
- //MA_SendDebugInfo("閫�鍑哄満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
- CurrExamStatus = EXAM_AREA_NONE;
- CurrExamMapIndex = -1;
+ if (testing > 0) {
+ CurrExamStatus = EXAM_AREA_RUN;
+ } else {
+ CurrExamStatus = EXAM_AREA_END;
}
+ }
+ if (CurrExamStatus != EXAM_AREA_RUN) {
+ // 鏌愰」缁撴潫
+ //DEBUG("閫�鍑哄満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
+ //MA_SendDebugInfo("閫�鍑哄満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
+ CurrExamStatus = EXAM_AREA_NONE;
+ CurrExamMapIndex = -1;
+ }
+ }
+}
+
+static void EngineStartHold(union sigval sig) {
+ AppTimer_delete(EngineStartHold);
+
+ // 涓嶅強鏃舵澗寮�鍚姩寮�鍏筹紝鎵�10鍒�
+ if (ReadSensor(SENSOR_ENGINE_START) == 1 && ExamStart) {
+ if (ExamType == TEST_TYPE_AREA)
+ AddExamFault(4, &rtkClock);
+ }
+}
+
+void SensorChanged(int *sensorId, int *sensorValue, int num)
+{
+ const int ENGINE_MIN_ROTATE = 200;
+
+ for (int i = 0; i < num; ++i) {
+ switch (sensorId[i]) {
+ case SENSOR_SEATBELT:
+ break;
+ case SENSOR_ENGINE_START:
+ AppTimer_delete(EngineStartHold);
+
+ if (sensorValue[i] == 1) {
+ if (ReadSensor(SENSOR_SHIFT_N) == 0) {
+ // 涓嶆槸绌烘尅鐐圭伀锛屼笉鍚堟牸
+ if (ExamType == TEST_TYPE_AREA)
+ AddExamFault(3, &rtkClock);
+ else
+ AddExamFault(4, &rtkClock);
+ }
+ AppTimer_add(EngineStartHold, D_SEC(2));
+ }
+ break;
+ case SENSOR_ENGINE_RPM:
+ if (sensorValue[i] < ENGINE_MIN_ROTATE) {
+ if (engineRuning) {
+ engineRuning = false;
+ if (ExamType == TEST_TYPE_AREA) {
+ // 鐔勭伀1娆★紝鎵�10鍒�
+ AddExamFault(5, &rtkClock);
+ } else {
+ AddExamFault(20, &rtkClock);
+ }
+ }
+ } else {
+ engineRuning = true;
+ }
+ break;
+ default:
+ break;
}
}
}
@@ -770,87 +791,6 @@
// DEBUG("tmDiff = %ld speed = %f m/Sec move = %d", tmDiff, speed, move);
return true;
-/* if (!TestStart) return;
-
- if (CarInArea == 0) {
-// if (FrontTireEnterArea(&py, &theParkEdgeMap)) {
-// CarInArea = TEST_PARK_BOTTOM;
-// TestItem = TEST_PARK_BOTTOM;
-// StartParkBottom();
-// }
-
- CarInArea = TEST_SLOPE;
- TestItem = TEST_SLOPE;
- StartSAS();
- }
-
- switch (TestItem) {
- case TEST_NONE: {
- break;
- }
- case TEST_PARK_BOTTOM: {
- errs = TestParkBottom(ErrorList, &theParkEdgeMap, GetCarModelCache(0), currSpeed, currDirect);
-
- if (errs != 0) {
- StopParkBottom();
- TestItem = TEST_NONE;
- }
- break;
- }
- case TEST_PARK_EDGE: {
- errs = TestParkEdge(ErrorList, &theParkEdgeMap, GetCarModelCache(0), currSpeed, currDirect);
-
- if (errs != 0) {
- StopParkEdge();
- TestItem = TEST_NONE;
- }
- break;
- }
-
- case TEST_TUNE_90: {
- errs = TestTurnA90(ErrorList, &theTurn90Map, GetCarModelCache(0), currSpeed, currDirect, currAzimuth);
-
- if (errs != 0) {
- StopTurnA90();
- TestItem = TEST_NONE;
- }
-
- break;
- }
-
- case TEST_SLOPE: {
- errs = TestSAS(ErrorList, &theSSMap, GetCarModelCache(0), currSpeed, currDirect);
-
- if (errs != 0) {
- StopSAS();
- TestItem = TEST_NONE;
- }
-
- break;
- }
- default:
- break;
- }
-
- if (ErrorList.size() > 0) {
- vector<int>::iterator it = ErrorList.end();
- it--;
- error_list_t list = GetErrorList(*it);
-
- int scr = 0;
-
- for (vector<int>::iterator it1 = ErrorList.begin(); it1 != ErrorList.end(); ++it1) {
- error_list_t list = GetErrorList(*it1);
-
- scr += list.dec_score;
- }
-
- char buff[256];
-
- sprintf(buff, "%s, 鎬昏鎵ｅ垎 %d", list.text_desc, scr);
-
- TextOsd(1, buff);
- }*/
}
static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum)
@@ -1033,6 +973,9 @@
{
struct ExamFault fault;
+ if (!ExamStart)
+ return;
+
fault.sn = examFaultIndex++;
sprintf(fault.utc, "%04d%02d%02d%02d%02d%02d.%02d", 2000 + rtkTime->YY,
rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
@@ -1088,14 +1031,6 @@
projectDistance * cos(toRadians(azimuth)) * cos(toRadians(projectAngle)) +
main_ant.Y;
}
-}
-
-static bool FrontTireEnterArea(const Polygon *car, const Polygon *map)
-{
- if (IntersectionOf(car->point[0], map) == GM_Containment) {
- return true;
- }
- return false;
}
void SystemShutdown(int event, int timeout)
--
Gitblit v1.8.0