From b3608d75b6dd9d74aa1a71e1a2ef2a970e6a050c Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期三, 18 三月 2020 23:29:48 +0800
Subject: [PATCH] 挡位匹配.
---
lib/src/main/cpp/test_common/car_sensor.h | 11 ++++-
lib/src/main/cpp/test_items/stop_and_start.cpp | 6 +--
lib/src/main/cpp/utils/xconvert.h | 3 +
lib/src/main/cpp/utils/xconvert.cpp | 10 +++++
lib/src/main/cpp/test_items2/road_exam.cpp | 65 +++++++++++++++++++++++++++++++-
5 files changed, 86 insertions(+), 9 deletions(-)
diff --git a/lib/src/main/cpp/test_common/car_sensor.h b/lib/src/main/cpp/test_common/car_sensor.h
index 0350fd9..290f6bd 100644
--- a/lib/src/main/cpp/test_common/car_sensor.h
+++ b/lib/src/main/cpp/test_common/car_sensor.h
@@ -19,7 +19,7 @@
MAIN_BEAM_LAMP,
SEATBELT,
ENGINE_START,
- CAR_STATUS_END
+ CAR_STATUS_END //////////////
};
enum {
@@ -35,7 +35,14 @@
FLASH_BEAM_LIGHT,
FOG_LIGHT,
INSERT_SEATBELT,
- ENGINE_START_ACTIVE
+ ENGINE_START_ACTIVE,
+ SHIFT_N,
+ SHIFT_R,
+ SHIFT_1,
+ SHIFT_2,
+ SHIFT_3,
+ SHIFT_4,
+ SHIFT_5
};
void CarSensorInit(void);
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 47d263f..980b782 100644
--- a/lib/src/main/cpp/test_items/stop_and_start.cpp
+++ b/lib/src/main/cpp/test_items/stop_and_start.cpp
@@ -28,7 +28,6 @@
static bool testing = false;
-static double slideDistance;
static PointF stopPoint;
static int prevMoveDirect;
@@ -50,7 +49,6 @@
{
DEBUG("杩涘叆鍧¤捣椤圭洰");
testing = true;
- slideDistance = 0.0;
prevMoveDirect = moveDirect;
@@ -134,9 +132,9 @@
if (stopCar) {
if (IntersectionOfLine(map->point[4], stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) == 1) {
// 鍙戠敓鍚庢粦
- slideDistance = DistanceOf(stopPoint, car->carXY[car->axial[AXIAL_FRONT]]);
+ double slideDistance = DistanceOf(stopPoint, car->carXY[car->axial[AXIAL_FRONT]]);
- if (slideLongDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) {
+ if (slideDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) {
slideNormalDistance = true;
}
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index c0b82e3..80da3de 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -22,6 +22,8 @@
static const int TURN_THRESHOLD = 10;
static const int TURN_CHECK_INTERVAL = D_SEC(1);
+const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
+const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;
static bool occurCrashRedLine;
static bool occurCrashGreenLine;
@@ -36,6 +38,10 @@
static bool reportStopCarOnRedArea;
static PointF stopPoint;
+static bool slideLongDistance;
+static bool slideNormalDistance;
+static bool occurSlide;
+
static struct drive_timer {
int hour;
int min;
@@ -49,6 +55,7 @@
static const uint32_t TURN_SIGNAL_LAMP_ADVANCE = D_SEC(3);
static const double MAX_SPEED = 40.0 * 1000.0 / 3600.0;
+static const int SPEED_SHIFT_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};
static void Rtk2DirveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime);
static char isTurn(int currYaw, int prevYaw);
@@ -71,6 +78,9 @@
prevMoveDirect = 0;
reportStopCarOnRedArea = false;
+ occurSlide = false;
+ slideLongDistance = false;
+ slideNormalDistance = false;
}
void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
@@ -85,14 +95,40 @@
occurOverSpeed = false;
}
+ // 鎸′綅鍖归厤妫�娴�
+ switch (ReadCarStatus(SHIFT)) {
+ case SHIFT_N:
+ if (moveDirect != 0) {
+
+ }
+ break;
+ case SHIFT_1:
+ if (ConvertMs2KMs(speed) < SPEED_SHIFT_TABLE[0][0] || ConvertMs2KMs(speed) > SPEED_SHIFT_TABLE[0][1]) {
+
+ }
+ break;
+ case SHIFT_2:
+ break;
+ case SHIFT_3:
+ break;
+ case SHIFT_4:
+ break;
+ case SHIFT_5:
+ break;
+ default:break;
+ }
+
+ // 璧锋鍚庢粦
if (moveDirect != prevMoveDirect) {
if (moveDirect == 0) {
stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
reportStopCarOnRedArea = false;
DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
- } else if (moveDirect == -1) {
-
+ } else if (moveDirect == -1 && prevMoveDirect == 0) {
+ DEBUG("寮�濮嬪悗婊�");
+ stopPoint = car->basePoint;
+ occurSlide = true;
}
prevMoveDirect = moveDirect;
} else if (moveDirect == 0) {
@@ -104,8 +140,31 @@
DEBUG("涓�斿仠杞�");
reportStopCarOnRedArea = true;
}
+ } else if (moveDirect == -1) {
+ if (occurSlide) {
+ double slideDistance = DistanceOf(stopPoint, car->basePoint);
- stopPoint = car->basePoint;
+ if (slideDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) {
+ slideNormalDistance = true;
+ }
+
+ if (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED && !slideLongDistance) {
+ // 鍚庢粦瓒呰繃30鍘樼背, 涓嶅悎鏍�
+ AddExamFault(5, rtkTime);
+ DEBUG("鍚庢粦瓒呰繃30鍘樼背");
+ slideLongDistance = true;
+ }
+ }
+ } else {
+ if (slideNormalDistance) {
+ // 鍚庢粦锛屾墸10鍒�
+ AddExamFault(18, rtkTime);
+ DEBUG("鍚庢粦瓒呰繃10鍘樼背, 浣嗕笉瓒呰繃30鍘樼背");
+ }
+
+ slideNormalDistance = false;
+ slideLongDistance = false;
+ occurSlide = false;
}
switch (ReadCarStatus(TURN_SIGNAL_LAMP)) {
diff --git a/lib/src/main/cpp/utils/xconvert.cpp b/lib/src/main/cpp/utils/xconvert.cpp
index 0232370..a4e524b 100644
--- a/lib/src/main/cpp/utils/xconvert.cpp
+++ b/lib/src/main/cpp/utils/xconvert.cpp
@@ -18,6 +18,16 @@
#define DAYS_PER_WEEK 7
#define DAYS_PER_YEAR 365
+double ConvertKMh2Ms(int kmh)
+{
+ return ((double)kmh) * 1000.0 / 3600.0;
+}
+
+double ConvertMs2KMs(double ms)
+{
+ return ms * 3600.0 / 1000.0;
+}
+
void ConvertPhoneNum(uint8_t *dst, int length, const char *src)
{
int p = length - 1;
diff --git a/lib/src/main/cpp/utils/xconvert.h b/lib/src/main/cpp/utils/xconvert.h
index cec7129..dbbe7b4 100644
--- a/lib/src/main/cpp/utils/xconvert.h
+++ b/lib/src/main/cpp/utils/xconvert.h
@@ -7,6 +7,9 @@
#include <cstdint>
+double ConvertKMh2Ms(int kmh);
+double ConvertMs2KMs(double ms);
+
void ConvertPhoneNum(uint8_t *dst, int length, const char *src);
void ConvertHex2String(char *str, const uint8_t *hex, int length);
void ConvertString2Hex(uint8_t *hex, int length, const char *str);
--
Gitblit v1.8.0