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, 鎬昏鎵e垎 %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