From cea2a94fc97e79897cdfd217be8250c075974a1a Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期日, 07 二月 2021 17:55:42 +0800
Subject: [PATCH] 坡起限制条件和手刹起步全阶段生效.
---
lib/src/main/cpp/driver_test.cpp | 36 ++++++++++++
lib/src/main/cpp/test_items/stop_and_start.cpp | 17 +++++
lib/src/main/cpp/test_items2/car_start.cpp | 5 -
lib/src/main/cpp/test_items/area_exam.cpp | 6 +
lib/src/main/cpp/rtk_module/rtk.cpp | 1
lib/src/main/cpp/test_common/car_sensor.cpp | 90 +++++++----------------------
lib/src/main/cpp/test_items/stop_and_start.h | 2
lib/src/main/cpp/rtk_platform/platform.cpp | 4
8 files changed, 84 insertions(+), 77 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 9e0f000..00e93e5 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -38,6 +38,7 @@
#include "test_items2/road_exam.h"
#include "test_items/area_exam.h"
#include "test_items2/prepare.h"
+#include "test_common/odo_graph.h"
#define DEBUG(fmt, args...) LOGD("<driver_test> <%s>: " fmt, __func__, ##args)
@@ -891,14 +892,49 @@
static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime)
{
+ static bool rec = false;
+ static bool handBreakActive = false, handBreakActive2 = false;
+ static double startCarMoveDistance;
+ static int prevMove = 0;
+
if (move != 0) {
if (ReadCarStatus(SEATBELT) == EJECT_SEATBELT && !reportSeatbeltEject) {
DEBUG("涓嶇郴瀹夊叏甯�");
reportSeatbeltEject = true;
AddExamFault(ExamType == TEST_TYPE_AREA? 10101: 30101, rtkTime);
}
+ if (rec) {
+ if (!handBreakActive2 && ReadOdo() - startCarMoveDistance >= examParam.start_car_limit_distance) {
+ handBreakActive2 = true;
+
+ if (ExamType == TEST_TYPE_ROAD_DUMMY_LIGHT || ExamType == TEST_TYPE_ROAD_TRUE_LIGHT) {
+ if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
+ DEBUG("Handbreak active move over 10m");
+ // 鎵嬪埞鎷夎捣鐘舵�佷笅锛岃椹朵簡10绫充互涓婏紝涓嶅悎鏍�
+ AddExamFault(40205, rtkTime);
+ } else if (handBreakActive) {
+ // 鎵嬪埞鎷夎捣鐘舵�佷笅锛岃椹朵簡1绫充互涓婏紝鎵�10鍒�
+ DEBUG("Handbreak active move over 1M");
+ AddExamFault(40206, rtkTime);
+ }
+ }
+ } else if (!handBreakActive && ReadOdo() - startCarMoveDistance >= examParam.open_door_drive_allow_distance && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
+ handBreakActive = true;
+
+ if (ExamType == TEST_TYPE_AREA) {
+ DEBUG("Handbreak active move over 1M");
+ AddExamFault(10107, rtkTime);
+ }
+ }
+ }
+ } else if (!rec || prevMove != 0) { // 璁板綍鍋滆溅鐐�
+ rec = true;
+ handBreakActive = handBreakActive2 = false;
+ startCarMoveDistance = ReadOdo();
}
+ prevMove = move;
+
if (ExamType != TEST_TYPE_AREA) {
if (exam_dummy_light == 4 || ExamType == TEST_TYPE_ROAD_TRUE_LIGHT || ExamType == TEST_TYPE_ROAD_CALIBRATE) {
TestRoadGeneral(RoadMap, CarModel, CarModelList, speed, move, rtkTime);
diff --git a/lib/src/main/cpp/rtk_module/rtk.cpp b/lib/src/main/cpp/rtk_module/rtk.cpp
index 6c03abe..67a2d35 100644
--- a/lib/src/main/cpp/rtk_module/rtk.cpp
+++ b/lib/src/main/cpp/rtk_module/rtk.cpp
@@ -225,7 +225,6 @@
#if 1
if (VirtualIsConnected()) { //PC妯℃嫙鐢ㄦ椂
static bool first = false;
- DEBUG("XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX");
if (!first) {
first = true;
diff --git a/lib/src/main/cpp/rtk_platform/platform.cpp b/lib/src/main/cpp/rtk_platform/platform.cpp
index c5238d9..3068568 100644
--- a/lib/src/main/cpp/rtk_platform/platform.cpp
+++ b/lib/src/main/cpp/rtk_platform/platform.cpp
@@ -469,7 +469,7 @@
break;
}
case RTK_STATUS_EVT: {
- DEBUG("妯″潡淇℃伅 %02X %02X %02X %02X %02X %02X", data[0], data[1], data[2], data[3], data[4], data[5]);
+// DEBUG("妯″潡淇℃伅 %02X %02X %02X %02X %02X %02X", data[0], data[1], data[2], data[3], data[4], data[5]);
if (length == 33)
MA_SendRtkStatus((char *)data, data[32]);
break;
@@ -808,7 +808,7 @@
void ReceivedRtk(const uint8_t *data, int length)
{
- DEBUG("ReceivedRtk length %d", length);
+// DEBUG("ReceivedRtk length %d", length);
AppTimer_delete(RequestRtkNoResp);
rtcmLength = length;
// 姹囨姤缁欏崟鐗囨満
diff --git a/lib/src/main/cpp/test_common/car_sensor.cpp b/lib/src/main/cpp/test_common/car_sensor.cpp
index 173a1e5..3c462b4 100644
--- a/lib/src/main/cpp/test_common/car_sensor.cpp
+++ b/lib/src/main/cpp/test_common/car_sensor.cpp
@@ -15,7 +15,7 @@
#define MAX_SENSOR_NUM 32
static uint16_t gpioStore;
-static int left_turn_signal, right_turn_signal;
+static bool left_turn_signal, right_turn_signal;
enum {
SENSOR_SEATBELT,
@@ -62,8 +62,7 @@
inline static int BX(int value);
static void WriteCarStatus(uint16_t id, int value);
-static void LRLightTimeout(union sigval sig);
-static void ChangeLRLight(int light);
+
static void ConfirmTurnSigalLater(union sigval sig);
static void flashBeamLightClose(union sigval sig);
@@ -81,7 +80,7 @@
memset(&Sensor, 0, sizeof(Sensor));
- left_turn_signal = right_turn_signal = 0;
+ left_turn_signal = right_turn_signal = false;
pthread_mutex_init(&sonser_mutex, NULL);
pthread_mutex_init(&status_rw_mutex, NULL);
@@ -245,42 +244,21 @@
}
}
-static void LRLightTimeout(union sigval sig)
-{
- AppTimer_delete(LRLightTimeout);
- if (!left_turn_signal && !right_turn_signal) {
- WriteCarStatus(TURN_SIGNAL_LAMP, OFF_LIGHT);
- } else if (left_turn_signal && right_turn_signal) {
- WriteCarStatus(TURN_SIGNAL_LAMP, HAZARD_LIGHTS);
- } else if (left_turn_signal) {
- WriteCarStatus(TURN_SIGNAL_LAMP, LEFT_TURN_LIGHT);
- } else {
- WriteCarStatus(TURN_SIGNAL_LAMP, RIGHT_TURN_LIGHT);
- }
-}
-
-static void ChangeLRLight(int light)
-{
- DEBUG("ChangeLRLight %d %d", left_turn_signal, right_turn_signal);
- if (!left_turn_signal && !right_turn_signal) {
- WriteCarStatus(TURN_SIGNAL_LAMP, OFF_LIGHT);
- } else if (left_turn_signal && right_turn_signal) {
- WriteCarStatus(TURN_SIGNAL_LAMP, HAZARD_LIGHTS);
- } else if (left_turn_signal) {
- WriteCarStatus(TURN_SIGNAL_LAMP, LEFT_TURN_LIGHT);
- } else {
- WriteCarStatus(TURN_SIGNAL_LAMP, RIGHT_TURN_LIGHT);
- }
-
- AppTimer_delete(LRLightTimeout);
- AppTimer_add(LRLightTimeout, 1500);
-}
-
static void ConfirmTurnSigalLater(union sigval sig)
{
AppTimer_delete(ConfirmTurnSigalLater);
- ChangeLRLight(sig.sival_int);
+ DEBUG("纭杞悜鐏� 宸� %d 鍙� %d", left_turn_signal, right_turn_signal);
+
+ if (!left_turn_signal && !right_turn_signal) {
+ WriteCarStatus(TURN_SIGNAL_LAMP, OFF_LIGHT);
+ } else if (left_turn_signal && right_turn_signal) {
+ WriteCarStatus(TURN_SIGNAL_LAMP, HAZARD_LIGHTS);
+ } else if (left_turn_signal) {
+ WriteCarStatus(TURN_SIGNAL_LAMP, LEFT_TURN_LIGHT);
+ } else {
+ WriteCarStatus(TURN_SIGNAL_LAMP, RIGHT_TURN_LIGHT);
+ }
}
static void flashBeamLightClose(union sigval sig)
@@ -293,43 +271,19 @@
DEBUG("鐘舵�佹敼鍙� %d = %d", id, value);
switch (id) {
case SENSOR_LEFT_TURN_SIGNAL: {
- left_turn_signal = value;
+ left_turn_signal = (bool) (value > 0);
- if (left_turn_signal) {
- AppTimer_delete(ConfirmTurnSigalLater);
- if (right_turn_signal) {
- // 鍒ゅ畾涓哄弻闂�
- AppTimer_add(ConfirmTurnSigalLater, 500, HAZARD_LIGHTS);
- } else {
- AppTimer_add(ConfirmTurnSigalLater, 200, LEFT_TURN_LIGHT);
- }
- } else {
- AppTimer_delete(ConfirmTurnSigalLater);
- if (right_turn_signal)
- AppTimer_add(ConfirmTurnSigalLater, 1500, RIGHT_TURN_LIGHT);
- else
- AppTimer_add(ConfirmTurnSigalLater, 1500, OFF_LIGHT);
- }
+ AppTimer_delete(ConfirmTurnSigalLater);
+
+ AppTimer_add(ConfirmTurnSigalLater, left_turn_signal? 100 : 1100);
break;
}
case SENSOR_RIGHT_TURN_SIGNAL: {
- right_turn_signal = value;
+ right_turn_signal = (bool) (value > 0);
- if (right_turn_signal) {
- AppTimer_delete(ConfirmTurnSigalLater);
- if (left_turn_signal) {
- // 鍒ゅ畾涓哄弻闂�
- AppTimer_add(ConfirmTurnSigalLater, 500, HAZARD_LIGHTS);
- } else {
- AppTimer_add(ConfirmTurnSigalLater, 200, RIGHT_TURN_LIGHT);
- }
- } else {
- AppTimer_delete(ConfirmTurnSigalLater);
- if (left_turn_signal)
- AppTimer_add(ConfirmTurnSigalLater, 1500, LEFT_TURN_LIGHT);
- else
- AppTimer_add(ConfirmTurnSigalLater, 1500, OFF_LIGHT);
- }
+ AppTimer_delete(ConfirmTurnSigalLater);
+
+ AppTimer_add(ConfirmTurnSigalLater, right_turn_signal? 100 : 1100);
break;
}
case SENSOR_FOG_LIGHT: {
diff --git a/lib/src/main/cpp/test_items/area_exam.cpp b/lib/src/main/cpp/test_items/area_exam.cpp
index 556d071..9eaded6 100644
--- a/lib/src/main/cpp/test_items/area_exam.cpp
+++ b/lib/src/main/cpp/test_items/area_exam.cpp
@@ -14,6 +14,7 @@
#include "turn_a90.h"
#include "../utils/xconvert.h"
#include "../common/apptimer.h"
+#include "../test_common/odo_graph.h"
#define DEBUG(fmt, args...) LOGD("<area_exam> <%s>: " fmt, __func__, ##args)
@@ -35,10 +36,13 @@
void InitAreaExam(void)
{
CurrExamMapIndex = -1;
+ ResetOdo();
}
void TestAreaGeneral(LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, double azimuth, const struct RtkTime *rtkTime)
{
+ UpdataOdo(speed, moveDirect, rtkTime);
+
DetectEnterOrExitMap(car, CarModelList, AreaMapList);
ExecuteExam(CurrExamMapIndex, AreaMapList, car, CarModelList, speed, moveDirect, azimuth, rtkTime);
@@ -134,7 +138,7 @@
case MAP_TYPE_STOP_START:
DEBUG("杩涘叆涓婂潯璧锋鍦哄湴 %d", AreaMapList[index].id);
- StartSAS(AreaMapList[index].id, move, rtkTime);
+ StartSAS(AreaMapList[index].id, &AreaMapList[index].map, car, move, rtkTime);
CurrExamStatus = EXAM_AREA_RUN;
break;
case MAP_TYPE_PART_EDGE:
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 47690a2..12e8000 100644
--- a/lib/src/main/cpp/test_items/stop_and_start.cpp
+++ b/lib/src/main/cpp/test_items/stop_and_start.cpp
@@ -44,9 +44,17 @@
static double DistanceOfTire2Edge(const Polygon *map, const car_model *car);
static bool ExitTestArea(const Polygon *map, const car_model *car);
-void StartSAS(int index, int moveDirect, const struct RtkTime *rtkTime)
+void StartSAS(int index, const Polygon *map, const car_model *car, int moveDirect, const struct RtkTime *rtkTime)
{
+ double yawEdge = YawOf(map->point[8], map->point[0]);
+
+ if (moveDirect < 0 || DeltaYaw(car->yaw, yawEdge) >= 90.0) {
+ testing = false;
+ return;
+ }
+
DEBUG("杩涘叆鍧¤捣椤圭洰");
+
testing = true;
mapIndex = index;
prevMoveDirect = moveDirect;
@@ -68,6 +76,9 @@
int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
+ if (!testing)
+ return 0;
+
if (CrashRedLine(map, car)) {
// 杞﹁疆鍘嬬嚎锛屼笉鍚堟牸
if (!occurCrashRedLine) {
@@ -81,6 +92,10 @@
if (ExitTestArea(map, car)) {
// 椹剁娴嬭瘯鍖�
+ if (!stopCar) {
+ // 涓嶅仠杞︾洿鎺ョ寮�
+ AddExamFault(10103, rtkTime);
+ }
testing = false;
}
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 894a7e3..0697693 100644
--- a/lib/src/main/cpp/test_items/stop_and_start.h
+++ b/lib/src/main/cpp/test_items/stop_and_start.h
@@ -9,7 +9,7 @@
using namespace std;
-void StartSAS(int index, int moveDirect, const struct RtkTime *rtkTime);
+void StartSAS(int index, const Polygon *map, const car_model *car, 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/cpp/test_items2/car_start.cpp b/lib/src/main/cpp/test_items2/car_start.cpp
index 31410f5..d111adf 100644
--- a/lib/src/main/cpp/test_items2/car_start.cpp
+++ b/lib/src/main/cpp/test_items2/car_start.cpp
@@ -58,8 +58,7 @@
}
if (moveDistance > examParam.start_car_limit_distance) {
- sensor = ReadCarSensorValue(HAND_BREAK);
-
+ /*sensor = ReadCarSensorValue(HAND_BREAK);
if (sensor.name == HAND_BREAK && sensor.value == BREAK_ACTIVE) {
DEBUG("Handbreak active move over 10m");
// 鎵嬪埞鎷夎捣鐘舵�佷笅锛岃椹朵簡10绫充互涓婏紝涓嶅悎鏍�
@@ -68,7 +67,7 @@
// 鎵嬪埞鎷夎捣鐘舵�佷笅锛岃椹朵簡1绫充互涓婏紝鎵�10鍒�
DEBUG("Handbreak active move over 1M");
AddExamFault(40206, rtkTime);
- }
+ }*/
PlayTTS(examParam.start_car_end_tts, NULL);
DEBUG("############# 瀹屾垚璧锋 ############");
--
Gitblit v1.8.0