From 8a09b209f1c546a2fa15329e8f69b4a4f89557c9 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期五, 21 八月 2020 18:59:19 +0800
Subject: [PATCH] 自适应触发和个子项修改。
---
lib/src/main/cpp/test_items2/stop_car.cpp | 251 ++++++++++++++++----------------------------------
1 files changed, 81 insertions(+), 170 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/stop_car.cpp b/lib/src/main/cpp/test_items2/stop_car.cpp
index 6526861..2ec36c9 100644
--- a/lib/src/main/cpp/test_items2/stop_car.cpp
+++ b/lib/src/main/cpp/test_items2/stop_car.cpp
@@ -11,221 +11,132 @@
#include "../common/apptimer.h"
#include "../test_common/car_sensor.h"
#include "../defs.h"
+#include "../test_common/odo_graph.h"
#define DEBUG(fmt, args...) LOGD("<stop_car> <%s>: " fmt, __func__, ##args)
-static bool inactiveBreakHandbreakTogether, notCloseEngine, inactiveHandBreakAfterOpenDoor, occurOpenDoor, doorNotClose, checkRoadDistance;
-static int examTtsSeq = 0;
-static int ttsPlayEnd;
-static double moveDistance;
-static double prevSpeed;
-static struct drive_timer prevPointTime;
-static int prevMoveDirect;
-static uint32_t stopTimepoint = 0;
-static uint32_t openDoorTimepoint;
+static bool BreakHandbreakReleaseSametime, OpenDoor;
-const int ENGINE_MIN_ROTATE = 200;
+static const int ENGINE_MIN_ROTATE = 200;
+static const double MAX_STOP_DISTANCE = 150;
static const uint32_t STOP_CAR_TIME = D_SEC(2);
static const uint32_t OPEN_DOOR_TIMEOUT = D_SEC(15);
static const double DISTANCE_TO_ROAD_EDGE_1 = 0.5;
static const double DISTANCE_TO_ROAD_EDGE_2 = 0.3;
-static void PlayTTSTimeout(union sigval sig);
+static double beginOdo;
+static int setup;
-void StartStopCarExam(std::string tts) {
+static void TtsBack(int seq)
+{
+ setup = 1;
+}
+
+void StartStopCarExam(void) {
DEBUG("闈犺竟鍋滆溅");
- ttsPlayEnd = 0;
- moveDistance = 0;
- prevMoveDirect = 0;
- inactiveBreakHandbreakTogether = false;
- notCloseEngine = false;
- inactiveHandBreakAfterOpenDoor = false;
- occurOpenDoor = false;
- doorNotClose = false;
- checkRoadDistance = false;
-
- if (!tts.empty()) {
- examTtsSeq = PlayTTS(tts.c_str(), NULL);
- } else {
- examTtsSeq = PlayTTS("璇烽潬杈瑰仠杞�", NULL);
- }
-
- AppTimer_delete(PlayTTSTimeout);
- AppTimer_add(PlayTTSTimeout, D_SEC(8));
+ BreakHandbreakReleaseSametime = false;
+ setup = 0;
+ OpenDoor = false;
+ PlayTTS("璇烽潬杈瑰仠杞�", TtsBack);
}
-void StopCarTTSDone(int id)
-{
- // 绛夎闊虫挱鎶ュ畬姣曞悗璁℃椂
- if (id == examTtsSeq) {
- DEBUG("StopCarTTSDone %d", id);
- ttsPlayEnd = 1;
- }
-}
+bool TestStopCar(road_exam_map &RoadMap, int roadIndex, const car_model *car, int moveDirect, const struct RtkTime *rtkTime) {
+ struct RtkTime time;
-void TerminateStopCarExam(void)
-{
- AppTimer_delete(PlayTTSTimeout);
-}
+ if (setup == 0)
+ return true;
-int ExecuteStopCarExam(road_t &road, const car_model *car,
- LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) {
- if (ttsPlayEnd == 1) {
- ttsPlayEnd = 2;
- prevSpeed = speed;
- Rtk2DriveTimer(prevPointTime, rtkTime);
- }
-
- if (ttsPlayEnd != 2)
- return 1;
-
- uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
- rtkTime->mss * 10,
- prevPointTime.hour, prevPointTime.min,
- prevPointTime.sec, prevPointTime.msec * 10);
- if (diff >= D_SEC(1)) {
- moveDistance += (double)diff * (speed + prevSpeed) / 2.0 / 1000.0;
- prevSpeed = speed;
- Rtk2DriveTimer(prevPointTime, rtkTime);
- DEBUG("鍋滆溅宸茶椹惰窛绂� %f", moveDistance);
- }
-
- if (moveDistance > 150) {
- // 150绫冲唴鏈仠杞︼紝涓嶅悎鏍�
+ if (roadIndex < 0) {
DEBUG("鍋滆溅璺濈瓒呮爣锛岄潬杈瑰仠杞︾粨鏉�");
AddExamFault(33, rtkTime);
- return -1;
+ return false;
}
- if (moveDirect != prevMoveDirect) {
+ if (setup == 1) {
+ beginOdo = ReadOdo();
+ setup = 2;
+ } else if (setup == 2) {
if (moveDirect == 0) {
- stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- DEBUG("####### 闈犺竟鍋滆溅锛屽仠杞︿簡 %d %d %d %d %d %d %d ######", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
+ time = *rtkTime;
+ setup = 3;
}
- prevMoveDirect = moveDirect;
- } else if (moveDirect == 0) {
- uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
-
- if (tp - stopTimepoint >= STOP_CAR_TIME && !checkRoadDistance) {
+ } else if (setup == 3) {
+ if (moveDirect != 0) {
+ setup = 2;
+ } else if (TimeGetDiff(rtkTime, &time) >= D_SEC(2)) {
// 鍋滆溅瓒�2绉掞紝寮�濮嬪垽鏂�
DEBUG("妫�娴嬪拰璺竟鐨勮窛绂�");
- PointF pc, vp;
- bool get_vp = false;
- double dis2roadEdge = 0;
- checkRoadDistance = true;
- // 鍓嶅悗杞殑涓偣
- pc.X = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].X + car->carXY[car->right_rear_tire[TIRE_OUTSIDE]].X) / 2;
- pc.Y = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].Y + car->carXY[car->right_rear_tire[TIRE_OUTSIDE]].Y) / 2;
+ PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ]);
+ PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, car->carXY[ car->right_rear_tire[TIRE_OUTSIDE] ]);
- // 妫�娴嬮亾璺竟缂樼嚎
- for (int i = 0; i < road.rightEdge.size(); ++i) {
- PointF p1, p2;
- Line edge;
-
- p1 = road.rightEdge[i].points[0];
- for (int j = 1; j < road.rightEdge[i].points.size(); ++j) {
- p2 = road.rightEdge[i].points[j];
- MakeLine(&edge, &p1, &p2);
-
-
- if (VerticalPointOnLine(pc, edge, vp)) {
- get_vp = true;
- goto FIND_VP_END;
- }
-
- p1 = p2;
- }
- }
-
-FIND_VP_END:
- if (get_vp) {
- dis2roadEdge = DistanceOf(pc, vp);
- } else {
- // 娌℃湁鎵惧埌鍖归厤绾跨锛屾寜鏈�灏忚窛绂婚《鐐硅绠�
- dis2roadEdge = 100;
- for (int i = 0; i < road.rightEdge.size(); ++i) {
- for (int j = 0; j < road.rightEdge[i].points.size(); ++j) {
- double dis;
- if (dis2roadEdge > (dis = DistanceOf(pc, road.rightEdge[i].points[j]))) {
- dis2roadEdge = dis;
- }
- }
- }
- }
-
- DEBUG("鍋滆溅璺濊矾杈硅窛绂� %f", dis2roadEdge);
-
- if (dis2roadEdge > DISTANCE_TO_ROAD_EDGE_1) {
+ if (DistanceOf(p1, car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ]) > DISTANCE_TO_ROAD_EDGE_1 ||
+ DistanceOf(p2, car->carXY[ car->right_rear_tire[TIRE_OUTSIDE] ]) > DISTANCE_TO_ROAD_EDGE_1) {
DEBUG("鍋滆溅瓒呭嚭璺竟0.5绫�");
// 鍋滆溅璺濈瓒呰繃50鍘樼背锛屼笉鍚堟牸
AddExamFault(36, rtkTime);
- } else if (dis2roadEdge > DISTANCE_TO_ROAD_EDGE_2) {
+ return false;
+ } else if (DistanceOf(p1, car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ]) > DISTANCE_TO_ROAD_EDGE_2 ||
+ DistanceOf(p2, car->carXY[ car->right_rear_tire[TIRE_OUTSIDE] ]) > DISTANCE_TO_ROAD_EDGE_2) {
DEBUG("鍋滆溅瓒呭嚭璺竟0.3绫�");
// 鍋滆溅璺濈瓒呰繃30鍘樼背锛屾墸10鍒�
AddExamFault(37, rtkTime);
+ return false;
}
+
+ setup = 4;
}
- }
+ } else if (setup == 4) {
+ car_sensor_value_t brk = ReadCarSensorValue(BREAK);
+ car_sensor_value_t hbrk = ReadCarSensorValue(HAND_BREAK);
+ car_sensor_value_t door = ReadCarSensorValue(DOOR);
+ car_sensor_value_t rpm = ReadCarSensorValue(ENGINE_RPM);
- if (!inactiveBreakHandbreakTogether && ReadCarStatus(BREAK) == BREAK_INACTIVE && ReadCarStatus(HAND_BREAK) == BREAK_INACTIVE) {
- // 鎷夋墜鍒瑰墠锛屾澗鑴氬埞锛屾墸10鍒�
- DEBUG("鎷夋墜鍒瑰墠锛屾澗鑴氬埞");
- AddExamFault(39, rtkTime);
- inactiveBreakHandbreakTogether = true;
- }
+ if (!BreakHandbreakReleaseSametime && brk.value == BREAK_INACTIVE && hbrk.value == BREAK_INACTIVE) {
+ // 鎷夋墜鍒瑰墠锛屾澗鑴氬埞锛屾墸10鍒�
+ DEBUG("鎷夋墜鍒瑰墠锛屾澗鑴氬埞");
+ AddExamFault(39, rtkTime);
+ BreakHandbreakReleaseSametime = true;
+ }
- if (!notCloseEngine && ReadCarStatus(ENGINE_RPM) < ENGINE_MIN_ROTATE && ReadCarStatus(DOOR) == DOOR_OPEN) {
- // 涓嬭溅鍓嶏紝涓嶇唲鐏紝鎵�5鍒�
- DEBUG("涓嬭溅鍓嶏紝涓嶇唲鐏�");
- AddExamFault(40, rtkTime);
+ if (door.value == DOOR_OPEN) {
+ if (!OpenDoor) {
+ if (rpm.value < ENGINE_MIN_ROTATE) {
+ // 涓嬭溅鍓嶏紝涓嶇唲鐏紝鎵�5鍒�
+ DEBUG("涓嬭溅鍓嶏紝涓嶇唲鐏�");
+ AddExamFault(40, rtkTime);
+ }
+ if (hbrk.value == BREAK_INACTIVE) {
+ // 寮�闂ㄥ墠锛屾湭鎷夋墜鍒癸紝 鎵�10鍒�
+ DEBUG("寮�闂ㄥ墠锛屾湭鎷夋墜鍒�");
+ AddExamFault(38, rtkTime);
+ }
+ time = *rtkTime;
+ OpenDoor = true;
+ }
- notCloseEngine = true;
- }
-
- if (!inactiveHandBreakAfterOpenDoor && ReadCarStatus(DOOR) == DOOR_OPEN && ReadCarStatus(HAND_BREAK) == BREAK_INACTIVE) {
- // 寮�闂ㄥ墠锛屾湭鎷夋墜鍒癸紝 鎵�10鍒�
- DEBUG("寮�闂ㄥ墠锛屾湭鎷夋墜鍒�");
- AddExamFault(38, rtkTime);
-
- inactiveHandBreakAfterOpenDoor = true;
- }
-
- if (ReadCarStatus(DOOR) == DOOR_OPEN) {
- if (!occurOpenDoor) {
- occurOpenDoor = true;
- openDoorTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- } else {
- uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
-
- if (!doorNotClose && tp - openDoorTimepoint >= OPEN_DOOR_TIMEOUT) {
+ if (TimeGetDiff(rtkTime, &time) > OPEN_DOOR_TIMEOUT) {
// 寮�闂ㄦ椂闂磋秴杩�15绉掞紝涓嶅悎鏍�
DEBUG("寮�闂ㄦ椂闂磋秴杩�15绉�");
AddExamFault(35, rtkTime);
- doorNotClose = true;
+ return false;
}
}
- } else if (occurOpenDoor) {
- DEBUG("寮�杞﹂棬锛岄潬杈瑰仠杞︾粨鏉�");
- return -1;
- }
- if (occurOpenDoor) {
- uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
-
- if (tp - openDoorTimepoint >= OPEN_DOOR_TIMEOUT + D_SEC(5)) {
- DEBUG("寮�闂ㄦ椂闂村ぇ浜�20绉掞紝闈犺竟鍋滆溅缁撴潫");
- return -1;
+ if (OpenDoor && door.value == DOOR_CLOSE) {
+ DEBUG("瀹屾垚鍋滆溅");
+ PlayTTS("闈犺竟鍋滆溅缁撴潫", NULL);
+ return false;
}
}
- return 1;
-}
+ if (ReadOdo() - beginOdo > MAX_STOP_DISTANCE) {
+ // 150绫冲唴鏈仠杞︼紝涓嶅悎鏍�
+ DEBUG("鍋滆溅璺濈瓒呮爣锛岄潬杈瑰仠杞︾粨鏉�");
+ AddExamFault(33, rtkTime);
+ return false;
+ }
-static void PlayTTSTimeout(union sigval sig)
-{
- AppTimer_delete(PlayTTSTimeout);
-
- ttsPlayEnd = 1;
+ return true;
}
--
Gitblit v1.8.0