From 4bd08f0355b6b2cf3c027202d5ad301b4e182953 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期五, 31 三月 2023 17:16:22 +0800
Subject: [PATCH] 科目二修改
---
lib/src/main/cpp/common/apptimer.h | 2
lib/src/main/cpp/driver_test.cpp | 1043 ++++-----------
lib/src/main/cpp/test_common/odo_graph.cpp | 41
lib/src/main/cpp/test_items/stop_and_start.cpp | 314 ++--
lib/src/main/cpp/common/semaphore.h | 39
lib/src/main/cpp/CMakeLists.txt | 13
lib/src/main/cpp/rtk_module/rtk.cpp | 87
lib/src/main/cpp/test_common/Geometry.cpp | 148 +
lib/src/main/cpp/test_items/driving_curve.h | 4
lib/src/main/cpp/test_items/park_bottom.cpp | 475 ++----
lib/src/main/cpp/mcu/ahp.cpp | 23
lib/src/main/cpp/test_items/park_edge.cpp | 128 -
lib/src/main/cpp/master/comm_if.cpp | 196 ++
lib/src/main/cpp/native-lib.cpp | 2
lib/src/main/cpp/utils/xconvert.h | 13
lib/src/main/cpp/mcu/ada.cpp | 18
lib/src/main/cpp/test_common/Geometry.h | 38
lib/src/main/cpp/rtk_platform/platform.cpp | 19
lib/src/main/cpp/test_common/odo_graph.h | 4
lib/src/main/cpp/map.h | 66
lib/src/main/cpp/test_items/driving_curve.cpp | 55
lib/src/main/cpp/rtk_module/rtk.h | 22
lib/src/main/cpp/test_items/park_edge.h | 2
lib/src/main/cpp/test_items/park_bottom.h | 5
lib/src/main/cpp/test_items/area_exam.cpp | 682 ++-------
lib/src/main/cpp/driver_test.h | 181 +-
lib/src/main/cpp/master/comm_if.h | 7
lib/src/main/cpp/test_items/turn_a90.cpp | 170 +-
lib/src/main/cpp/common/apptimer.cpp | 4
/dev/null | 22
lib/src/main/cpp/test_items/area_exam.h | 7
lib/src/main/cpp/utils/xconvert.cpp | 86 +
lib/src/main/cpp/common/observer.h | 64
lib/src/main/cpp/test_items/turn_a90.h | 2
lib/src/main/cpp/test_items/stop_and_start.h | 4
35 files changed, 1,795 insertions(+), 2,191 deletions(-)
diff --git a/lib/src/main/cpp/CMakeLists.txt b/lib/src/main/cpp/CMakeLists.txt
index af96185..32b4e42 100644
--- a/lib/src/main/cpp/CMakeLists.txt
+++ b/lib/src/main/cpp/CMakeLists.txt
@@ -48,19 +48,6 @@
test_items/turn_a90.cpp
test_items/area_exam.cpp
- test_items2/prepare.cpp
- test_items2/common_check.cpp
- test_items2/dummy_light.cpp
- test_items2/road_exam.cpp
- test_items2/through_something.cpp
- test_items2/drive_straight.cpp
- test_items2/stop_car.cpp
- test_items2/operate_gear.cpp
- test_items2/smart_item.cpp
- test_items2/car_start.cpp
- test_items2/overtake.cpp
- test_items2/change_lane.cpp
-
rtk_module/rtk.cpp
rtk_module/virtual_rtk.cpp
master/comm_if.cpp
diff --git a/lib/src/main/cpp/common/apptimer.cpp b/lib/src/main/cpp/common/apptimer.cpp
index bcfe799..ff87408 100644
--- a/lib/src/main/cpp/common/apptimer.cpp
+++ b/lib/src/main/cpp/common/apptimer.cpp
@@ -203,14 +203,14 @@
return as.count();
}
-uint32_t AppTimer_GetGmtTickCount(void)
+uint64_t AppTimer_GetGmtTickCount(void)
{
std::chrono::seconds as = std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch());
return as.count();
}
-std::string FormatTime(const char *fmt)
+std::string FormatTime(const char *fmt = "%Y-%m-%d %H:%M:%S")
{
auto now = std::chrono::system_clock::now();
auto timet = std::chrono::system_clock::to_time_t(now);
diff --git a/lib/src/main/cpp/common/apptimer.h b/lib/src/main/cpp/common/apptimer.h
index 60e9b4b..b06ece7 100644
--- a/lib/src/main/cpp/common/apptimer.h
+++ b/lib/src/main/cpp/common/apptimer.h
@@ -58,7 +58,7 @@
uint32_t AppTimer_GetTickCount(void);
-uint32_t AppTimer_GetGmtTickCount(void);
+uint64_t AppTimer_GetGmtTickCount(void);
std::string FormatTime(const char *fmt);
diff --git a/lib/src/main/cpp/common/observer.h b/lib/src/main/cpp/common/observer.h
new file mode 100644
index 0000000..0b2c804
--- /dev/null
+++ b/lib/src/main/cpp/common/observer.h
@@ -0,0 +1,64 @@
+//
+// Created by YY on 2023/3/30.
+//
+
+#ifndef MYAPPLICATION3_OBSERVER_H
+#define MYAPPLICATION3_OBSERVER_H
+
+#include <iostream>
+#include <string>
+#include <functional>
+#include <map>
+
+namespace ilovers {
+ class NonCopyable {
+ protected:
+ NonCopyable() = default;
+
+ ~NonCopyable() = default;
+
+ NonCopyable(const NonCopyable &) = delete;
+
+ NonCopyable &operator=(const NonCopyable &) = delete;
+ };
+
+ template<typename Func>
+ class Observer : NonCopyable {
+ public:
+ Observer() {}
+
+ ~Observer() {}
+
+ int Connect(Func &&f) {
+ return Assgin(f);
+ }
+
+ int Connect(const Func &f) {
+ return Assgin(f);
+ }
+
+ void Disconnect(int key) {
+ m_connections.erase(key);
+ }
+
+ template<typename... Args>
+ void Notify(Args &&... args) {
+ for (auto &it: m_connections) {
+ it.second(std::forward<Args>(args)...);
+ }
+ }
+
+ private:
+ template<typename F>
+ int Assgin(F &&f) {
+ int k = m_observerId++;
+ m_connections.emplace(k, std::forward<F>(f));
+ return k;
+ }
+
+ int m_observerId = 0;
+ std::map<int, Func> m_connections;
+ };
+}
+
+#endif //MYAPPLICATION3_OBSERVER_H
diff --git a/lib/src/main/cpp/common/semaphore.h b/lib/src/main/cpp/common/semaphore.h
new file mode 100644
index 0000000..a5b769d
--- /dev/null
+++ b/lib/src/main/cpp/common/semaphore.h
@@ -0,0 +1,39 @@
+//
+// Created by YY on 2022/12/30.
+//
+
+#ifndef MYAPPLICATION3_SEMAPHORE_H
+#define MYAPPLICATION3_SEMAPHORE_H
+
+#include <mutex>
+#include <condition_variable>
+
+namespace ilovers{
+ class semaphore {
+ public:
+ semaphore(int value=1): count{value}, wakeups{0} {}
+
+ void wait(void) {
+ std::unique_lock<std::mutex> lock{mutex};
+ if (--count<0) { // count is not enough ?
+ condition.wait(lock, [&]()->bool{ return wakeups>0;}); // suspend and wait ...
+ --wakeups; // ok, me wakeup !
+ }
+ }
+ void signal(void) {
+ std::lock_guard<std::mutex> lock{mutex};
+ if(++count<=0) { // have some thread suspended ?
+ ++wakeups;
+ condition.notify_one(); // notify one !
+ }
+ }
+
+ private:
+ int count;
+ int wakeups;
+ std::mutex mutex;
+ std::condition_variable condition;
+ };
+};
+
+#endif //MYAPPLICATION3_SEMAPHORE_H
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 365bbd8..1563f56 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -35,10 +35,11 @@
#include "test_common/car_sensor.h"
#include "mcu/mcu_if.h"
#include "test_common/car_sensor.h"
-#include "test_items2/road_exam.h"
#include "test_items/area_exam.h"
-#include "test_items2/prepare.h"
#include "test_common/odo_graph.h"
+#include "map.h"
+#include "common/semaphore.h"
+#include "common/string_util.h"
#define DEBUG(fmt, args...) LOGD("<driver_test> <%s>: " fmt, __func__, ##args)
@@ -133,15 +134,18 @@
vector<ExamFault> ExamFaultList;
static int examFaultIndex = 0;
-static LIST_AREA_MAP AreaMapList;
+//static LIST_AREA_MAP AreaMapList;
+static area_map_t AreaMap;
+
+ilovers::semaphore sem(0);
static road_exam_map RoadMap;
static int exam_dummy_light;
-static car_model *CarModel = NULL;
+static car_model_t CarModel;
-static LIST_CAR_MODEL CarModelList; // 涓�娈垫椂闂寸殑杞﹁締杞ㄨ抗闆嗗悎
+//static LIST_CAR_MODEL CarModelList; // 涓�娈垫椂闂寸殑杞﹁締杞ㄨ抗闆嗗悎
static struct dummy_light_exam *DummyLightContent;
static int DummyLightContentSize;
@@ -155,35 +159,32 @@
#define RTK_BUFFER_SIZE 100
#define CAR_MODEL_CACHE_SIZE 10
-static rtk_info *RtkBuffer = NULL;
-static int RtkBufferNum = 0, RtkBufferIn = 0;
+static std::list<rtk_info_t> RtkInfoList;
-static std::mutex clock_mutex;
-static struct RtkTime rtkClock;
+// 瀛樺叆鍓嶅悗2缁勫缓妯℃暟鎹�
+static modeling_t realtimeBodyModeling[2];
+static motion_t realtimeMotionStatus;
+
+static prime_t prime;
static void SetExamParamDefault(void);
static void EngineStartHold(apptimer_var_t val);
-static void ExecuteExam(const struct RtkTime* rtkTime);
-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 motion_t CalcMotionState(std::list<rtk_info_t> &s);
+static void ExecuteExam(prime_t &prime);
+
static void ReadDriverExamPrimerTimeout(apptimer_var_t val);
-static void UpdateCarBodyCoord(struct RtkTime *rtkTime, double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel);
-static bool UpdateCarCoord(double &spd, int &mov, int &idx);
-
-static void PrintObdInfo(struct RtkTime *rtkTime, double speed);
-
-
-static void ClockGener(apptimer_var_t val);
+static void CalcBodyModeling(modeling_t &car, car_model_t &carModel, rtk_info_t &rtk);
+static void work_thread(void);
+static void UploadModeling(motion_t &motion, modeling_t &modeling);
void DriverTestInit(void)
{
ExamStart = false;
SetExamParamDefault();
- CarModel = NULL;
- CarModelList.clear();
+// CarModelList.clear();
- AreaMapList.clear();
+// AreaMapList.clear();
RoadMap.roads.clear();
RoadMap.specialAreas.clear();
@@ -195,10 +196,7 @@
DummyLightContentSize = 0;
DummyLightContent = NULL;
- RtkBuffer = (rtk_info *) malloc(RTK_BUFFER_SIZE * sizeof(rtk_info));
- RtkBufferNum = RtkBufferIn = 0;
-
-// AppTimer_add(ClockGener, 200); // App鑷繁浜х敓瀹氭椂鑺傛媿
+ std::thread(work_thread).detach();
}
static void SetExamParamDefault(void)
@@ -313,14 +311,21 @@
if (ExamStart)
return;
- for (int i = 0; i < AreaMapList.size(); ++i) {
- if (AreaMapList[i].map.point != NULL)
- free(AreaMapList[i].map.point);
- if (AreaMapList[i].map2.point != NULL)
- free(AreaMapList[i].map2.point);
- }
+// for (int i = 0; i < AreaMapList.size(); ++i) {
+// if (AreaMapList[i].map.point != NULL)
+// free(AreaMapList[i].map.point);
+// if (AreaMapList[i].map2.point != NULL)
+// free(AreaMapList[i].map2.point);
+// }
+//
+// LIST_AREA_MAP().swap(AreaMapList);
- LIST_AREA_MAP().swap(AreaMapList);
+
+ vector<curve_map_t>().swap(AreaMap.curve_map);
+ vector<park_button_map_t>().swap(AreaMap.park_button_map);
+ vector<park_edge_map_t>().swap(AreaMap.park_edge_map);
+ vector<turn_a90_map_t>().swap(AreaMap.turn_a90_map);
+ vector<uphill_map_t>().swap(AreaMap.uphill_map);
}
void AddAreaMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2)
@@ -329,33 +334,48 @@
return;
DEBUG("鍔犲叆鍦板浘淇℃伅 id %d type %d pointNum %d point2Num %d", id, type, pointNum, pointNum2);
- struct area_exam_map newMap;
+// struct area_exam_map newMap;
+//
+// newMap.id = id;
+// newMap.type = type;
+// newMap.map.num = pointNum;
+// newMap.map2.num = 0;
+// newMap.map.point = NULL;
+// newMap.map2.point = NULL;
+//
+// if (pointNum > 0) {
+// newMap.map.point = (PointF *) malloc(pointNum * sizeof(PointF));
+// for (int i = 0; i < pointNum; ++i) {
+// newMap.map.point[i].X = map[i][0];
+// newMap.map.point[i].Y = map[i][1];
+// }
+// }
+//
+// if (pointNum2 > 0 && map2 != NULL) {
+// newMap.map2.num = pointNum2;
+// newMap.map2.point = (PointF *) malloc(pointNum2 * sizeof(PointF));
+// for (int i = 0; i < pointNum2; ++i) {
+// newMap.map2.point[i].X = map2[i][0];
+// newMap.map2.point[i].Y = map2[i][1];
+// }
+// }
+//
+// AreaMapList.push_back(newMap);
+}
- newMap.id = id;
- newMap.type = type;
- newMap.map.num = pointNum;
- newMap.map2.num = 0;
- newMap.map.point = NULL;
- newMap.map2.point = NULL;
+void AddCurveMap(curve_map_t &map)
+{
+ AreaMap.curve_map.push_back(map);
+}
- if (pointNum > 0) {
- newMap.map.point = (PointF *) malloc(pointNum * sizeof(PointF));
- for (int i = 0; i < pointNum; ++i) {
- newMap.map.point[i].X = map[i][0];
- newMap.map.point[i].Y = map[i][1];
- }
- }
+void AddParkButtonMap(park_button_map_t &map)
+{
+ AreaMap.park_button_map.push_back(map);
+}
- if (pointNum2 > 0 && map2 != NULL) {
- newMap.map2.num = pointNum2;
- newMap.map2.point = (PointF *) malloc(pointNum2 * sizeof(PointF));
- for (int i = 0; i < pointNum2; ++i) {
- newMap.map2.point[i].X = map2[i][0];
- newMap.map2.point[i].Y = map2[i][1];
- }
- }
-
- AreaMapList.push_back(newMap);
+void AddParkEdgeMap(park_edge_map_t &map)
+{
+ AreaMap.park_edge_map.push_back(map);
}
void CleanRoadMap(void)
@@ -426,75 +446,59 @@
if (point == NULL || pointNum == 0 || ExamStart) return;
- if (CarModel != NULL) {
- if (CarModel->body != NULL)
- free(CarModel->body);
- if (CarModel->carDesc != NULL)
- free(CarModel->carDesc);
- if (CarModel->carXY != NULL)
- free(CarModel->carXY);
- free(CarModel);
- CarModel = NULL;
- }
+ vector<int>().swap(CarModel.body);
+ vector<car_desc_t>().swap(CarModel.carDesc);
- CarModel = (car_model *)malloc(sizeof(car_model));
- CarModel->basePoint.X = basePoint[0];
- CarModel->basePoint.Y = basePoint[1];
- CarModel->axial[0] = axial[0];
- CarModel->axial[1] = axial[1];
- CarModel->left_front_tire[0] = left_front_tire[0];
- CarModel->left_front_tire[1] = left_front_tire[1];
- CarModel->right_front_tire[0] = right_front_tire[0];
- CarModel->right_front_tire[1] = right_front_tire[1];
- CarModel->left_rear_tire[0] = left_rear_tire[0];
- CarModel->left_rear_tire[1] = left_rear_tire[1];
- CarModel->right_rear_tire[0] = right_rear_tire[0];
- CarModel->right_rear_tire[1] = right_rear_tire[1];
+ CarModel.basePoint.X = basePoint[0];
+ CarModel.basePoint.Y = basePoint[1];
+ CarModel.axial[0] = axial[0];
+ CarModel.axial[1] = axial[1];
+ CarModel.left_front_tire[0] = left_front_tire[0];
+ CarModel.left_front_tire[1] = left_front_tire[1];
+ CarModel.right_front_tire[0] = right_front_tire[0];
+ CarModel.right_front_tire[1] = right_front_tire[1];
+ CarModel.left_rear_tire[0] = left_rear_tire[0];
+ CarModel.left_rear_tire[1] = left_rear_tire[1];
+ CarModel.right_rear_tire[0] = right_rear_tire[0];
+ CarModel.right_rear_tire[1] = right_rear_tire[1];
- CarModel->bodyNum = bodyNum;
if (bodyNum == 0 || body == NULL) {
- CarModel->bodyNum = 6;
- CarModel->body = (int *) malloc(sizeof(int) * 6);
for (int i = 0; i < 6; ++i) {
- CarModel->body[i] = i;
+ CarModel.body.push_back(i);
}
} else {
- CarModel->body = (int *) malloc(sizeof(int) * CarModel->bodyNum);
- for (int i = 0; i < CarModel->bodyNum; ++i) {
- CarModel->body[i] = body[i];
+ for (int i = 0; i < bodyNum; ++i) {
+ CarModel.body.push_back(body[i]);
}
}
- CarModel->antPitch = antPitch;
- CarModel->antHeight = antHeight;
- CarModel->groundHeight = groundHeight;
+ CarModel.antPitch = antPitch;
+ CarModel.antHeight = antHeight;
+ CarModel.groundHeight = groundHeight;
- CarModel->pointNum = pointNum;
- CarModel->carDesc = (struct car_desc_ *)malloc(sizeof(struct car_desc_) * pointNum);
- CarModel->carXY = (PointF *) malloc(sizeof(PointF) * pointNum);
-
+ CarModel.carDesc.resize(pointNum);
// 娴嬮噺鍧愭爣杞崲涓鸿窛绂�-瑙掑害褰㈠紡
double C02 = (point[0][0]-basePoint[0])*(point[0][0]-basePoint[0]) +
(point[0][1]-basePoint[1])*(point[0][1]-basePoint[1]);
double C0 = sqrt(C02);
- CarModel->carDesc[0].distance = sqrt(C02);
- CarModel->carDesc[0].angle = 0.0;
+ CarModel.carDesc[0].distance = sqrt(C02);
+ CarModel.carDesc[0].angle = 0.0;
for (int i = 1; i < pointNum; ++i) {
double dis2 = (point[i][0]-basePoint[0])*(point[i][0]-basePoint[0]) +
(point[i][1]-basePoint[1])*(point[i][1]-basePoint[1]);
double dis = sqrt(dis2);
- CarModel->carDesc[i].distance = dis;
+ CarModel.carDesc[i].distance = dis;
- CarModel->carDesc[i].angle = 180 * acos((dis2 + C02 - ((point[i][0]-point[0][0])*(point[i][0]-point[0][0]) +
+ CarModel.carDesc[i].angle = 180 * acos((dis2 + C02 - ((point[i][0]-point[0][0])*(point[i][0]-point[0][0]) +
(point[i][1]-point[0][1])*(point[i][1]-point[0][1])))/(2*C0*dis)) / M_PI;
if (i > axial[1])
- CarModel->carDesc[i].angle = 360.0 - CarModel->carDesc[i].angle;
+ CarModel.carDesc[i].angle = 360.0 - CarModel.carDesc[i].angle;
- DEBUG("鍔犲叆鐐�<%d> 璺濈 %f 瑙掑害 %f", i, CarModel->carDesc[i].distance, CarModel->carDesc[i].angle);
+ DEBUG("鍔犲叆鐐�<%d> 璺濈 %f 瑙掑害 %f", i, CarModel.carDesc[i].distance, CarModel.carDesc[i].angle);
}
// CarModel->carDesc[0].distance = 0.2465;
@@ -518,81 +522,6 @@
DEBUG("SetCarMeasurePoint Calc Over");
}
-/***********************************************
- * TIME1 - TIME2: millisecond
- * @param rtkTime1
- * @param rtkTime2
- * @return
- */
-uint32_t TimeGetDiff(const struct RtkTime *rtkTime1, const struct RtkTime *rtkTime2)
-{
- char tm1[64], tm2[64];
-
- sprintf(tm1, "%02d%02d%02d%02d%02d%02d%03d", rtkTime1->YY, rtkTime1->MM, rtkTime1->DD, rtkTime1->hh, rtkTime1->mm, rtkTime1->ss, rtkTime1->mss*10);
- sprintf(tm2, "%02d%02d%02d%02d%02d%02d%03d", rtkTime2->YY, rtkTime2->MM, rtkTime2->DD, rtkTime2->hh, rtkTime2->mm, rtkTime2->ss, rtkTime2->mss*10);
-
- if (strcmp(tm1, tm2) < 0) {
- return (uint32_t)(-1);
- }
-
- if (rtkTime1->YY == rtkTime2->YY && rtkTime1->MM == rtkTime2->MM && rtkTime1->DD == rtkTime2->DD) {
- return TimeGetDiff(rtkTime1->hh, rtkTime1->mm, rtkTime1->ss, rtkTime1->mss*10,
- rtkTime2->hh, rtkTime2->mm, rtkTime2->ss, rtkTime2->mss*10);
- } else {
- return (TimeMakeComposite(2000 + rtkTime1->YY, rtkTime1->MM, rtkTime1->DD, rtkTime1->hh, rtkTime1->mm, rtkTime1->ss) -
- TimeMakeComposite(2000 + rtkTime2->YY, rtkTime2->MM, rtkTime2->DD, rtkTime2->hh, rtkTime2->mm, rtkTime2->ss)) * 1000
- + (1000 + rtkTime1->mss*10 - rtkTime2->mss*10) % 1000;
- }
-}
-
-void SetDummyLightExam(int n, struct dummy_light_exam *cfg)
-{
- DEBUG("鑾峰彇妯℃嫙璺�冪伅鍏夋祴璇曢」鐩� N = %d %d", n, ExamStart);
-
- static const int CONV_TABLE[] = {(FLASH_BEAM_LAMP<<8)+OFF_LIGHT,
- (TURN_SIGNAL_LAMP<<8)+OFF_LIGHT,
- (TURN_SIGNAL_LAMP<<8)+OFF_LIGHT,
- (TURN_SIGNAL_LAMP<<8)+OFF_LIGHT,
- (FOG_LAMP<<8)+OFF_LIGHT,
- (CLEARANCE_LAMP<<8)+OFF_LIGHT,
- (MAIN_BEAM_LAMP<<8)+OFF_LIGHT,
- (DIPPED_BEAM_LAMP<<8)+OFF_LIGHT,
- 0,
- (DIPPED_BEAM_LAMP<<8)+DIPPED_BEAM_LIGHT,
- (MAIN_BEAM_LAMP<<8)+MAIN_BEAM_LIGHT,
- (CLEARANCE_LAMP<<8)+CLEARANCE_LIGHT,
- (FOG_LAMP<<8)+FOG_LIGHT,
- (TURN_SIGNAL_LAMP<<8)+LEFT_TURN_LIGHT,
- (TURN_SIGNAL_LAMP<<8)+RIGHT_TURN_LIGHT,
- (TURN_SIGNAL_LAMP<<8)+HAZARD_LIGHTS,
- (FLASH_BEAM_LAMP<<8)+FLASH_BEAM_LIGHT};
-
- const int *cov = CONV_TABLE + 8;
-
- if (ExamStart) return;
-
- if (DummyLightContent != NULL) {
- delete []DummyLightContent;
- DummyLightContent = NULL;
- DummyLightContentSize = 0;
- }
-
- DummyLightContent = new struct dummy_light_exam[n];
- DummyLightContentSize = n;
-
- for (int i = 0; i < n; i++) {
- DummyLightContent[i].item = cfg[i].item;
- DummyLightContent[i].tts = cfg[i].tts;
- DummyLightContent[i].wrongCode = cfg[i].wrongCode;
- // Sensor Name<<8 + Sensor Status
- for (int j = 0; j < cfg[i].process.size(); ++j) {
- DummyLightContent[i].process.push_back(cov[cfg[i].process[j]]);
- }
- for (int j = 0; j < cfg[i].solution.size(); ++j) {
- DummyLightContent[i].solution.push_back(cov[cfg[i].solution[j]]);
- }
- }
-}
void StartDriverExam(int start, int type)
{
@@ -602,9 +531,7 @@
if (start == 0) {
DEBUG("缁撴潫鑰冭瘯");
- TerminateRoadExam();
TerminateAreaExam();
-
ExamStart = false;
MA_SendExamStatus(0, 0);
@@ -613,12 +540,12 @@
// type = TEST_TYPE_ROAD_CALIBRATE;
- if (AreaMapList.size() == 0 && type == TEST_TYPE_AREA) {
+ /*if (AreaMapList.size() == 0 && type == TEST_TYPE_AREA) {
DEBUG("娌℃湁鍦鸿�冨湴鍥�");
err = true;
MA_SendExamStatus(0, -1);
- }
- if (CarModel == NULL) {
+ }*/
+ if (CarModel.carDesc.size() == 0) {
DEBUG("娌℃湁杞︽ā");
err = true;
MA_SendExamStatus(0, -2);
@@ -656,311 +583,197 @@
}
if (type == TEST_TYPE_ROAD_TRUE_LIGHT) {
RoadMap.calibrate = 0;
- InitRoadExam(RoadMap);
}
if (type == TEST_TYPE_AREA) {
InitAreaExam();
}
if (type == TEST_TYPE_ROAD_CALIBRATE) {
RoadMap.calibrate = 1;
- InitRoadExam(RoadMap);
}
}
MA_SendExamStatus(1, 0);
}
}
-static void ClockGener(apptimer_var_t val)
+/***************************************
+ * 瑙﹀彂鑰冭瘯璇勫垽
+ */
+static void work_thread(void)
{
- rtk_info rtk;
+ while (true) {
+ sem.wait();
- rtk.qf = 3;
- rtk.heading = 0;
- rtk.pitch = 0;
- rtk.roll = 0;
- rtk.x = 0;
- rtk.y = 0;
-
- struct timeval tv;
- struct timezone tz;
-
- gettimeofday(&tv, &tz);
-
- struct tm *pTime = localtime(&tv.tv_sec);
-
- rtk.YY = (pTime->tm_year + 1900) % 100;
- rtk.MM = pTime->tm_mon + 1;
- rtk.DD = pTime->tm_mday;
- rtk.hh = pTime->tm_hour;
- rtk.mm = pTime->tm_min;
- rtk.ss = pTime->tm_sec;
- rtk.dss = tv.tv_usec / 1000;
-
-// DEBUG("妯℃嫙鏃堕棿 锛�%d-%d-%d %d:%d:%d.%d", rtk.YY, 1+pTime->tm_mon, pTime->tm_mday, pTime->tm_hour, pTime->tm_min, pTime->tm_sec, rtk.dss);
-
- UpdateRTKInfo(&rtk);
-
- AppTimer_add(ClockGener, 200);
+ // 璁＄畻褰撳墠杞﹂�燂紝鍓嶈繘鍚庨��鎴栧仠姝�
+ realtimeMotionStatus = CalcMotionState(RtkInfoList);
+ // 绱姞閲岀▼
+ UpdataOdo(realtimeMotionStatus);
+ // 璁＄畻褰撳墠杞﹁締鐐瑰潗鏍囧��
+ prime.prev_modeling_index = prime.curr_modeling_index;
+ prime.curr_modeling_index = (prime.curr_modeling_index+1) % (sizeof (realtimeBodyModeling) / sizeof (realtimeBodyModeling[0]));
+ CalcBodyModeling(realtimeBodyModeling[prime.curr_modeling_index], CarModel, RtkInfoList.front());
+ // 鍚慤I涓婃姤杞﹁締鐐瑰潗鏍�
+ UploadModeling(realtimeMotionStatus, realtimeBodyModeling[prime.curr_modeling_index]);
+ // 瑙﹀彂鑰冭瘯椤圭洰
+ if (ExamStart) {
+ ExecuteExam(prime);
+ }
+ }
}
-void UpdateRTKInfo(const rtk_info *s)
+static motion_t CalcMotionState(std::list<rtk_info_t> &s)
{
- std::unique_lock<std::mutex> lk(clock_mutex);
- rtkClock.YY = s->YY;
- rtkClock.MM = s->MM;
- rtkClock.DD = s->DD;
- rtkClock.hh = s->hh;
- rtkClock.mm = s->mm;
- rtkClock.ss = s->ss;
- rtkClock.mss = s->dss;
- lk.unlock();
-
- if (ExamStart) {
- ExecuteExam(&rtkClock); // 鎵ц鏃犻渶杞﹁締瀹氫綅鐨勯」鐩�
- }
-
- if (s->qf == 3) {
- RtkBuffer[RtkBufferIn] = *s;
- RtkBufferIn = (RtkBufferIn + 1) % RTK_BUFFER_SIZE;
- if (RtkBufferNum < RTK_BUFFER_SIZE)
- RtkBufferNum++;
- } else {
- return;
- }
+ motion_t motion;
double speed;
- int move;
- int index;
+ move_status_t move;
- if (UpdateCarCoord(speed, move, index)) {
- struct carBrief brief;
+ if (s.size() < 2) {
+ return motion;
+ }
- sprintf(brief.utc, "%04d%02d%02d%02d%02d%02d.%02d", 2000 + RtkBuffer[index].YY,
- RtkBuffer[index].MM, RtkBuffer[index].DD, RtkBuffer[index].hh,
- RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);
+ auto curr = s.begin();
- brief.qf = RtkBuffer[index].qf;
- brief.map_id = -1;//GetMapId(CurrExamMapIndex, MapList, MapNum);
- brief.move = move;
- brief.speed = speed * 3.6;
- brief.heading = RtkBuffer[index].heading;
- brief.main_ant[0] = RtkBuffer[index].x;
- brief.main_ant[1] = RtkBuffer[index].y;
+ motion.timestamp = curr->utc_time;
- brief.axial[0] = CarModel->axial[0];
- brief.axial[1] = CarModel->axial[1];
- brief.left_front_tire[0] = CarModel->left_front_tire[0];
- brief.left_front_tire[1] = CarModel->left_front_tire[1];
- brief.right_front_tire[0] = CarModel->right_front_tire[0];
- brief.right_front_tire[1] = CarModel->right_front_tire[1];
- brief.left_rear_tire[0] = CarModel->left_rear_tire[0];
- brief.left_rear_tire[1] = CarModel->left_rear_tire[1];
- brief.right_rear_tire[0] = CarModel->right_rear_tire[0];
- brief.right_rear_tire[1] = CarModel->right_rear_tire[1];
+ // 鏌ユ壘1绉掑墠鐨勭偣,濡傛灉鎵句笉鍒板垯瑙嗕负鍋滆溅
+ auto prev = s.begin();
+ std::advance(prev, 1);
- brief.bodyNum = CarModel->bodyNum;
- brief.body = (int *) malloc(sizeof(int) * CarModel->bodyNum);
- for (int i = 0; i < CarModel->bodyNum; ++i) {
- brief.body[i] = CarModel->body[i];
+ for (; prev != s.end(); ++prev) {
+ if (prev->utc_time - curr->utc_time > 3000) {
+ return motion;
+ }
+ if (prev->utc_time - curr->utc_time >= 1000) {
+ break;
+ }
+ }
+
+ if (prev == s.end()) {
+ return motion;
+ }
+
+ // 璁＄畻閫熷害(绫�/绉�)銆佸墠杩涘悗閫�
+ speed = sqrt(pow(curr->x - prev->x, 2) + pow(curr->y - prev->y, 2)) * 1000 /
+ static_cast<double>(curr->utc_time - prev->utc_time);
+
+ double deg = 0.0;
+
+ if (speed < 0.05) {
+ // 鍋滆溅
+ move = STOP;
+ } else {
+ // 鍒ゆ柇鍓嶈繘杩樻槸鍚庨��
+ if (fabs(curr->y - prev->y) <= GLB_EPSILON) {
+ if (curr->x > prev->x) {
+ deg = 90;
+ } else {
+ deg = 270;
+ }
+ } else if (fabs(curr->x - prev->x) <= GLB_EPSILON) {
+ if (curr->y > prev->y) {
+ deg = 0;
+ } else {
+ deg = 180;
+ }
+ } else {
+ deg = toDegree(atan(fabs(curr->x - prev->x) /
+ fabs(curr->y - prev->y)));
+
+ if (curr->x > prev->x &&
+ curr->y > prev->y) {
+
+ } else if (curr->x < prev->x &&
+ curr->y > prev->y) {
+ deg = 360 - deg;
+ } else if (curr->x < prev->x &&
+ curr->y < prev->y) {
+ deg = 180 + deg;
+ } else if (curr->x > prev->x &&
+ curr->y < prev->y) {
+ deg = 180 - deg;
+ }
}
- brief.pointNum = CarModel->pointNum;
- brief.point = (double *) malloc(CarModel->pointNum * 2 * sizeof(double));
- for (int i = 0, j = 0; i < CarModel->pointNum; ++i) {
- brief.point[j++] = round(CarModel->carXY[i].X, 4);
- brief.point[j++] = round(CarModel->carXY[i].Y, 4);
+ deg = fabs(curr->heading - deg);
+ if (deg > 180) {
+ deg = 360 - deg;
+ }
+ if (deg < 90) {
+ // 鍓嶈繘
+ move = FORWARD;
+ } else {
+ // 鍚庨��
+ move = BACKWARD;
+ }
+ }
+
+ motion.speed = speed;
+ motion.move = move;
+
+ return motion;
+}
+
+void UpdateRTKInfo(const rtk_info_t *s)
+{
+ RtkInfoList.push_front(*s);
+
+ while (RtkInfoList.size() > 100) {
+ RtkInfoList.pop_back();
+ }
+ sem.signal();
+}
+
+static void UploadModeling(motion_t &motion, modeling_t &modeling)
+{
+ struct carBrief brief;
+
+ struct TimeStructure ts;
+
+ TimeBreakdown(modeling.utc_time / 1000, &ts);
+
+ sprintf(brief.utc, "%04d%02d%02d%02d%02d%02d.%02d", ts.Year, ts.Month, ts.Day,
+ ts.Hour, ts.Minute, ts.Second, (modeling.utc_time % 1000) / 10);
+
+ brief.qf = 3;
+ brief.map_id = -1;//GetMapId(CurrExamMapIndex, MapList, MapNum);
+ brief.move = motion.move;
+ brief.speed = ConvertMs2KMh(motion.speed);
+ brief.heading = modeling.yaw;
+ brief.main_ant[0] = modeling.base_point.X;
+ brief.main_ant[1] = modeling.base_point.Y;
+
+ brief.axial[0] = CarModel.axial[0];
+ brief.axial[1] = CarModel.axial[1];
+ brief.left_front_tire[0] = CarModel.left_front_tire[0];
+ brief.left_front_tire[1] = CarModel.left_front_tire[1];
+ brief.right_front_tire[0] = CarModel.right_front_tire[0];
+ brief.right_front_tire[1] = CarModel.right_front_tire[1];
+ brief.left_rear_tire[0] = CarModel.left_rear_tire[0];
+ brief.left_rear_tire[1] = CarModel.left_rear_tire[1];
+ brief.right_rear_tire[0] = CarModel.right_rear_tire[0];
+ brief.right_rear_tire[1] = CarModel.right_rear_tire[1];
+
+ brief.body.assign(CarModel.body.begin(), CarModel.body.end());
+
+ for (auto po: modeling.points) {
+ brief.point.push_back({round(po.X, 4), round(po.Y, 4)});
}
MA_SendCarPosition(&brief);
-
- free(brief.body);
- free(brief.point);
-
- struct RtkTime rtkTime;
- double azimuth = RtkBuffer[index].heading;
-
- rtkTime.YY = RtkBuffer[index].YY;
- rtkTime.MM = RtkBuffer[index].MM;
- rtkTime.DD = RtkBuffer[index].DD;
- rtkTime.hh = RtkBuffer[index].hh;
- rtkTime.mm = RtkBuffer[index].mm;
- rtkTime.ss = RtkBuffer[index].ss;
- rtkTime.mss = RtkBuffer[index].dss;
-
- if (ExamStart) {
- ExecuteExam(speed, move, azimuth, &rtkTime);
- }
-
-// PrintObdInfo(&rtkTime, speed);
- }
}
-static void PrintObdInfo(struct RtkTime *rtkTime, double speed) {
- static struct RtkTime cTime = *rtkTime;
-
- if (TimeGetDiff(rtkTime, &cTime) >= D_SEC(3)) {
- cTime = *rtkTime;
- DEBUG("GEAR %d RPM %d OBD_SPEED %f SPEED %f",
- ReadCarStatus(GEAR) - GEAR_N,
- ReadCarStatus(ENGINE_RPM),
- ((double)ReadCarStatus(OBD_SPEED)) / 10.0,
- speed * 3.6);
- }
-}
-
-static void ExecuteExam(const struct RtkTime* rtkTime)
-{
- {
- static const char *NAME[] = {"OBD_SPEED",
- "ENGINE_RPM",
- "鎸′綅",
- "杞悜鐏�",
- "杩戝厜鐏�",
- "闆剧伅",
- "绀哄粨鐏�",
- "闂伅鎻愮ず",
- "杩滃厜鐏�",
- "瀹夊叏甯�",
- "鍚姩寮曟搸",
- "鍒硅溅",
- "鎵嬪埞",
- "鍓埞杞�",
- "杞﹂棬",
- "缁曡溅涓�",
- "缁曡溅浜�",
- "缁曡溅涓�",
- "缁曡溅鍥�",
- "CAR_STATUS_END"};
-
- static const char *VALUE[] = {
- "鍏抽棴",
- "鍛婅鐏�",
- "宸﹁浆淇″彿",
- "鍙宠浆淇″彿",
- "绀哄粨鐏寒",
- "杩戝厜鐏寒",
- "杩滃厜鐏寒",
- "杩滆繎鍒囨崲",
- "闆剧伅浜�",
- "鎻掑叆",
- "鍦ㄥ惎鍔ㄤ綅",
- "绌烘。",
- "涓�妗�",
- "浜屾。",
- "涓夋。",
- "鍥涙。",
- "浜旀。",
- "鍊掓尅",
- "韪╀笅",
- "闂ㄥ叧闂�",
- "缁曡溅鍙戠敓"
- };
-
-
-
- static int cs[CAR_STATUS_END] = {0};
-
- int cs_temp[CAR_STATUS_END];
-
- for (int i = 0; i < CAR_STATUS_END; ++i) {
-// DEBUG("璇诲彇......");
- cs_temp[i] = ReadCarStatus(i);
-
-// DEBUG("璇诲彇 %d <---- %d", i, cs_temp[i]);
- }
-
- for (int i = 0; i < 2; ++i) {
- if (cs_temp[i] != cs[i]) {
-// DEBUG("杞﹁締鐘舵�� %s = %d", NAME[i], cs_temp[i]);
-
- cs[i] = cs_temp[i];
- }
- }
-
- for (int i = 2; i < CAR_STATUS_END; ++i) {
- if (cs_temp[i] != cs[i]) {
- DEBUG("杞﹁締鐘舵�� %s = %s", NAME[i], VALUE[ cs_temp[i] ]);
-
- cs[i] = cs_temp[i];
-
-// char buff[128];
-// sprintf(buff, "%s锛�%s", NAME[i], VALUE[ cs_temp[i] ]);
-// PlayTTS(buff, NULL);
- }
- }
-
- }
-
- if (ReadCarStatus(ENGINE_RPM) < ENGINE_MIN_ROTATE) {
- if (engineRuning) {
- engineRuning = false;
- if (ExamType == TEST_TYPE_AREA) {
- // 鐔勭伀1娆★紝鎵�10鍒�
- AddExamFault(10210, rtkTime);
- } else {
- AddExamFault(30208, rtkTime);
- }
- }
- } else {
- engineRuning = true;
- }
-
- if (ReadCarStatus(ENGINE_START) == ENGINE_START_ACTIVE) {
- if (!engineStart) {
- DEBUG("妫�娴嬪埌鐐圭伀");
- engineStart = true;
- if (ReadCarStatus(GEAR) != GEAR_N) {
- DEBUG("涓嶅湪绌烘尅鐐圭伀");
- // 涓嶆槸绌烘尅鐐圭伀锛屼笉鍚堟牸
- if (ExamType == TEST_TYPE_AREA)
- AddExamFault(10105, rtkTime);
- else
- AddExamFault(30105, rtkTime);
- }
- AppTimer_delete(EngineStartHold);
- AppTimer_add(EngineStartHold, examParam.hold_start_key_limit_time);
- }
- } else if (engineStart) {
- DEBUG("妫�娴嬪埌鍏抽棴鐐圭伀");
- engineStart = false;
- AppTimer_delete(EngineStartHold);
- }
-
- if (ExamType == TEST_TYPE_ROAD_DUMMY_LIGHT) {
- if (exam_dummy_light == 0) {
-// StartPrepare();
-//// StartDummyLightExam(DummyLightContent, DummyLightContentSize, rtkTime);
-// exam_dummy_light = 1;
-// DEBUG("寮�濮嬩笂杞﹀噯澶�");
-
- exam_dummy_light = 2; // 棰戦棴涓婅溅鍑嗗
- } else if (exam_dummy_light == 2) {
- DEBUG("寮�濮嬬伅鍏夎�冭瘯");
- StartDummyLightExam(DummyLightContent, DummyLightContentSize, rtkTime);
- exam_dummy_light = 3;
- } else if (exam_dummy_light == 3) {
- if (!ExecuteDummyLightExam(rtkTime)) {
- exam_dummy_light = 4;
- // 姹囨姤鐏厜鑰冭瘯缁撴潫
- DEBUG("鐏厜鑰冭瘯缁撴潫");
- InitRoadExam(RoadMap);
- }
- }
- }
-}
-
-static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime)
+static void ExecuteExam(prime_t &prime)
{
static bool rec = false;
static bool handBreakActive = false, handBreakActive2 = false;
static double startCarMoveDistance;
- static int prevMove = 0;
+ static move_status_t prevMove = STOP;
- if (move != 0) {
+ if (prime.pMotion->move != STOP) {
if (ReadCarStatus(SEATBELT) == EJECT_SEATBELT && !reportSeatbeltEject) {
DEBUG("涓嶇郴瀹夊叏甯�");
reportSeatbeltEject = true;
- AddExamFault(ExamType == TEST_TYPE_AREA? 10101: 30101, rtkTime);
+ AddExamFault(ExamType == TEST_TYPE_AREA? 10101: 30101);
}
if (rec) {
if (!handBreakActive2 && ReadOdo() - startCarMoveDistance >= examParam.start_car_limit_distance) {
@@ -970,11 +783,11 @@
if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
DEBUG("Handbreak active move over 10m");
// 鎵嬪埞鎷夎捣鐘舵�佷笅锛岃椹朵簡10绫充互涓婏紝涓嶅悎鏍�
- AddExamFault(40205, rtkTime);
+ AddExamFault(40205);
} else if (handBreakActive) {
// 鎵嬪埞鎷夎捣鐘舵�佷笅锛岃椹朵簡1绫充互涓婏紝鎵�10鍒�
DEBUG("Handbreak active move over 1M");
- AddExamFault(40206, rtkTime);
+ AddExamFault(40206);
}
}
} else if (!handBreakActive && ReadOdo() - startCarMoveDistance >= examParam.open_door_drive_allow_distance && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
@@ -982,254 +795,34 @@
if (ExamType == TEST_TYPE_AREA) {
DEBUG("Handbreak active move over 1M");
- AddExamFault(10107, rtkTime);
+ AddExamFault(10107);
}
}
}
- } else if (!rec || prevMove != 0) { // 璁板綍鍋滆溅鐐�
+ } else if (!rec || prevMove != STOP) { // 璁板綍鍋滆溅鐐�
rec = true;
handBreakActive = handBreakActive2 = false;
startCarMoveDistance = ReadOdo();
}
- prevMove = move;
+ prevMove = prime.pMotion->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);
- }
- } else {
- TestAreaGeneral(AreaMapList, CarModel, CarModelList, speed, move, azimuth, rtkTime);
- }
+ AreaExam(prime);
}
static void EngineStartHold(apptimer_var_t val) {
DEBUG("鐐圭伀瓒呮椂");
if (ReadCarStatus(ENGINE_START) == ENGINE_START_ACTIVE) {
- struct RtkTime rtkTime;
-
- std::unique_lock<std::mutex> lk(clock_mutex);
- rtkTime = rtkClock;
- lk.unlock();
-
// 涓嶅強鏃舵澗寮�鍚姩寮�鍏筹紝鎵�10鍒�
if (ExamType == TEST_TYPE_AREA) {
- AddExamFault(10201, &rtkTime);
+ AddExamFault(10201);
} else if (ExamType != TEST_TYPE_ROAD_CALIBRATE) {
- AddExamFault(40207, &rtkTime);
+ AddExamFault(40207);
}
}
}
-/*************************************************
- * 2娆￠噰鏍风浉宸殑鏃堕棿, a 鏈�杩戠殑锛宐 鍏堝墠鐨�
- * @param a
- * @param b
- * @return ms
- */
-static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b)
-{
- return TimeGetDiff(a->hh, a->mm, a->ss, a->dss*10, b->hh, b->mm, b->ss, b->dss*10);
-}
-
-static bool UpdateCarCoord(double &spd, int &mov, int &idx)
-{
- long tmDiff;
-
- if (CarModel == NULL)
- return false;
-
- if (RtkBufferNum < 2)
- return false;
-
- int p1 = ((RtkBufferIn-1)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE; // 鏈�杩戦噰鏍峰��
- int p2 = ((RtkBufferIn-2)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE; // 鎵惧埌1绉掑墠鐨勯噰鏍峰��
- int pn = 0;
-
- for (pn = 1; pn < RtkBufferNum; ++pn) {
- p2 = ((RtkBufferIn-1-pn)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;
- if ((tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2])) >= D_SEC(1)) break;
- }
-
- if (pn == RtkBufferNum)
- return false;
-
- // 濡傛灉涓�瀹氱殑鏃堕棿閮芥病鏈夋湁鏁堝畾浣嶏紝鍒犻櫎涔嬪墠鐨勫��
- /*tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2]);
-
- DEBUG("tmDiff = %ld, p1 = %d p2 = %d dss = %d dss2 = %d", tmDiff, p1, p2, RtkBuffer[p1].dss, RtkBuffer[p2].dss);
-
- if (tmDiff > D_SEC(5)) {
- if (p1 != 0)
- RtkBuffer[0] = RtkBuffer[p1];
- RtkBufferIn = RtkBufferNum = 1;
- return false;
- }*/
-
- // 璁＄畻杞﹁締杞粨鐐�
- PointF main_ant_coord;
- main_ant_coord.X = RtkBuffer[p1].x;
- main_ant_coord.Y = RtkBuffer[p1].y;
-
- struct RtkTime tm;
-
- tm.YY = RtkBuffer[p1].YY;
- tm.MM = RtkBuffer[p1].MM;
- tm.DD = RtkBuffer[p1].DD;
- tm.hh = RtkBuffer[p1].hh;
- tm.mm = RtkBuffer[p1].mm;
- tm.ss = RtkBuffer[p1].ss;
- tm.mss = RtkBuffer[p1].dss;
-
- UpdateCarBodyCoord(&tm, RtkBuffer[p1].heading, RtkBuffer[p1].pitch, RtkBuffer[p1].roll, main_ant_coord, CarModel);
-
- car_model *newModel = (car_model *)malloc(sizeof(car_model));
-
- newModel->tm = CarModel->tm;
- newModel->basePoint = CarModel->basePoint;
- newModel->axial[0] = CarModel->axial[0];
- newModel->axial[1] = CarModel->axial[1];
- newModel->left_front_tire[0] = CarModel->left_front_tire[0];
- newModel->left_front_tire[1] = CarModel->left_front_tire[1];
- newModel->right_front_tire[0] = CarModel->right_front_tire[0];
- newModel->right_front_tire[1] = CarModel->right_front_tire[1];
- newModel->left_rear_tire[0] = CarModel->left_rear_tire[0];
- newModel->left_rear_tire[1] = CarModel->left_rear_tire[1];
- newModel->right_rear_tire[0] = CarModel->right_rear_tire[0];
- newModel->right_rear_tire[1] = CarModel->right_rear_tire[1];
- newModel->bodyNum = CarModel->bodyNum;
- newModel->body = (int *) malloc(sizeof(int) * newModel->bodyNum);
- for (int i = 0; i < newModel->bodyNum; ++i) {
- newModel->body[i] = CarModel->body[i];
- }
- newModel->pointNum = CarModel->pointNum;
- newModel->carXY = (PointF *) malloc(sizeof(PointF) * newModel->pointNum);
- for (int i = 0; i < newModel->pointNum; ++i) {
- newModel->carXY[i] = CarModel->carXY[i];
- }
- newModel->carDesc = NULL;
- newModel->antPitch = CarModel->antPitch;
- newModel->yaw = CarModel->yaw;
- newModel->pitch = CarModel->pitch;
-
- CarModelList.push_front(newModel);
-
- while (CarModelList.size() > 100) {
- car_model *ptr = CarModelList.back();
-
- if (ptr->body != NULL)
- free(ptr->body);
- if (ptr->carXY != NULL)
- free(ptr->carXY);
- if (ptr->carDesc != NULL)
- free(ptr->carDesc);
-
- free(ptr);
-
- CarModelList.pop_back();
- }
-
- // 璁＄畻閫熷害(绫�/绉�)銆佸墠杩涘悗閫�
- double speed = sqrt(pow(RtkBuffer[p1].x - RtkBuffer[p2].x, 2) + pow(RtkBuffer[p1].y - RtkBuffer[p2].y, 2)) * 1000 /
- (double)(tmDiff);
-
-// DEBUG("浣嶇Щ %f 鏃堕棿 %ld 閫熷害 %f", sqrt(pow(RtkBuffer[p1].x - RtkBuffer[p2].x, 2) + pow(RtkBuffer[p1].y - RtkBuffer[p2].y, 2)), tmDiff, speed);
-// DEBUG("%d %d %f, %f - %d %d %f, %f", RtkBuffer[p1].ss, RtkBuffer[p1].dss, RtkBuffer[p1].x, RtkBuffer[p1].y, RtkBuffer[p2].ss, RtkBuffer[p2].dss, RtkBuffer[p2].x, RtkBuffer[p2].y);
-
- int move = 0;
- double deg = 0.0;
-
- if (speed < 0.05) {
- // 鍋滆溅
- move = 0;
- } else {
- // 鍒ゆ柇鍓嶈繘杩樻槸鍚庨��
- if (fabs(RtkBuffer[p1].y - RtkBuffer[p2].y) <= GLB_EPSILON) {
- if (RtkBuffer[p1].x > RtkBuffer[p2].x) {
- deg = 90;
- } else {
- deg = 270;
- }
- } else if (fabs(RtkBuffer[p1].x - RtkBuffer[p2].x) <= GLB_EPSILON) {
- if (RtkBuffer[p1].y > RtkBuffer[p2].y) {
- deg = 0;
- } else {
- deg = 180;
- }
- } else {
- deg = atan(fabs(RtkBuffer[p1].x - RtkBuffer[p2].x) /
- fabs(RtkBuffer[p1].y - RtkBuffer[p2].y));
-
- deg = toDegree(deg);
-
- if (RtkBuffer[p1].x > RtkBuffer[p2].x &&
- RtkBuffer[p1].y > RtkBuffer[p2].y) {
-
- } else if (RtkBuffer[p1].x < RtkBuffer[p2].x &&
- RtkBuffer[p1].y > RtkBuffer[p2].y) {
- deg = 360 - deg;
- } else if (RtkBuffer[p1].x < RtkBuffer[p2].x &&
- RtkBuffer[p1].y < RtkBuffer[p2].y) {
- deg = 180 + deg;
- } else if (RtkBuffer[p1].x > RtkBuffer[p2].x &&
- RtkBuffer[p1].y < RtkBuffer[p2].y) {
- deg = 180 - deg;
- }
- }
-
- deg = fabs(RtkBuffer[p1].heading - deg);
- if (deg > 180) {
- deg = 360 - deg;
- }
- if (deg < 90) {
- // 鍓嶈繘
- move = 1;
- } else {
- // 鍚庨��
- move = -1;
- }
- }
-
- spd = speed;
- mov = move;
- idx = p1;
-// DEBUG("tmDiff = %ld speed = %f m/Sec move = %d", tmDiff, speed, move);
-
- return true;
-}
-
-static int currRoad = -1, currCrossing = -1;
-
-void RoadChange(int road, int status)
-{
- struct roadStatusBrief brief;
-
- brief.road_id = road;
- brief.status = status;
-
- MA_SendRoadStatus(&brief);
-
- currRoad = (status == 1? road : -1);
-
- DEBUG("鎶ュ憡闀垮畼 杩涘嚭璺 road %d status %d", road, status);
-}
-
-void CrossingChange(int road, int crossing, int status)
-{
- struct crossingStatusBrief brief;
-
- brief.road_id = road;
- brief.crossing_index = crossing;
- brief.status = status;
-
- MA_SendCrossingStatus(&brief);
-
- currCrossing = (status == 1? crossing : -1);
-
- DEBUG("鎶ュ憡闀垮畼 杩涘嚭璺彛 road %d crossing %d status %d", road, crossing, status);
-}
-
-void AddExamFault(int wrong, const struct RtkTime *rtkTime)
+void AddExamFault(int wrong)
{
struct ExamFault fault;
@@ -1237,8 +830,8 @@
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);
+
+ strcpy(fault.utc, StringUtil::FormatUTCTime(AppTimer_GetGmtTickCount()).c_str());
// if (ExamType != TEST_TYPE_AREA) {
// wrong += 1000;
@@ -1253,35 +846,6 @@
MA_SendExamWrong(ExamFaultList);
ExamFaultList.clear();
-}
-
-void MasterInqRoadStatus(void)
-{
- struct roadStatusBrief brief;
- struct crossingStatusBrief brief2;
-
- brief2.road_id = brief.road_id = currRoad;
-
- if (currRoad >= 0) {
- brief.status = 1;
-
- if (currCrossing >= 0) {
- brief2.crossing_index = currCrossing;
- brief2.status = 1;
- } else {
- brief2.crossing_index = -1;
- brief2.status = 0;
- }
- }
- else {
- brief.status = 0;
- brief2.crossing_index = -1;
- brief2.status = 0;
- }
-
- MA_SendRoadStatus(&brief);
-
- MA_SendCrossingStatus(&brief2);
}
/***************************************************************
@@ -1300,46 +864,47 @@
* @param azimuth
* @param coord
*/
-static void UpdateCarBodyCoord(struct RtkTime *rtkTime, double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel)
+static void CalcBodyModeling(modeling_t &car, car_model_t &carModel, rtk_info_t &rtk)
{
- // 淇话瑙掍慨姝�
-// DEBUG("淇话瑙� %f", pitch);
- carModel->yaw = azimuth;
- carModel->pitch = pitch;
- carModel->tm = *rtkTime;
-
- pitch = pitch - carModel->antPitch;
+ car.utc_time = rtk.utc_time;
+ car.yaw = rtk.heading;
+ car.pitch = rtk.pitch;
+ car.roll = rtk.roll;
+// 淇话瑙掍慨姝�
+ double pitch = rtk.pitch - carModel.antPitch;
+ double azimuth = rtk.heading;
// DEBUG("yaw = %f 淇淇话瑙� %f", azimuth, pitch);
// 涓诲ぉ绾挎姇褰变慨姝�
- carModel->basePoint.X = main_ant.X + fabs(carModel->antHeight - carModel->groundHeight) * sin(toRadians(pitch)) * sin(toRadians(azimuth));
- carModel->basePoint.Y = main_ant.Y + fabs(carModel->antHeight - carModel->groundHeight) * sin(toRadians(pitch)) * cos(toRadians(azimuth));
+ car.base_point.X = rtk.x + fabs(carModel.antHeight - carModel.groundHeight) * sin(toRadians(pitch)) * sin(toRadians(azimuth));
+ car.base_point.Y = rtk.y + fabs(carModel.antHeight - carModel.groundHeight) * sin(toRadians(pitch)) * cos(toRadians(azimuth));
- for (int i = 0; i < carModel->pointNum; ++i) {
- double qrx = carModel->carDesc[i].distance * sin(toRadians(carModel->carDesc[i].angle));
- double qry =
- carModel->carDesc[i].distance * cos(toRadians(carModel->carDesc[i].angle)) *
+ // 棣栨璁＄畻
+ if (car.points.size() != carModel.carDesc.size()) {
+ car.points.resize(carModel.carDesc.size());
+ }
+
+ for (int i = 0; i < carModel.carDesc.size(); ++i) {
+ double qrx = carModel.carDesc[i].distance * sin(toRadians(carModel.carDesc[i].angle));
+ double qry = carModel.carDesc[i].distance * cos(toRadians(carModel.carDesc[i].angle)) *
cos(toRadians(pitch));
double projectDistance = sqrt(pow(qrx, 2) + pow(qry, 2));
double projectAngle = toDegree(acos(qry / projectDistance));
- if (carModel->carDesc[i].angle > 180) {
+ if (carModel.carDesc[i].angle > 180) {
projectAngle = 360 - projectAngle;
}
// double tx = projectDistance*sin(toRadians(azimuth));
// double ty = projectDistance*cos(toRadians(azimuth));
- carModel->carXY[i].X =
- projectDistance * sin(toRadians(azimuth)) * cos(toRadians(projectAngle)) -
- projectDistance * cos(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
- carModel->basePoint.X;
- carModel->carXY[i].Y =
- projectDistance * sin(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
- projectDistance * cos(toRadians(azimuth)) * cos(toRadians(projectAngle)) +
- carModel->basePoint.Y;
-
+ car.points[i].X = projectDistance * sin(toRadians(azimuth)) * cos(toRadians(projectAngle)) -
+ projectDistance * cos(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
+ car.base_point.X;
+ car.points[i].Y = projectDistance * sin(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
+ projectDistance * cos(toRadians(azimuth)) * cos(toRadians(projectAngle)) +
+ car.base_point.Y;
// DEBUG("<%d>. 鏍囪窛 %f 鏍囪 %f X = %f Y = %f", i, carModel->carDesc[i].distance, carModel->carDesc[i].angle,
// carModel->carXY[i].X, carModel->carXY[i].Y);
}
@@ -1359,21 +924,7 @@
void SensorXChanged(uint16_t id, int value)
{
- handlePrepare(id, value);
- handleLigthExam(id, value);
+// handlePrepare(id, value);
+// handleLigthExam(id, value);
}
-void PrepareOver(int res)
-{
- DEBUG("涓婅溅鍑嗗缁撴潫 %d", res);
-
- if (res != 0) {
- struct RtkTime rtkTime;
-
- std::unique_lock<std::mutex> lk(clock_mutex);
- rtkTime = rtkClock;
- lk.unlock();
- AddExamFault(40101, &rtkTime);
- }
- exam_dummy_light = 2;
-}
diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h
index 878dafd..f773daa 100644
--- a/lib/src/main/cpp/driver_test.h
+++ b/lib/src/main/cpp/driver_test.h
@@ -5,13 +5,12 @@
#ifndef RTKDRIVERTEST_DRIVER_TEST_H
#define RTKDRIVERTEST_DRIVER_TEST_H
-#include "test_common/Geometry.h"
-#include "rtk_module/rtk.h"
-#include "test_items2/dummy_light.h"
-
#include <vector>
#include <string>
#include <list>
+#include "test_common/Geometry.h"
+#include "rtk_module/rtk.h"
+#include "map.h"
#define EXAM_AREA_NONE 0
#define EXAM_AREA_START 1
@@ -23,68 +22,78 @@
#define AXIAL_FRONT 0
#define AXIAL_REAR 1
-#define MAP_TYPE_PARK_BUTTOM 1
-#define MAP_TYPE_STOP_START 2
-#define MAP_TYPE_PART_EDGE 3
-#define MAP_TYPE_CURVE 4
-#define MAP_TYPE_TURN_90 5
-
-using namespace std;
-
-struct car_desc_ {
- double distance; // 璺濈涓诲ぉ绾跨殑璺濈
- double angle; // 浠庝腑杞寸嚎閫嗘椂閽堝舰鎴愮殑瑙掑害
-};
-
-struct RtkTime {
- int YY; // 2浣嶆暟
- int MM;
- int DD;
- int hh;
- int mm;
- int ss;
- int mss;
-};
+typedef enum {
+ MAP_TYPE_NONE,
+ MAP_TYPE_PARK_BUTTOM,
+ MAP_TYPE_UPHILL,
+ MAP_TYPE_PARK_EDGE,
+ MAP_TYPE_CURVE,
+ MAP_TYPE_TURN_90
+} map_type_t;
typedef struct {
- struct RtkTime tm;
+ double distance; // 璺濈涓诲ぉ绾跨殑璺濈
+ double angle; // 浠庝腑杞寸嚎閫嗘椂閽堝舰鎴愮殑瑙掑害
+} car_desc_t;
+
+typedef struct {
+// struct RtkTime tm;
PointF basePoint; // 鐜板満娴嬮噺鐨勪富澶╃嚎鍧愭爣
int axial[2];
int left_front_tire[2];
int right_front_tire[2];
int left_rear_tire[2];
int right_rear_tire[2];
- int bodyNum;
- int *body;
- int pointNum;
+// int bodyNum;
+ std::vector<int> body;
+// int pointNum;
double antPitch; // 鍓嶅悗澶╃嚎鍥犲畨瑁呬綅缃箣鍥烘湁pitch瑙�
double antHeight; // 杞﹂《澶╃嚎楂樼▼
double groundHeight; // 澶у湴楂樼▼
double yaw;
double pitch;
- struct car_desc_ *carDesc; // 缁忕幇鍦烘祴閲忕偣璁＄畻鍚庣殑鍊�
- PointF *carXY; // 璁＄畻鍚庣殑杞﹁韩鍚勭偣鍧愭爣
-} car_model;
-
-// 鎬绘暟
-// 涓酱绾垮墠鐐�,id 0锛屼腑杞寸嚎鍚庣偣id
-// 宸﹀寘鍥达紝 鍙冲寘鍥�
-// 宸﹀墠杞紝澶栧唴
-typedef struct {
- int body_num;
- int body[64];
- int front_left_tire[2];
- int front_right_tire[2];
- int rear_left_tire[2];
- int rear_right_tire[2];
-} carModelDesc_t;
+ std::vector<car_desc_t> carDesc;
+// struct car_desc_ *carDesc; // 缁忕幇鍦烘祴閲忕偣璁＄畻鍚庣殑鍊�
+// PointF *carXY; // 璁＄畻鍚庣殑杞﹁韩鍚勭偣鍧愭爣
+} car_model_t;
typedef struct {
- uint32_t uptime;
- int point_num;
- PointF *points;
- carModelDesc_t *desc;
-} car_model_cache_t;
+ int64_t utc_time = 0;
+ double yaw;
+ double pitch;
+ double roll;
+ PointF base_point;
+ std::vector<PointF> points;
+} modeling_t;
+
+typedef enum {
+ BACKWARD = -1,
+ STOP = 0,
+ FORWARD = 1
+} move_status_t;
+
+typedef struct {
+ int64_t timestamp = 0;
+ double speed = 0;
+ move_status_t move = STOP;
+} motion_t;
+
+typedef struct {
+ map_type_t type;
+ int map_idx; // 瀵瑰簲绉戠洰鍦板浘闆嗗悎涓殑搴忓彿
+} map_t;
+
+typedef struct {
+ bool examing = false;
+ int curr_modeling_index = -1;
+ int prev_modeling_index = -1;
+ area_map_t *pMap;
+ car_model_t *pModel;
+ modeling_t *pModeling;
+ motion_t *pMotion;
+ map_t curr_exam_map;
+ map_t arriving_map;
+} prime_t;
struct ExamFault {
int sn;
@@ -236,7 +245,7 @@
double road_slide_red_distance;
int road_total_distance;
int road_max_speed; // Km per hour
- vector<vector<int>> gear_speed_table;
+ std::vector<std::vector<int>> gear_speed_table;
int gear_n_allow_time; // Sec
int same_gear_min_time; // Sec, x绉掑唴锛屼笉鍏佽N->X->N->X缃悓涓�鎸′綅
int gear_speed_error_cumulative_time; // Sec
@@ -248,69 +257,67 @@
int start_car_limit_distance;
double open_door_drive_allow_distance;
- string prepare_tts;
- string touch_leftfront_tts;
- string touch_leftrear_tts;
- string touch_rightfront_tts;
- string touch_rightrear_tts;
- string start_engine_tts;
+ std::string prepare_tts;
+ std::string touch_leftfront_tts;
+ std::string touch_leftrear_tts;
+ std::string touch_rightfront_tts;
+ std::string touch_rightrear_tts;
+ std::string start_engine_tts;
- string start_car_begin_tts;
- string start_car_end_tts;
+ std::string start_car_begin_tts;
+ std::string start_car_end_tts;
int change_lane_limit_distance;
- string change_lane_begin_tts;
- string change_lane_end_tts;
+ std::string change_lane_begin_tts;
+ std::string change_lane_end_tts;
int shift_limit_distance;
int shift_hold_time; // Sec
- string shift_begin_tts;
- string shift_end_tts;
- string shift_up_tts;
- string shift_down_tts;
+ std::string shift_begin_tts;
+ std::string shift_end_tts;
+ std::string shift_up_tts;
+ std::string shift_down_tts;
- string straight_begin_tts;
- string straight_end_tts;
+ std::string straight_begin_tts;
+ std::string straight_end_tts;
int straight_limit_distance;
double straight_max_offset;
int overtake_limit_distance;
- string overtake_begin_tts;
- string overtake_end_tts;
+ std::string overtake_begin_tts;
+ std::string overtake_end_tts;
int stop_car_limit_distance;
int stop_car_open_door_allow_time;
double stop_car_edge_red_distance;
double stop_car_edge_yellow_distance;
- string stop_car_begin_tts;
- string stop_car_end_tts;
+ std::string stop_car_begin_tts;
+ std::string stop_car_end_tts;
double crossing_stop_valid_distance;
int cross_school_max_speed;
int crossing_break_valid_distance;
- string crossing_go_straight_tts;
- string crossing_turn_left_tts;
- string crossing_turn_right_tts;
- string crossing_turn_back_tts;
- string crossing_turn_unknown_tts;
+ std::string crossing_go_straight_tts;
+ std::string crossing_turn_left_tts;
+ std::string crossing_turn_right_tts;
+ std::string crossing_turn_back_tts;
+ std::string crossing_turn_unknown_tts;
} exam_param_t;
extern exam_param_t examParam;
-
-typedef vector<struct area_exam_map> LIST_AREA_MAP;
-
-typedef list<car_model *> LIST_CAR_MODEL;
void DriverTestInit(void);
void ReadDriverExamPrimer(void);
void ClearAreaMap(void);
void AddAreaMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2);
-
+void AddCurveMap(curve_map_t &map);
+void AddParkButtonMap(park_button_map_t &map);
+void AddParkEdgeMap(park_edge_map_t &map);
void CleanRoadMap(void);
-void SetRoadMap(road_exam_map &map, vector<scheme_t> &scheme);
-void SetRoadExamScheme(vector<scheme_t> &scheme);
+void SetRoadMap(road_exam_map &map, std::vector<scheme_t> &scheme);
+void SetRoadExamScheme(std::vector<scheme_t> &scheme);
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
@@ -318,16 +325,14 @@
void StartDriverExam(int start, int type);
-void UpdateRTKInfo(const rtk_info *s);
-void AddExamFault(int wrong, const struct RtkTime *rtkTime);
+void UpdateRTKInfo(const rtk_info_t *s);
+void AddExamFault(int wrong);
void RoadChange(int road, int status);
void CrossingChange(int road, int crossing, int status);
void SystemShutdown(int event, int timeout);
void SetDummyLightExam(int n, struct dummy_light_exam *cfg);
-
-uint32_t TimeGetDiff(const struct RtkTime *rtkTime1, const struct RtkTime *rtkTime2);
void MasterInqRoadStatus(void);
diff --git a/lib/src/main/cpp/map.h b/lib/src/main/cpp/map.h
new file mode 100644
index 0000000..6b40fec
--- /dev/null
+++ b/lib/src/main/cpp/map.h
@@ -0,0 +1,66 @@
+//
+// Created by YY on 2022/12/28.
+//
+
+#ifndef MYAPPLICATION3_MAP_H
+#define MYAPPLICATION3_MAP_H
+
+#include <string>
+#include <vector>
+#include "test_common/Geometry.h"
+
+typedef struct {
+ int id;
+ std::string name;
+ double line_width;
+ std::vector<PointF> map;
+} park_button_map_t;
+
+typedef struct {
+ int id;
+ std::string name;
+ double line_width;
+ std::vector<PointF> map;
+} park_edge_map_t;
+
+typedef struct {
+ int id;
+ std::string name;
+ double line_width;
+ std::vector<PointF> map;
+} turn_a90_map_t;
+
+typedef struct {
+ int id;
+ std::string name;
+ double line_width;
+ std::vector<PointF> map;
+} uphill_map_t;
+
+typedef struct {
+ int id;
+ std::string name;
+ double line_width;
+ PointF left_start_point;
+ PointF right_start_point;
+ PointF left_end_point;
+ PointF right_end_point;
+ PointF front_half_big_circle_centre;
+ double front_half_big_circle_radius;
+ PointF front_half_small_circle_centre;
+ double front_half_small_circle_radius;
+ PointF back_half_big_circle_centre;
+ double back_half_big_circle_radius;
+ PointF back_half_small_circle_centre;
+ double back_half_small_circle_radius;
+} curve_map_t;
+
+typedef struct {
+ std::vector<curve_map_t> curve_map;
+ std::vector<park_button_map_t> park_button_map;
+ std::vector<park_edge_map_t> park_edge_map;
+ std::vector<turn_a90_map_t> turn_a90_map;
+ std::vector<uphill_map_t> uphill_map;
+} area_map_t;
+
+#endif //MYAPPLICATION3_MAP_H
diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp
index 18a4d2f..486fa55 100644
--- a/lib/src/main/cpp/master/comm_if.cpp
+++ b/lib/src/main/cpp/master/comm_if.cpp
@@ -16,6 +16,7 @@
#include "../jni_log.h"
#include "../mcu/mcu_if.h"
#include "../test_common/car_sensor.h"
+#include "../map.h"
#include <string>
#include <iostream>
@@ -146,6 +147,31 @@
DEBUG("鍙戦�佸け璐�");
usleep(500000);
sem_post(&sem_msg_income);
+ }
+ }
+ }
+}
+
+static void LoadPoint(PointF &p, const rapidjson::Value &v)
+{
+ if (v.IsArray() && v.Size() >= 2) {
+ p.X = v[0].GetDouble();
+ p.Y = v[1].GetDouble();
+ }
+}
+
+static void LoadPoints(vector<PointF> &ps, const rapidjson::Value &v)
+{
+ if (v.IsArray()) {
+ PointF point;
+ for (int i = 0; i < v.Size();) {
+ if (v.Size() - i >= 2) {
+ point.X = v[i].GetDouble();
+ point.Y = v[i+1].GetDouble();
+ ps.push_back(point);
+ i += 2;
+ } else {
+ break;
}
}
}
@@ -420,7 +446,7 @@
writer.Double(brief->coord_y);
writer.Key("coord_y_dir");
writer.String(b);
- writer.Key("heading");
+ writer.Key("yaw");
writer.Double(brief->heading);
writer.Key("pitch");
writer.Double(brief->pitch);
@@ -556,7 +582,7 @@
writer.Int(brief->move);
writer.Key("speed");
writer.Double(brief->speed);
- writer.Key("heading");
+ writer.Key("yaw");
writer.Double(brief->heading);
writer.Key("main_ant");
@@ -597,16 +623,16 @@
writer.Key("body");
writer.StartArray();
- for (int i = 0; i < brief->bodyNum; ++i) {
+ for (int i = 0; i < brief->body.size(); ++i) {
writer.Int(brief->body[i]);
}
writer.EndArray();
writer.Key("point");
writer.StartArray();
- for (int i = 0; i < brief->pointNum; ++i) {
- writer.Double(brief->point[i*2]);
- writer.Double(brief->point[i*2+1]);
+ for (int i = 0; i < brief->point.size(); ++i) {
+ writer.Double(brief->point[i][0]);
+ writer.Double(brief->point[i][1]);
}
writer.EndArray();
writer.EndObject();
@@ -1673,63 +1699,136 @@
if (a.IsArray()) {
for (Value::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr) {
// a Map
- int id, type, pointNum = 0, point2Num = 0;
- double (*map)[2] = NULL, (*map2)[2] = NULL;
+ string name;
+ int id = -1, type = -1;
+ //int id, type, pointNum = 0, point2Num = 0;
+ //double (*map)[2] = NULL, (*map2)[2] = NULL;
if (itr->IsObject()) {
if (itr->HasMember("id")) {
const Value &s = (*itr)["id"];
id = s.GetInt();
}
+ if (itr->HasMember("name")) {
+ const Value &s = (*itr)["id"];
+ name = s.GetString();
+ }
if (itr->HasMember("item")) {
const Value &s = (*itr)["item"];
type = s.GetInt();
- }
- if (itr->HasMember("point")) {
- const Value &s = (*itr)["point"];
- int map_index = 0;
- for (Value::ConstValueIterator itr2 = s.Begin();
- itr2 != s.End(); ++itr2, ++map_index) {
- // 鏇茬嚎椹鹃┒鏈�2缁�
- const Value &s2 = (*itr2)["x-y"];
-
- if (map_index == 0) {
- int i = 0, j = 0;
- pointNum = s2.Size() / 2;
- map = (double (*)[2]) new double[pointNum][2];
-// map = (double (*)[2]) malloc(pointNum * 2 * sizeof(double));
-
- for (Value::ConstValueIterator itr3 = s2.Begin();
- itr3 != s2.End(); ++itr3) {
- map[i][j] = (*itr3).GetDouble();
- if (++j == 2) {
- j = 0;
- i++;
- }
+ switch (type) {
+ case MAP_TYPE_CURVE: {
+ curve_map_t map;
+ map.id = id;
+ if (itr->HasMember("left_start_point")) {
+ const Value &s2 = (*itr)["left_start_point"];
+ LoadPoint(map.left_start_point, s2);
}
- } else if (map_index == 1) {
- int i = 0, j = 0;
- point2Num = s2.Size() / 2;
- map2 = (double (*)[2]) new double[s2.Size()][2];
-// map2 = (double (*)[2]) malloc(point2Num * 2 * sizeof(double));
-
- for (Value::ConstValueIterator itr3 = s2.Begin();
- itr3 != s2.End(); ++itr3) {
- map2[i][j] = (*itr3).GetDouble();
- if (++j == 2) {
- j = 0;
- i++;
- }
+ if (itr->HasMember("right_start_point")) {
+ const Value &s2 = (*itr)["right_start_point"];
+ LoadPoint(map.right_start_point, s2);
}
+ if (itr->HasMember("left_end_point")) {
+ const Value &s2 = (*itr)["left_end_point"];
+ LoadPoint(map.left_end_point, s2);
+ }
+ if (itr->HasMember("right_end_point")) {
+ const Value &s2 = (*itr)["right_end_point"];
+ LoadPoint(map.right_end_point, s2);
+ }
+ AddCurveMap(map);
+ break;
}
+ case MAP_TYPE_PARK_BUTTOM: {
+ park_button_map_t map;
+
+ map.id = id;
+ if (itr->HasMember("point") && (*itr)["point"].IsArray()) {
+ const Value &arr = (*itr)["point"];
+ LoadPoints(map.map, arr);
+ }
+ AddParkButtonMap(map);
+ break;
+ }
+ case MAP_TYPE_PARK_EDGE: {
+ park_edge_map_t map;
+ map.id = id;
+ if (itr->HasMember("point") && (*itr)["point"].IsArray()) {
+ const Value &arr = (*itr)["point"];
+ LoadPoints(map.map, arr);
+ }
+ AddParkEdgeMap(map);
+ break;
+ }
+ case MAP_TYPE_UPHILL: {
+ uphill_map_t map;
+ map.id = id;
+ if (itr->HasMember("point") && (*itr)["point"].IsArray()) {
+ const Value &arr = (*itr)["point"];
+ LoadPoints(map.map, arr);
+ }
+ break;
+ }
+ case MAP_TYPE_TURN_90: {
+ turn_a90_map_t map;
+ map.id = id;
+ if (itr->HasMember("point") && (*itr)["point"].IsArray()) {
+ const Value &arr = (*itr)["point"];
+ LoadPoints(map.map, arr);
+ }
+ break;
+ }
+ default:
+ break;
}
}
- AddAreaMap(id, type, map, pointNum, map2, point2Num);
+// if (itr->HasMember("point")) {
+// const Value &s = (*itr)["point"];
+// int map_index = 0;
+//
+// for (Value::ConstValueIterator itr2 = s.Begin();
+// itr2 != s.End(); ++itr2, ++map_index) {
+// // 鏇茬嚎椹鹃┒鏈�2缁�
+// const Value &s2 = (*itr2)["x-y"];
+//
+// if (map_index == 0) {
+// int i = 0, j = 0;
+// pointNum = s2.Size() / 2;
+// map = (double (*)[2]) new double[pointNum][2];
+//// map = (double (*)[2]) malloc(pointNum * 2 * sizeof(double));
+//
+// for (Value::ConstValueIterator itr3 = s2.Begin();
+// itr3 != s2.End(); ++itr3) {
+// map[i][j] = (*itr3).GetDouble();
+// if (++j == 2) {
+// j = 0;
+// i++;
+// }
+// }
+// } else if (map_index == 1) {
+// int i = 0, j = 0;
+// point2Num = s2.Size() / 2;
+// map2 = (double (*)[2]) new double[s2.Size()][2];
+//// map2 = (double (*)[2]) malloc(point2Num * 2 * sizeof(double));
+//
+// for (Value::ConstValueIterator itr3 = s2.Begin();
+// itr3 != s2.End(); ++itr3) {
+// map2[i][j] = (*itr3).GetDouble();
+// if (++j == 2) {
+// j = 0;
+// i++;
+// }
+// }
+// }
+// }
+// }
- if (map) delete[]map;
- if (map2) delete[]map2;
+// AddAreaMap(id, type, map, pointNum, map2, point2Num);
+//
+// if (map) delete[]map;
+// if (map2) delete[]map2;
}
}
}
@@ -1968,7 +2067,7 @@
Document doc;
doc.Parse(value);
if (!doc.HasParseError()) {
- DEBUG("瑙ｆ瀽鐏厜閰嶇疆...");
+ /*DEBUG("瑙ｆ瀽鐏厜閰嶇疆...");
if (doc.HasMember("exam")) {
const Value& s = doc["exam"];
s.GetInt();
@@ -2022,12 +2121,11 @@
delete []content;
}
- }
+ }*/
}
break;
}
case ID_MS_INQ_ROAD_CROSSING: {
- MasterInqRoadStatus();
break;
}
case ID_MS_BLUETOOTH_NAME: {
diff --git a/lib/src/main/cpp/master/comm_if.h b/lib/src/main/cpp/master/comm_if.h
index 5d02cbd..8a13a4d 100644
--- a/lib/src/main/cpp/master/comm_if.h
+++ b/lib/src/main/cpp/master/comm_if.h
@@ -6,6 +6,7 @@
#define MYAPPLICATION2_COMM_IF_H
#include <cstdint>
+#include <vector>
#include "../driver_test.h"
struct mcuBrief {
@@ -106,10 +107,8 @@
int right_front_tire[2];
int left_rear_tire[2];
int right_rear_tire[2];
- int *body;
- int bodyNum;
- double *point;
- int pointNum;
+ std::vector<int> body;
+ std::vector<std::vector<double>> point;
};
struct roadStatusBrief {
diff --git a/lib/src/main/cpp/mcu/ada.cpp b/lib/src/main/cpp/mcu/ada.cpp
index 4ab6d02..6a2dbf4 100644
--- a/lib/src/main/cpp/mcu/ada.cpp
+++ b/lib/src/main/cpp/mcu/ada.cpp
@@ -23,7 +23,6 @@
static void AdaCommandEntry(uint16_t id, const uint8_t *data, int length)
{
- DEBUG("McuCommandEntry 0x%02X", id);
switch (id) {
case ID_ADA_INFO_RESP: {
DEBUG("ID_ADA_INFO_RESP");
@@ -120,7 +119,22 @@
void InitAda(void)
{
// TODO
- std::thread(AdaRun).detach();
+ static struct serial_config serialConfig;
+ strcpy(serialConfig.name, "/dev/ttyCH341USB4");
+ serialConfig.baud = 115200;
+ serialConfig.data_bit = 8;
+ serialConfig.verify_bit = 'N';
+ serialConfig.stop_bit = 1;
+ serialConfig.flow_ctrl = 0;
+
+ std::thread([&] {
+ while (true) {
+ std::thread t(UartThread1, &serialConfig);
+ t.join();
+ std::this_thread::sleep_for(std::chrono::seconds(3));
+ }
+ }).detach();
+
}
static void SendDfuFile(int fileLen, int sentLen, int blockLen, const uint8_t *data) {
diff --git a/lib/src/main/cpp/mcu/ahp.cpp b/lib/src/main/cpp/mcu/ahp.cpp
index a14afb6..185f8df 100644
--- a/lib/src/main/cpp/mcu/ahp.cpp
+++ b/lib/src/main/cpp/mcu/ahp.cpp
@@ -36,7 +36,7 @@
}
case ID_GNSS_DATA: {
-// DEBUG("ID_GNSS_DATA %d", length);
+ DEBUG("ID_GNSS_DATA %d", length);
memcpy(RxBuf + RxBufLen, data, length);
RxBufLen += length;
@@ -113,8 +113,9 @@
pCls = nullptr;
}
-static void AhpRun(void)
+void InitAhp(void)
{
+ // TODO
static struct serial_config serialConfig;
strcpy(serialConfig.name, "/dev/ttyCH341USB3");
@@ -124,17 +125,13 @@
serialConfig.stop_bit = 1;
serialConfig.flow_ctrl = 0;
- while (true) {
- std::thread t(UartThread, &serialConfig);
- t.join();
- std::this_thread::sleep_for(std::chrono::seconds(3));
- }
-}
-
-void InitAhp(void)
-{
- // TODO
- std::thread(AhpRun).detach();
+ std::thread([&] {
+ while (true) {
+ std::thread t(UartThread, &serialConfig);
+ t.join();
+ std::this_thread::sleep_for(std::chrono::seconds(3));
+ }
+ }).detach();
}
static void SendDfuFile(int fileLen, int sentLen, int blockLen, const uint8_t *data) {
diff --git a/lib/src/main/cpp/native-lib.cpp b/lib/src/main/cpp/native-lib.cpp
index c3e60c6..90f370d 100644
--- a/lib/src/main/cpp/native-lib.cpp
+++ b/lib/src/main/cpp/native-lib.cpp
@@ -419,7 +419,7 @@
static void startNative(JNIEnv *env, jobject thiz, jboolean ayDevice) {
// TODO: implement startNative()
// 淇濆瓨鍏ㄥ眬JVM浠ヤ究鍦ㄥ瓙绾跨▼涓娇鐢�
- DEBUG("鍚姩Native");
+ DEBUG("鍚姩Native 閫昏緫cpu鏁伴噺 = %d", std::thread::hardware_concurrency());
// 涓嶈兘鐩存帴璧嬪��(g_obj = ojb)
sg_obj = env->NewGlobalRef(thiz);
diff --git a/lib/src/main/cpp/rtk_module/rtk.cpp b/lib/src/main/cpp/rtk_module/rtk.cpp
index 88adfa5..de85611 100644
--- a/lib/src/main/cpp/rtk_module/rtk.cpp
+++ b/lib/src/main/cpp/rtk_module/rtk.cpp
@@ -21,6 +21,7 @@
#include "virtual_rtk.h"
#include "../mcu/mcu_if.h"
#include "../mcu/ahp.h"
+#include "../utils/xconvert.h"
#define DEBUG(fmt, args...) LOGD("<rtk> <%s>: " fmt, __func__, ##args)
@@ -40,9 +41,25 @@
static gpsStatus_t gpsStatus;
static char rtkModel[32] = {0};
-static rtk_info CurrRTKInfo;
static bool needSetPjk = false;
static int lostCnt;
+
+// 缁勫悎PJK鍜孴RA鐨勬秷鎭唴瀹�
+static struct {
+ int year;
+ int month;
+ int day;
+ int hour;
+ int min;
+ int sec;
+ int millisec;
+ int qf;
+ double heading;
+ double pitch;
+ double roll;
+ double x;
+ double y;
+} xy_temp;
static void CheckPjkParam(void);
static void CheckPjkParamTimeout(apptimer_var_t val);
@@ -312,28 +329,31 @@
int ss = str2int(s->nmea_value[0].data + 4, 2);
int dss = str2int(s->nmea_value[0].data + 7, 2);
- CurrRTKInfo.MM = str2int(s->nmea_value[1].data, 2);
- CurrRTKInfo.DD = str2int(s->nmea_value[1].data + 2, 2);
- CurrRTKInfo.YY = str2int(s->nmea_value[1].data + 4, 2);
+ xy_temp.month = str2int(s->nmea_value[1].data, 2);
+ xy_temp.day = str2int(s->nmea_value[1].data + 2, 2);
+ xy_temp.year = str2int(s->nmea_value[1].data + 4, 2);
- CurrRTKInfo.qf = str2int(s->nmea_value[6].data, s->nmea_value[6].length);
+ xy_temp.qf = str2int(s->nmea_value[6].data, s->nmea_value[6].length);
// NOTE: RTK妯″潡鏄互鍗楀寳鍚戜负X杞达紝瑗夸笢鍚戜负Y杞达紝鎴戜滑浜ゆ崲涓嬶紝浠ョ鍚堜竴鑸�昏緫
- str2float(&CurrRTKInfo.y, s->nmea_value[2].data, s->nmea_value[2].length);
- str2float(&CurrRTKInfo.x, s->nmea_value[4].data, s->nmea_value[4].length);
+ str2float(&xy_temp.y, s->nmea_value[2].data, s->nmea_value[2].length);
+ str2float(&xy_temp.x, s->nmea_value[4].data, s->nmea_value[4].length);
- if (CurrRTKInfo.hh == hh && CurrRTKInfo.mm == mm && CurrRTKInfo.ss == ss && CurrRTKInfo.dss == dss) {
- PlatformStatusChanged(RTK_UPDATE_EVT, (uint8_t *)&CurrRTKInfo, sizeof(CurrRTKInfo));
-// UpdateRTKInfo(&CurrRTKInfo);
-// up_num++;
- /*if ((up_num % 5) == 0)*/ {
-// NewMgrEvent(DRIVER_UPDATE_EVT);
- }
+ if (xy_temp.hour == hh && xy_temp.min == mm && xy_temp.sec == ss && xy_temp.millisec == dss) {
+ rtk_info_t rtk;
+ rtk.qf = xy_temp.qf;
+ rtk.utc_time = static_cast<int64_t>(TimeMakeComposite(2000 + xy_temp.year, xy_temp.month, xy_temp.day, xy_temp.hour, xy_temp.min, xy_temp.sec))*static_cast<int64_t>(1000) + static_cast<int64_t>(dss*10);
+ rtk.x = xy_temp.x;
+ rtk.y = xy_temp.y;
+ rtk.heading = xy_temp.heading;
+ rtk.pitch = xy_temp.pitch;
+ rtk.roll = xy_temp.roll;
+ PlatformStatusChanged(RTK_UPDATE_EVT, (uint8_t *)&rtk, sizeof(rtk));
}
- CurrRTKInfo.hh = hh;
- CurrRTKInfo.mm = mm;
- CurrRTKInfo.ss = ss;
- CurrRTKInfo.dss = dss;
+ xy_temp.hour = hh;
+ xy_temp.min = mm;
+ xy_temp.sec = ss;
+ xy_temp.millisec = dss;
}
void handleGPTRA(const struct nmea *s) {
@@ -344,22 +364,25 @@
int ss = str2int(s->nmea_value[0].data + 4, 2);
int dss = str2int(s->nmea_value[0].data + 7, 2);
- str2float(&CurrRTKInfo.heading, s->nmea_value[1].data, s->nmea_value[1].length);
- str2float(&CurrRTKInfo.pitch, s->nmea_value[2].data, s->nmea_value[2].length);
- str2float(&CurrRTKInfo.roll, s->nmea_value[3].data, s->nmea_value[3].length);
+ str2float(&xy_temp.heading, s->nmea_value[1].data, s->nmea_value[1].length);
+ str2float(&xy_temp.pitch, s->nmea_value[2].data, s->nmea_value[2].length);
+ str2float(&xy_temp.roll, s->nmea_value[3].data, s->nmea_value[3].length);
- if (CurrRTKInfo.hh == hh && CurrRTKInfo.mm == mm && CurrRTKInfo.ss == ss && CurrRTKInfo.dss == dss) {
- PlatformStatusChanged(RTK_UPDATE_EVT, (uint8_t *)&CurrRTKInfo, sizeof(CurrRTKInfo));
-// UpdateRTKInfo(&CurrRTKInfo);
-// up_num++;
- /*if ((up_num % 5) == 0)*/ {
-// NewMgrEvent(DRIVER_UPDATE_EVT);
- }
+ if (xy_temp.hour == hh && xy_temp.min == mm && xy_temp.sec == ss && xy_temp.millisec == dss) {
+ rtk_info_t rtk;
+ rtk.qf = xy_temp.qf;
+ rtk.utc_time = static_cast<int64_t>(TimeMakeComposite(2000 + xy_temp.year, xy_temp.month, xy_temp.day, xy_temp.hour, xy_temp.min, xy_temp.sec))*static_cast<int64_t>(1000) + static_cast<int64_t>(dss*10);
+ rtk.x = xy_temp.x;
+ rtk.y = xy_temp.y;
+ rtk.heading = xy_temp.heading;
+ rtk.pitch = xy_temp.pitch;
+ rtk.roll = xy_temp.roll;
+ PlatformStatusChanged(RTK_UPDATE_EVT, (uint8_t *)&rtk, sizeof(rtk));
}
- CurrRTKInfo.hh = hh;
- CurrRTKInfo.mm = mm;
- CurrRTKInfo.ss = ss;
- CurrRTKInfo.dss = dss;
+ xy_temp.hour = hh;
+ xy_temp.min = mm;
+ xy_temp.sec = ss;
+ xy_temp.millisec = dss;
}
static void CheckPjkParam(void)
diff --git a/lib/src/main/cpp/rtk_module/rtk.h b/lib/src/main/cpp/rtk_module/rtk.h
index 7d66f83..f31d204 100644
--- a/lib/src/main/cpp/rtk_module/rtk.h
+++ b/lib/src/main/cpp/rtk_module/rtk.h
@@ -43,13 +43,15 @@
}gpsStatus_t;
typedef struct {
- int YY;
- int MM;
- int DD;
- int hh;
- int mm;
- int ss;
- int dss;
+// int YY;
+// int MM;
+// int DD;
+// int hh;
+// int mm;
+// int ss;
+// int dss;
+
+ int64_t utc_time; // 鍗曚綅姣
int qf;
double heading;
double pitch;
@@ -61,8 +63,8 @@
std::stringstream sst;
sst<<"RTK:"<<"qf="<<qf
- <<" "<<2000+YY<<intw(MM, 2)<<intw(DD, 2)<<intw(hh,2)<<intw(mm, 2)<<intw(ss, 2)<<"."<<intw(dss, 2)
- <<" heading="<<round(heading, 3)
+// <<" "<<2000+YY<<intw(MM, 2)<<intw(DD, 2)<<intw(hh,2)<<intw(mm, 2)<<intw(ss, 2)<<"."<<intw(dss, 2)
+ <<" yaw="<<round(heading, 3)
<<" pitch="<<round(pitch, 3)
<<" roll="<<round(roll, 3)
<<" x="<<round(x, 3)
@@ -70,7 +72,7 @@
return sst.str();
}
-}rtk_info;
+}rtk_info_t;
void RtkCommModeSel(int mode);
void ConfigRTKModule(bool ayDevice);
diff --git a/lib/src/main/cpp/rtk_platform/platform.cpp b/lib/src/main/cpp/rtk_platform/platform.cpp
index 6f49707..a48d7d2 100644
--- a/lib/src/main/cpp/rtk_platform/platform.cpp
+++ b/lib/src/main/cpp/rtk_platform/platform.cpp
@@ -23,9 +23,6 @@
#include "../utils/num.h"
#include "../driver_test.h"
#include "../test_common/car_sensor.h"
-#include "../test_items2/stop_car.h"
-#include "../test_items2/operate_gear.h"
-#include "../test_items2/drive_straight.h"
#include "../rtk_module/virtual_rtk.h"
#define PARSE_BUFF_SIZE 4096
@@ -402,7 +399,7 @@
case GPS_UPDATE_EVT: {
const gpsStatus_t *gps = (gpsStatus_t *) data;
- DEBUG("GPS: %s", const_cast<gpsStatus_t *>(gps)->toString().c_str());
+// DEBUG("GPS: %s", const_cast<gpsStatus_t *>(gps)->toString().c_str());
gbf.qf = gps->gps_status;
gbf.latitude = gps->latitude;
@@ -432,9 +429,9 @@
uint32_t ost = AppTimer_GetTickCount();
uint32_t ost1 = ost;
- const rtk_info *rtk = (rtk_info *) data;
+ const rtk_info_t *rtk = (rtk_info_t *) data;
- DEBUG("RTK: %s", const_cast<rtk_info *>(rtk)->toString().c_str());
+// DEBUG("RTK: %s", const_cast<rtk_info_t *>(rtk)->toString().c_str());
rbf.qf = rtk->qf;
rbf.coord_x = rtk->y;
@@ -446,8 +443,11 @@
rbf.coord_y_dir = 'E';
rbf.rtcm_length = rtcmLength;
- sprintf(rbf.utc, "%04d%02d%02d%02d%02d%02d.%02d", 2000 + rtk->YY, rtk->MM, rtk->DD,
- rtk->hh, rtk->mm, rtk->ss, rtk->dss);
+ struct TimeStructure ts;
+
+ TimeBreakdown(rtk->utc_time / 1000, &ts);
+ sprintf(rbf.utc, "%04d%02d%02d%02d%02d%02d.%02d", ts.Year, ts.Month, ts.Day,
+ ts.Hour, ts.Minute, ts.Second, (rtk->utc_time % 1000) / 10);
if (!strcmp(rbf.utc, gbf.utc)) {
rbf.sat_num = gbf.sat_num;
rbf.latitude = gbf.latitude;
@@ -461,7 +461,8 @@
ost1 = AppTimer_GetTickCount() - ost1;
- UpdateRTKInfo(rtk);
+ if (rtk->qf == 3)
+ UpdateRTKInfo(rtk);
ost = AppTimer_GetTickCount() - ost;
diff --git a/lib/src/main/cpp/test_common/Geometry.cpp b/lib/src/main/cpp/test_common/Geometry.cpp
index f1fbba9..8fe70d1 100644
--- a/lib/src/main/cpp/test_common/Geometry.cpp
+++ b/lib/src/main/cpp/test_common/Geometry.cpp
@@ -78,7 +78,7 @@
polygon->num = 0;
}
-Relation IntersectionOf(const Polygon *polygon1, const Polygon *polygon2)
+relation_t IntersectionOf(const Polygon *polygon1, const Polygon *polygon2)
{
bool inside = false, outside = false, tangent = false;
@@ -87,7 +87,7 @@
}
for (int idx = 0; idx < polygon1->num; ++idx) {
- Relation relation = IntersectionOf(polygon1->point[idx], polygon2);
+ relation_t relation = IntersectionOf(polygon1->point[idx], polygon2);
if (relation == GM_Containment) {
inside = true;
@@ -112,7 +112,7 @@
return GM_None;
}
-Relation IntersectionOf(Line line, const Polygon *polygon)
+relation_t IntersectionOf(Line line, const Polygon *polygon)
{
if (polygon->num == 0) {
return GM_None;
@@ -135,7 +135,7 @@
// LOGD("line1(%d %d - %d %d) line2(%d %d - %d %d)", line.X1, line.Y1, line.X2, line.Y2,
// line2.X1, line2.Y1, line2.X2, line2.Y2);
- Relation relation = IntersectionOf(line, line2);
+ relation_t relation = IntersectionOf(line, line2);
// LOGD("relation = %d", relation);
@@ -155,7 +155,7 @@
return tangent ? GM_Tangent : IntersectionOf(point2, polygon);
}
-Relation IntersectionOf(PointF point, const Polygon *polygon)
+relation_t IntersectionOf(PointF point, const Polygon *polygon)
{
switch (polygon->num)
{
@@ -222,7 +222,7 @@
return (counter % 2 == 1) ? GM_Containment : GM_None;
}
-Relation IntersectionOf(PointF point, Line line)
+relation_t IntersectionOf(PointF point, Line line)
{
double bottomY = fmin(line.Y1, line.Y2);
double topY = fmax(line.Y1, line.Y2);
@@ -247,7 +247,7 @@
}
}
-Relation IntersectionOf(Line line1, Line line2)
+relation_t IntersectionOf(Line line1, Line line2)
{
// Fail if either line segment is zero-length.
if ((isEqual(line1.X1, line1.X2) && isEqual(line1.Y1, line1.Y2)) || (isEqual(line2.X1, line2.X2) && isEqual(line2.Y1, line2.Y2)))
@@ -313,7 +313,7 @@
}
/*********************************************************
- * p2----------->p1 绾跨鍜孻杞寸殑澶硅
+ * p1----------->p2 绾跨鍜孻杞寸殑澶硅
* @param p1
* @param p2
* @return yaw
@@ -322,40 +322,102 @@
{
double deg = 0.0;
- if (fabs(p1.Y - p2.Y) <= GLB_EPSILON) {
- if (p1.X > p2.X) {
+ if (fabs(p2.Y - p1.Y) <= GLB_EPSILON) {
+ if (p2.X > p1.X) {
deg = 90;
} else {
deg = 270;
}
- } else if (fabs(p1.X - p2.X) <= GLB_EPSILON) {
- if (p1.Y > p2.Y) {
+ } else if (fabs(p2.X - p1.X) <= GLB_EPSILON) {
+ if (p2.Y > p1.Y) {
deg = 0;
} else {
deg = 180;
}
} else {
- deg = atan(fabs(p1.X - p2.X) /
- fabs(p1.Y - p2.Y));
+ deg = atan(fabs(p2.X - p1.X) /
+ fabs(p2.Y - p1.Y));
deg = toDegree(deg);
- if (p1.X > p2.X &&
- p1.Y > p2.Y) {
+ if (p2.X > p1.X &&
+ p2.Y > p1.Y) {
- } else if (p1.X < p2.X &&
- p1.Y > p2.Y) {
+ } else if (p2.X < p1.X &&
+ p2.Y > p1.Y) {
deg = 360 - deg;
- } else if (p1.X < p2.X &&
- p1.Y < p2.Y) {
+ } else if (p2.X < p1.X &&
+ p2.Y < p1.Y) {
deg = 180 + deg;
- } else if (p1.X > p2.X &&
- p1.Y < p2.Y) {
+ } else if (p2.X > p1.X &&
+ p2.Y < p1.Y) {
deg = 180 - deg;
}
}
return deg;
+}
+
+double YawOf(Line &line)
+{
+ PointF p1 = {
+ .X = line.X1,
+ .Y = line.Y1
+ };
+
+ PointF p2 = {
+ .X = line.X2,
+ .Y = line.Y2
+ };
+
+ return YawOf(p1, p2);
+}
+
+/**************************************************
+ * p1----->p3 涓� p1----->p2 涓ゅ皠绾跨殑閫嗘椂閽堝す瑙�
+ * 宸︿晶灏忎簬180锛屽彸渚уぇ浜�180
+ * @param p1
+ * @param p2
+ * @param p3
+ * @return
+ */
+double AngleOf(PointF p1, PointF p2, PointF p3)
+{
+ int rel = IntersectionOfLine(p1, p2, p3);
+
+ switch (rel) {
+ case RELATION_LEFT:
+ case RELATION_RIGHT: {
+ double a = DistanceOf(p2, p3);
+ double b = DistanceOf(p1, p2);
+ double c = DistanceOf(p1, p3);
+
+ double deg = toDegree(acos((pow(b, 2) + pow(c, 2) - pow(a, 2)) / (2 * b * c)));
+
+ return (rel == RELATION_LEFT) ? deg : (360-deg);
+ }
+ case RELATION_BACK:
+ return 180;
+ default:
+ return 0;
+ };
+
+ return 0;
+}
+
+/************************************************
+ * dest鐩稿浜巄ase鐨勯�嗘椂閽堝す瑙�
+ * @param base
+ * @param dest
+ * @return
+ */
+double AngleOfTowLine(Line base, Line dest)
+{
+ PointF p1 = {.X = base.X1, .Y = base.Y1};
+ PointF p2 = {.X = base.X2, .Y = base.Y2};
+ PointF p3 = {.X = dest.X2 + (base.X1 - dest.X1), .Y = dest.Y2 + (base.Y1 - dest.Y1)};
+
+ return AngleOf(p1, p2, p3);
}
double DeltaYaw(double yaw1, double yaw2)
@@ -449,27 +511,27 @@
* @param p3
* @return 0 - straight, 1 - left, -1 - right, 2 - front, -2 - back
*/
-int IntersectionOfLine(PointF p1, PointF p2, PointF p3)
+relational_position_t IntersectionOfLine(PointF p1, PointF p2, PointF p3)
{
double lr = (p1.X-p3.X)*(p2.Y-p3.Y) - (p1.Y-p3.Y)*(p2.X-p3.X);
if (fabs(lr) <= EPSILON2) {
double fb = (p2.X-p1.X)*(p3.X-p1.X) + (p2.Y-p1.Y)*(p3.Y-p1.Y);
if (fabs(fb) <= EPSILON2)
- return 0;
+ return REL_POS_ON;
else if (fb > 0)
- return 2;
+ return REL_POS_FRONT;
else
- return -2;
+ return REL_POS_REAR;
} else if (lr > 0) {
- return 1;
+ return REL_POS_LEFT;
} else {
- return -1;
+ return REL_POS_RIGHT;
}
}
// 0 - straight, 1 - left, -1 - right, 2 - front, -2 - back
-int IntersectionOfLine(PointF p, Line line)
+relational_position_t IntersectionOfLine(PointF p, Line line)
{
PointF p1, p2;
@@ -688,4 +750,30 @@
}
return deg;
-}
\ No newline at end of file
+}
+
+bool Calc3PointCircle(PointF p1, PointF p2, PointF p3, Circle &circle)
+{
+ double a = p1.X - p2.X;
+ double b = p1.Y - p2.Y;
+ double c = p1.X - p3.X;
+ double d = p1.Y - p3.Y;
+ double e = ((pow(p1.X,2) - pow(p2.X,2)) - (pow(p2.Y,2) - pow(p1.Y,2))) / 2;
+ double f = ((pow(p1.X,2) - pow(p3.X,2)) - (pow(p3.Y,2) - pow(p1.Y,2))) / 2;
+
+ // 涓夌偣涓嶈兘鍏辩嚎
+ if (isEqual2(a*d, b*c)) {
+ circle.centre.X = circle.centre.Y = circle.radius = 0;
+ return false;
+ }
+
+ double x0 = (d*e - b*f)/(a*d - b*c);
+ double y0 = (a*f - c*e)/(a*d - b*c);
+ double r = sqrt(pow(x0-p1.X, 2) + pow(y0-p1.Y, 2));
+
+ circle.centre.X = x0;
+ circle.centre.Y = y0;
+ circle.radius = r;
+
+ return true;
+}
diff --git a/lib/src/main/cpp/test_common/Geometry.h b/lib/src/main/cpp/test_common/Geometry.h
index 276fccd..08947b9 100644
--- a/lib/src/main/cpp/test_common/Geometry.h
+++ b/lib/src/main/cpp/test_common/Geometry.h
@@ -9,13 +9,13 @@
#include <initializer_list>
#include <vector>
-enum Relation
+typedef enum
{
GM_None,
GM_Tangent,
GM_Intersection,
GM_Containment
-};
+} relation_t;
typedef struct PointF_ {
double X;
@@ -34,6 +34,13 @@
PointF *point;
} Polygon;
+typedef struct Circle_ {
+ PointF centre;
+ double radius;
+} Circle;
+
+#define MAKE_LINE(a, b, c) { (a).X1=b.X; (a).Y1=b.Y; (a).X2=c.X; (a).Y2=c.Y; }
+
double toRadians(double degree);
double toDegree(double radians);
bool isEqual(double a, double b);
@@ -44,14 +51,17 @@
void CleanPolygon(Polygon *polygon);
void MakeHidePoint(PointF *point, const PointF *bp, const Line *bl);
-Relation IntersectionOf(const Polygon *polygon1, const Polygon *polygon2);
-Relation IntersectionOf(Line line, const Polygon *polygon);
-Relation IntersectionOf(PointF point, const Polygon *polygon);
-Relation IntersectionOf(PointF point, Line line);
-Relation IntersectionOf(Line line1, Line line2);
+relation_t IntersectionOf(const Polygon *polygon1, const Polygon *polygon2);
+relation_t IntersectionOf(Line line, const Polygon *polygon);
+relation_t IntersectionOf(PointF point, const Polygon *polygon);
+relation_t IntersectionOf(PointF point, Line line);
+relation_t IntersectionOf(Line line1, Line line2);
double DistanceOf(PointF point1, PointF point2);
double DistanceOf(PointF point, Line line);
double YawOf(PointF p1, PointF p2);
+double YawOf(Line &line);
+double AngleOf(PointF p1, PointF p2, PointF p3);
+double AngleOfTowLine(Line base, Line dest);
double DeltaYaw(double yaw1, double yaw2);
double CalculateAngle(Line base, Line dest);
PointF rotatePoint(PointF oldPoint, PointF centre, double degree);
@@ -59,8 +69,17 @@
bool InsidePolygon(const Polygon *t1, const Polygon *t2);
bool PartInsidePolygon(const Polygon *t1, const Polygon *t2);
bool OutsidePolygon(const Polygon *t1, const Polygon *t2);
-int IntersectionOfLine(PointF p1, PointF p2, PointF p3);
-int IntersectionOfLine(PointF p, Line line);
+
+typedef enum {
+ REL_POS_ON,
+ REL_POS_LEFT,
+ REL_POS_RIGHT,
+ REL_POS_FRONT,
+ REL_POS_REAR
+} relational_position_t;
+
+relational_position_t IntersectionOfLine(PointF p1, PointF p2, PointF p3);
+relational_position_t IntersectionOfLine(PointF p, Line line);
PointF GetVerticalPoint(PointF p1, PointF p2, PointF p3);
bool VerticalPointOnLine(PointF point, Line line);
bool VerticalPointOnLine(PointF point, Line line, PointF &vp);
@@ -68,5 +87,6 @@
PointF PointExtend(PointF ori, double length, double yaw);
bool IsSamePoint(PointF p1, PointF p2);
double AvgYaw(std::vector<double> &angles);
+bool Calc3PointCircle(PointF p1, PointF p2, PointF p3, Circle &circle);
#endif //GUI_GEOMETRY_H
diff --git a/lib/src/main/cpp/test_common/odo_graph.cpp b/lib/src/main/cpp/test_common/odo_graph.cpp
index 87381a9..555b177 100644
--- a/lib/src/main/cpp/test_common/odo_graph.cpp
+++ b/lib/src/main/cpp/test_common/odo_graph.cpp
@@ -3,18 +3,18 @@
//
#include "odo_graph.h"
-#include "../test_items2/road_exam.h"
#include "../common/apptimer.h"
#include "../utils/xconvert.h"
+#include "../driver_test.h"
static double odoGraph;
-static struct RtkTime odoTimer;
-static double odoPrevSpeed;
-static int odoCnt;
+static int64_t prevTimestamp;
+static double prevSpeed;
+static bool isstop;
void ResetOdo(void)
{
- odoCnt = 0;
+ isstop = true;
odoGraph = 0;
}
@@ -23,22 +23,27 @@
return odoGraph;
}
-void UpdataOdo(double speed, int moveDirect, const struct RtkTime *rtkTime) {
+void UpdataOdo(motion_t &motion) {
// 琛岄┒璺濈锛屽惈鍊掕溅
- if (odoCnt == 0 && moveDirect != 0) {
- odoPrevSpeed = speed;
- odoCnt = 1;
- odoTimer = *rtkTime;
- } else if (odoCnt == 1) {
- if (moveDirect != 0) {
- uint32_t tm = TimeGetDiff(rtkTime, &odoTimer);
- if (tm >= D_SEC(1)) {
- odoGraph += ((double) tm) * (odoPrevSpeed + speed) / 2.0 / 1000.0;
- odoTimer = *rtkTime;
- odoPrevSpeed = speed;
+ if (isstop && motion.move != STOP) {
+ prevSpeed = motion.speed;
+ isstop = false;
+ prevTimestamp = motion.timestamp;
+ } else if (!isstop) {
+ if (motion.move != STOP) {
+ uint32_t elapsed = motion.timestamp - prevTimestamp;
+ if (elapsed >= D_SEC(5)) { // 涓�旈暱鏃堕棿鏈畾浣嶏紝閲嶆柊寮�濮嬫祴閲�
+ prevSpeed = motion.speed;
+ prevTimestamp = motion.timestamp;
+ return;
+ }
+ if (elapsed >= D_SEC(1)) {
+ odoGraph += ((double) elapsed) * (prevSpeed + motion.speed) / 2.0 / 1000.0;
+ prevTimestamp = motion.timestamp;
+ prevSpeed = motion.speed;
}
} else {
- odoCnt = 0;
+ isstop = true;
}
}
}
\ No newline at end of file
diff --git a/lib/src/main/cpp/test_common/odo_graph.h b/lib/src/main/cpp/test_common/odo_graph.h
index 37f548a..68584cc 100644
--- a/lib/src/main/cpp/test_common/odo_graph.h
+++ b/lib/src/main/cpp/test_common/odo_graph.h
@@ -5,8 +5,10 @@
#ifndef MYAPPLICATION2_ODO_GRAPH_H
#define MYAPPLICATION2_ODO_GRAPH_H
+#include "../driver_test.h"
+
void ResetOdo(void);
double ReadOdo(void);
-void UpdataOdo(double speed, int moveDirect, const struct RtkTime *rtkTime);
+void UpdataOdo(motion_t &motion);
#endif //MYAPPLICATION2_ODO_GRAPH_H
diff --git a/lib/src/main/cpp/test_items/area_exam.cpp b/lib/src/main/cpp/test_items/area_exam.cpp
index 75f9da1..bed7a7c 100644
--- a/lib/src/main/cpp/test_items/area_exam.cpp
+++ b/lib/src/main/cpp/test_items/area_exam.cpp
@@ -16,17 +16,208 @@
#include "../common/apptimer.h"
#include "../test_common/odo_graph.h"
#include "../test_common/Geometry.h"
+#include "../common/observer.h"
+
#define DEBUG(fmt, args...) LOGD("<area_exam> <%s>: " fmt, __func__, ##args)
-static int CurrExamStatus = EXAM_AREA_NONE; // 1 娴嬭瘯瀹屾垚 0 娴嬭瘯涓� -1 娴嬭瘯閿欒閫�鍑�
-static int CurrExamMapIndex = -1;
-static int CurrEnterMapIndex = -1;
+ilovers::Observer<std::function<void(move_status_t)>> CarMoveEvent;
-static void DetectEnterOrExitMap(int moveDirect, const car_model *CarModel, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList);
-static int EnterMap(int moveDirect, const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList);
-static bool ExitMap(const car_model *car, int index, LIST_AREA_MAP &mapList);
-static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList);
-static void ExecuteExam(int index, LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int move, double azimuth, const struct RtkTime* rtkTime);
+static bool ProximityArea(Line &base_line, Line &line);
+static bool CrossingStartLine(Line &trace, Line &start_line);
+static void ProximityReminders(prime_t &prime);
+static void DetectCarMove(prime_t &prime);
+
+void AreaExam(prime_t &prime)
+{
+ switch (prime.curr_exam_map.type) {
+ case MAP_TYPE_PARK_BUTTOM:
+ break;
+ case MAP_TYPE_PARK_EDGE:
+ break;
+ case MAP_TYPE_TURN_90:
+ break;
+ case MAP_TYPE_UPHILL:
+ break;
+ case MAP_TYPE_CURVE:
+ break;
+ default:
+ if (EnterMap()) {
+
+ }
+ break;
+ }
+ DetectCarMove(prime);
+}
+
+static void DetectCarMove(prime_t &prime)
+{
+ static move_status_t prevMove = STOP;
+ if (prime.pMotion->move != prevMove) {
+ // Notify
+ CarMoveEvent.Notify(prime.pMotion->move);
+ prevMove = prime.pMotion->move;
+ }
+}
+
+void RegisterCarMoveObserver(void (*ptr)(move_status_t))
+{
+ CarMoveEvent.Connect(ptr);
+}
+
+void UnregisterCarMoveObserver(int handle)
+{
+ CarMoveEvent.Disconnect(handle);
+}
+
+void EnterMap(prime_t &prime)
+{
+ if (prime.curr_exam_map.type != 0) {
+ return;
+ }
+
+ if (prime.prev_modeling_index == -1 || prime.curr_modeling_index == -1) {
+ return;
+ }
+
+ PointF &lp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
+ PointF &lp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
+ PointF &rp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
+ PointF &rp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
+
+ Line left_trace, right_trace;
+
+ MAKE_LINE(left_trace, lp1, lp2);
+ MAKE_LINE(right_trace, rp1, rp2);
+
+ Line start_line;
+ for (int i = 0; i < prime.pMap->park_button_map.size(); ++i) {
+ MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[1], prime.pMap->park_button_map[i].map[0]);
+ if (CrossingStartLine(left_trace, start_line)) {
+ prime.curr_exam_map.type = MAP_TYPE_PARK_BUTTOM;
+ prime.curr_exam_map.map_idx = i;
+ StartParkBottom(prime);
+ return;
+ }
+ MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[7], prime.pMap->park_button_map[i].map[6]);
+ if (CrossingStartLine(left_trace, start_line)) {
+ prime.curr_exam_map.type = MAP_TYPE_PARK_BUTTOM;
+ prime.curr_exam_map.map_idx = i;
+ StartParkBottom(prime);
+ return;
+ }
+ }
+
+ for (int i = 0; i < prime.pMap->park_edge_map.size(); ++i) {
+ MAKE_LINE(start_line, prime.pMap->park_edge_map[i].map[1], prime.pMap->park_edge_map[i].map[0]);
+ if (CrossingStartLine(left_trace, start_line)) {
+ prime.curr_exam_map.type = MAP_TYPE_PARK_EDGE;
+ prime.curr_exam_map.map_idx = i;
+ return;
+ }
+ }
+
+ for (int i = 0; i < prime.pMap->uphill_map.size(); ++i) {
+ PointF vPoint = Calc3Point(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[0], DistanceOf(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[7]), 'R');
+ MAKE_LINE(start_line, prime.pMap->uphill_map[i].map[0], vPoint);
+ if (CrossingStartLine(left_trace, start_line)) {
+ prime.curr_exam_map.type = MAP_TYPE_UPHILL;
+ prime.curr_exam_map.map_idx = i;
+ return;
+ }
+ }
+ for (int i = 0; i < prime.pMap->curve_map.size(); ++i) {
+ MAKE_LINE(start_line, prime.pMap->curve_map[i].right_start_point, prime.pMap->curve_map[i].left_start_point);
+ if (CrossingStartLine(left_trace, start_line)) {
+ prime.curr_exam_map.type = MAP_TYPE_CURVE;
+ prime.curr_exam_map.map_idx = i;
+ return;
+ }
+ }
+ for (int i = 0; i < prime.pMap->turn_a90_map.size(); ++i) {
+ MAKE_LINE(start_line, prime.pMap->turn_a90_map[i].map[0], prime.pMap->turn_a90_map[i].map[1]);
+ if (CrossingStartLine(left_trace, start_line)) {
+ prime.curr_exam_map.type = MAP_TYPE_TURN_90;
+ prime.curr_exam_map.map_idx = i;
+ return;
+ }
+ }
+}
+
+// 杞﹁疆椹惰繃绾匡紝涓旇溅澶翠綅浜庡彸渚�
+static bool CrossingStartLine(Line &trace, Line &start_line)
+{
+ PointF head = {.X = trace.X1, .Y = trace.Y1};
+
+ if (IntersectionOf(trace, start_line) == GM_Intersection
+ && IntersectionOfLine(head, start_line) == RELATION_RIGHT) {
+ return true;
+ }
+ return false;
+}
+
+void FarawayMap(prime_t &prime)
+{
+ if (prime.arriving_map.type != 0) {
+ PointF &car_head = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]];
+ PointF car_head_trend = PointExtend(car_head, 7, prime.pModeling->yaw);
+ Line car_head_line;
+ MAKE_LINE(car_head_line, car_head, car_head_trend);
+
+
+ }
+}
+
+static void ProximityReminders(prime_t &prime)
+{
+ if (prime.curr_exam_map.type != MAP_TYPE_NONE && prime.arriving_map.type != MAP_TYPE_NONE) {
+ return;
+ }
+
+ PointF &car_head = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]];
+ PointF car_head_trend = PointExtend(car_head, 6, prime.pModeling->yaw);
+ Line car_head_line;
+ MAKE_LINE(car_head_line, car_head, car_head_trend);
+
+ Line start_line;
+ for (int i = 0; i < prime.pMap->park_button_map.size(); ++i) { // 宸﹀彸2鏉℃帶鍒剁嚎閮藉彲浣滀负鍏ュ彛
+ MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[1], prime.pMap->park_button_map[i].map[0]);
+
+ MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[7], prime.pMap->park_button_map[i].map[6]);
+
+ }
+ for (int i = 0; i < prime.pMap->park_edge_map.size(); ++i) {
+ MAKE_LINE(start_line, prime.pMap->park_edge_map[i].map[1], prime.pMap->park_edge_map[i].map[0]);
+ }
+
+ for (int i = 0; i < prime.pMap->uphill_map.size(); ++i) {
+ PointF vPoint = Calc3Point(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[0], DistanceOf(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[7]), 'R');
+ MAKE_LINE(start_line, prime.pMap->uphill_map[i].map[0], vPoint);
+ }
+ for (int i = 0; i < prime.pMap->curve_map.size(); ++i) {
+ MAKE_LINE(start_line, prime.pMap->curve_map[i].right_start_point, prime.pMap->curve_map[i].left_start_point);
+ }
+ for (int i = 0; i < prime.pMap->turn_a90_map.size(); ++i) {
+ MAKE_LINE(start_line, prime.pMap->turn_a90_map[i].map[0], prime.pMap->turn_a90_map[i].map[1]);
+ }
+}
+
+// 鍒ゆ柇鏄惁鎺ヨ繎鍦哄湴鐨勮捣濮嬬嚎
+// 杞﹀ご瓒嬪娍绾挎槸鍚﹀拰鍏ュ彛绾跨浉浜�
+// 鍜屽叆鍙ｇ嚎鐨勫す瑙�
+static bool ProximityArea(Line &base_line, Line &line)
+{
+ PointF head = {.X = line.X1, .Y = line.Y1};
+
+ if (IntersectionOf(base_line, line) == GM_Intersection
+ && DistanceOf(head, base_line) > 1) {
+ double angle = AngleOfTowLine(base_line, line);
+ if (angle >= 240 && angle <= 300) {
+ return true;
+ }
+ }
+
+ return false;
+}
void TerminateAreaExam(void)
{
@@ -37,479 +228,4 @@
{
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(moveDirect, car, CarModelList, AreaMapList);
-
- ExecuteExam(CurrExamMapIndex, AreaMapList, car, CarModelList, speed, moveDirect, azimuth, rtkTime);
-}
-
-void DistanceOfTire2X(std::vector<double> &array, const car_model *car, std::vector<Line> line_set)
-{
- double ld = 100, rd = 100;
-
- array.clear();
-
- for (auto line = line_set.begin(); line != line_set.end(); line++) {
- double ld_t = 100, rd_t = 100;
- double d;
- if (VerticalPointOnLine(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], *line)) {
- d = DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], *line);
- if (d < ld_t) {
- ld_t = d;
- }
- }
- if (VerticalPointOnLine(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], *line)) {
- d = DistanceOf(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], *line);
- if (d < ld_t) {
- ld_t = d;
- }
- }
-
- if (VerticalPointOnLine(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], *line)) {
- d = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], *line);
- if (d < rd_t) {
- rd_t = d;
- }
- }
- if (VerticalPointOnLine(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], *line)) {
- d = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], *line);
- if (d < rd_t) {
- rd_t = d;
- }
- }
-
- if (isEqual2(ld_t, rd_t)) {
- if (!isEqual(ld_t, 100)) {
- if (rd_t < rd)
- rd = rd_t;
- if (ld_t < ld)
- ld = ld_t;
- }
- } else if (ld_t > rd_t) {
- if (rd_t < rd)
- rd = rd_t;
- } else {
- if (ld_t < ld)
- ld = ld_t;
- }
- }
-
- array.push_back(ld);
- array.push_back(rd);
-}
-
-static void DetectEnterOrExitMap(int moveDirect, const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList)
-{
- if (CurrExamMapIndex < 0) {
- if (CurrEnterMapIndex < 0) {
- CurrEnterMapIndex = EnterMap(moveDirect, car, CarModelList, mapList);
- if (CurrEnterMapIndex >= 0) {
- DEBUG("杩涘叆鏌愪釜瀛愰」鐩� idx = %d", CurrEnterMapIndex);
- CurrExamMapIndex = CurrEnterMapIndex;
- CurrExamStatus = EXAM_AREA_START;
- }
- } else {
- if (ExitMap(car, CurrEnterMapIndex, mapList)) {
- DEBUG("绂诲紑鏌愪釜瀛愰」鐩� idx = %d", CurrEnterMapIndex);
- CurrEnterMapIndex = -1;
- }
- }
- }
-}
-
-static void ExecuteExam(int index, LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int move, double azimuth, const struct RtkTime* rtkTime)
-{
- if (index >= 0) {
- if (CurrExamStatus == EXAM_AREA_START) {
- DEBUG("CurrExamMapIndex %d mtype %d", AreaMapList[index].id, AreaMapList[index].type);
-
- switch (AreaMapList[index].type) {
- case MAP_TYPE_PARK_BUTTOM:
- DEBUG("杩涘叆鍊掕溅鍏ュ簱鍦哄湴 %d", AreaMapList[index].id);
-
- StartParkBottom(AreaMapList[index].id, move, rtkTime);
- CurrExamStatus = EXAM_AREA_RUN;
- break;
- case MAP_TYPE_STOP_START:
- DEBUG("杩涘叆涓婂潯璧锋鍦哄湴 %d", AreaMapList[index].id);
-
- StartSAS(AreaMapList[index].id, &AreaMapList[index].map, car, move, rtkTime);
- CurrExamStatus = EXAM_AREA_RUN;
- break;
- case MAP_TYPE_PART_EDGE:
- DEBUG("杩涘叆渚ф柟浣嶅仠杞﹀満鍦� %d", AreaMapList[index].id);
-
- StartParkEdge(AreaMapList[index].id, move, rtkTime);
- CurrExamStatus = EXAM_AREA_RUN;
- break;
- case MAP_TYPE_CURVE:
- DEBUG("杩涘叆鏇茬嚎琛岄┒鍦哄湴 %d", AreaMapList[index].id);
-
- StartDrivingCurve(AreaMapList[index].id, move, rtkTime);
- CurrExamStatus = EXAM_AREA_RUN;
- break;
- case MAP_TYPE_TURN_90:
- DEBUG("杩涘叆鐩磋杞集鍦哄湴 %d", AreaMapList[index].id);
-
- StartTurnA90(AreaMapList[index].id, move, azimuth, rtkTime);
- CurrExamStatus = EXAM_AREA_RUN;
- break;
- default:break;
- }
- } else if (CurrExamStatus == EXAM_AREA_RUN) {
- int testing = 0;
- switch (AreaMapList[index].type) {
- case MAP_TYPE_PARK_BUTTOM:
- testing = TestParkBottom(&AreaMapList[index].map,
- car, NULL, speed, move, rtkTime);
- break;
- case MAP_TYPE_STOP_START:
- testing = TestSAS(&AreaMapList[index].map, car, NULL, speed, move, rtkTime);
- break;
- case MAP_TYPE_PART_EDGE:
- testing = TestParkEdge(&AreaMapList[index].map, car, NULL, speed, move, rtkTime);
- break;
- case MAP_TYPE_CURVE:
- testing = TestDrivingCurve(&AreaMapList[index].map, &AreaMapList[index].map2, car, NULL, speed, move, rtkTime);
- break;
- case MAP_TYPE_TURN_90:
- testing = TestTurnA90(&AreaMapList[index].map, car, NULL, azimuth, speed, move, rtkTime);
- break;
- default:
- break;
- }
-
- if (testing > 0) {
- CurrExamStatus = EXAM_AREA_RUN;
- } else {
- CurrExamStatus = EXAM_AREA_END;
- }
- }
- if (CurrExamStatus != EXAM_AREA_RUN) {
- // 鏌愰」缁撴潫
- CurrExamStatus = EXAM_AREA_NONE;
- CurrExamMapIndex = -1;
- DEBUG("缁撴潫褰撳墠椤圭洰");
- }
- }
-}
-
-static int EnterMap(int moveDirect, const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList)
-{
- vector<int> score(mapList.size(), 0); // 鍦哄湴閲嶅悎鏃讹紝杞﹀ご灏鹃兘鍦ㄥ唴鐨勪紭鍏�
-
- for (int i = 0; i < mapList.size() && car != NULL; ++i) {
- // 杞﹀墠杞垨鍚庤疆杞ㄨ抗瓒婅繃瑙﹀彂绾�
- if (mapList[i].type == MAP_TYPE_STOP_START) {
- // 鏋勯�犺櫄鎷熺殑宸︿笂瑙掔偣
- double x9, y9, xo, yo;
-
- xo = (mapList[i].map.point[0].X + mapList[i].map.point[7].X) / 2;
- yo = (mapList[i].map.point[0].Y + mapList[i].map.point[7].Y) / 2;
-
- x9 = 2*xo - mapList[i].map.point[8].X;
- y9 = 2*yo - mapList[i].map.point[8].Y;
-
- Line triggerLine;
-
- triggerLine.X1 = mapList[i].map.point[0].X;
- triggerLine.Y1 = mapList[i].map.point[0].Y;
- triggerLine.X2 = x9;
- triggerLine.Y2 = y9;
-
- if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
- return i;
- }
- if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
- Line triggerLine;
-
- MakeLine(&triggerLine, &(mapList[i].map.point[1]), &(mapList[i].map.point[0]));
-
- if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
- return i;
-
- MakeLine(&triggerLine, &(mapList[i].map.point[7]), &(mapList[i].map.point[6]));
-
- if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
- return i;
-
- // 鍏ㄨ溅鍦ㄥ満鍦板唴
- if (moveDirect == 0) {
- Polygon carBody;
- bool ret = false;
- carBody.num = car->bodyNum;
- carBody.point = (PointF *) malloc(carBody.num * sizeof(PointF));
- for (int i = 0; i < carBody.num; ++i) {
- carBody.point[i] = car->carXY[car->body[i]];
- }
-
- if (IntersectionOf(&carBody, &mapList[i].map) == GM_Containment) {
- ret = true;
- }
-
- free(carBody.point);
-
- if (ret)
- return i;
- }
-// // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴
-// if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
-// Line enterLine1, enterLine2;
-//
-// MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
-// MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
-//
-// if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 &&
-// DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
-// score[i]++;
-// }
-// // 杞﹀熬椤剁偣鍦ㄥ満鍦板唴
-// if (IntersectionOf(car->carXY[ car->axial[AXIAL_REAR] ], &mapList[i].map) == GM_Containment) {
-// Line enterLine1, enterLine2;
-//
-// MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
-// MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
-//
-// if (DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine1) > 0.1 &&
-// DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine2) > 0.1)
-// score[i]++;
-// }
- }
- if (mapList[i].type == MAP_TYPE_PART_EDGE) {
- Line triggerLine;
-
- MakeLine(&triggerLine, &(mapList[i].map.point[1]), &(mapList[i].map.point[0]));
-
- if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
- return i;
-
- // 鍏ㄨ溅鍦ㄥ満鍦板唴
- if (moveDirect == 0) {
- Polygon carBody;
- bool ret = false;
- carBody.num = car->bodyNum;
- carBody.point = (PointF *) malloc(carBody.num * sizeof(PointF));
- for (int i = 0; i < carBody.num; ++i) {
- carBody.point[i] = car->carXY[car->body[i]];
- }
-
- if (IntersectionOf(&carBody, &mapList[i].map) == GM_Containment) {
- ret = true;
- }
-
- free(carBody.point);
-
- if (ret)
- return i;
- }
-// // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴
-// if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
-// Line enterLine;
-//
-// MakeLine(&enterLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
-//
-// if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine) > 0.1)
-// score[i]++;
-// }
-// // 杞﹀熬椤剁偣鍦ㄥ満鍦板唴
-// if (IntersectionOf(car->carXY[ car->axial[AXIAL_REAR] ], &mapList[i].map) == GM_Containment) {
-// Line enterLine;
-//
-// MakeLine(&enterLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
-//
-// if (DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine) > 0.1)
-// score[i]++;
-// }
- }
- if (mapList[i].type == MAP_TYPE_TURN_90) {
- // 杞﹀墠杞垨鍚庤疆杞ㄨ抗瓒婅繃瑙﹀彂绾�
- Line triggerLine;
-
- MakeLine(&triggerLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
-
- if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
- return i;
- }
- if (mapList[i].type == MAP_TYPE_CURVE) {
- // 杞﹀墠杞垨鍚庤疆杞ㄨ抗瓒婅繃瑙﹀彂绾�
- Line triggerLine;
-
- MakeLine(&triggerLine, &mapList[i].map2.point[0], &mapList[i].map.point[0]);
- if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
- return i;
- }
- }
-
- for (int i = 0; i < score.size(); ++i) {
- if (score[i] == 2) {
- return i;
- }
- }
-
- for (int i = 0; i < score.size(); ++i) {
- if (score[i] == 1) {
- return i;
- }
- }
-
- return -1;
-}
-
-static bool ExitMap(const car_model *car, int index, LIST_AREA_MAP &mapList)
-{
- bool ret = false;
- if (index < 0 || index >= mapList.size()) return true;
-
- if (mapList[index].type == MAP_TYPE_PARK_BUTTOM ||
- mapList[index].type == MAP_TYPE_PART_EDGE ||
- mapList[index].type == MAP_TYPE_TURN_90) {
- // 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
- Polygon carBody;
-
- carBody.num = car->bodyNum;
- carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
- for (int i = 0; i < carBody.num; ++i) {
- carBody.point[i] = car->carXY[car->body[i]];
- }
-
- if (IntersectionOf(&carBody, &mapList[index].map) == GM_None) {
- ret = true;
- }
-
- free(carBody.point);
- }
- if (mapList[index].type == MAP_TYPE_STOP_START) {
- // 鏋勯�犺櫄鎷熺殑宸︿笂瑙掔偣
- double x10, y10, x9, y9, xo, yo;
-
- bool enter = false;
-
- xo = (mapList[index].map.point[0].X + mapList[index].map.point[7].X) / 2;
- yo = (mapList[index].map.point[0].Y + mapList[index].map.point[7].Y) / 2;
-
- x9 = 2*xo - mapList[index].map.point[8].X;
- y9 = 2*yo - mapList[index].map.point[8].Y;
-
- if (mapList[index].map.num > 9) {
- // 鏋勯�犺櫄鎷熺殑鍙充笂瑙掔偣
- xo = (mapList[index].map.point[9].X + mapList[index].map.point[7].X) / 2;
- yo = (mapList[index].map.point[9].Y + mapList[index].map.point[7].Y) / 2;
- x10 = 2*xo - mapList[index].map.point[8].X;
- y10 = 2*yo - mapList[index].map.point[8].Y;
- }
-
- Polygon map;
-
- map.num = 4;
- map.point = (PointF *) malloc(map.num * sizeof(PointF));
-
- if (mapList[index].map.num <= 9) {
- map.point[0] = mapList[index].map.point[0];
- map.point[1] = mapList[index].map.point[8];
- map.point[2] = mapList[index].map.point[7];
- map.point[3].X = x9;
- map.point[3].Y = y9;
- } else {
- map.point[0] = mapList[index].map.point[0];
- map.point[1] = mapList[index].map.point[9];
- map.point[2].X = x10;
- map.point[2].Y = y10;
- map.point[3].X = x9;
- map.point[3].Y = y9;
- }
- // 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
- Polygon carBody;
-
- carBody.num = car->bodyNum;
- carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
- for (int i = 0; i < carBody.num; ++i) {
- carBody.point[i] = car->carXY[car->body[i]];
- }
-
- if (IntersectionOf(&carBody, &map) == GM_None) {
- ret = true;
- }
-
- free(carBody.point);
- free(map.point);
- }
- if (mapList[index].type == MAP_TYPE_CURVE) {
- ret = ExitDrivingCurveArea(&mapList[index].map, &mapList[index].map2, car);
- }
-
- return ret;
-}
-
-static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList)
-{
- bool trigger = false;
-
- if (CarModelList.size() < 5)
- return trigger;
-
- Polygon trace, trace2;
- int pn = 0;
-
- trace2.num = trace.num = 5;
- trace.point = (PointF *) malloc(sizeof(PointF) * trace.num);
- trace2.point = (PointF *) malloc(sizeof(PointF) * trace2.num);
-
- list<car_model *>::iterator iter = CarModelList.begin();
-
- car_model *c1 = *iter;
-
- trace.point[pn] = centerOfTwoPoint(c1->carXY[c1->left_front_tire[TIRE_OUTSIDE]], c1->carXY[c1->right_front_tire[TIRE_OUTSIDE]]);
- trace2.point[pn++] = centerOfTwoPoint(c1->carXY[c1->left_rear_tire[TIRE_OUTSIDE]], c1->carXY[c1->right_rear_tire[TIRE_OUTSIDE]]);
-
- ++iter;
-
- while (iter != CarModelList.end() && pn < trace.num) {
- car_model *c2 = *iter;
-
- uint32_t tdiff = TimeGetDiff(c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);
-
- if (tdiff >= D_SEC(1)) {
- trace.point[pn] = centerOfTwoPoint(c2->carXY[c2->left_front_tire[TIRE_OUTSIDE]], c2->carXY[c2->right_front_tire[TIRE_OUTSIDE]]);
- trace2.point[pn++] = centerOfTwoPoint(c2->carXY[c2->left_rear_tire[TIRE_OUTSIDE]], c2->carXY[c2->right_rear_tire[TIRE_OUTSIDE]]);
- c1 = c2;
- }
- ++iter;
- }
-
- PointF p1, p2;
-
- p1.X = triggerLine.X1;
- p1.Y = triggerLine.Y1;
- p2.X = triggerLine.X2;
- p2.Y = triggerLine.Y2;
-
- int pp = 0;
- for (int p = 1; p < pn; ++p) {
- Line trace_line, trace2_line;
-
- MakeLine(&trace_line, &trace.point[pp], &trace.point[p]);
- MakeLine(&trace2_line, &trace2.point[pp], &trace2.point[p]);
-
- if ((IntersectionOf(trace_line, triggerLine) == GM_Intersection &&
- IntersectionOfLine(p1, p2, centerOfTwoPoint(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], car->carXY[car->right_front_tire[TIRE_OUTSIDE]])) == -1 &&
- DistanceOf(centerOfTwoPoint(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], car->carXY[car->right_front_tire[TIRE_OUTSIDE]]), triggerLine) > 0.1) ||
- (IntersectionOf(trace2_line, triggerLine) == GM_Intersection &&
- IntersectionOfLine(p1, p2, centerOfTwoPoint(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], car->carXY[car->right_rear_tire[TIRE_OUTSIDE]])) == -1 &&
- DistanceOf(centerOfTwoPoint(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]), triggerLine) > 0.1) ) {
- // 纰板埌瑙﹀彂绾�
- DEBUG("纰版挒瑙﹀彂绾� 寮曞彂鍦板浘");
- trigger = true;
- goto SEARCH_TRIGGER_LINE_END;
- }
- }
-
- SEARCH_TRIGGER_LINE_END:
- free(trace.point);
-
- return trigger;
}
diff --git a/lib/src/main/cpp/test_items/area_exam.h b/lib/src/main/cpp/test_items/area_exam.h
index f87ffc5..1259cdf 100644
--- a/lib/src/main/cpp/test_items/area_exam.h
+++ b/lib/src/main/cpp/test_items/area_exam.h
@@ -8,9 +8,12 @@
#include <vector>
#include "../driver_test.h"
+
void InitAreaExam(void);
void TerminateAreaExam(void);
-void TestAreaGeneral(LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, double azimuth, const struct RtkTime *rtkTime);
-void DistanceOfTire2X(std::vector<double> &array, const car_model *car, std::vector<Line> line_set);
+void AreaExam(prime_t &prime);
+
+void TestAreaGeneral(area_map_t &map, const car_model_t *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, double azimuth, const struct RtkTime *rtkTime);
+void DistanceOfTire2X(std::vector<double> &array, const car_model_t *car, std::vector<Line> line_set);
#endif //MYAPPLICATION2_AREA_EXAM_H
diff --git a/lib/src/main/cpp/test_items/driving_curve.cpp b/lib/src/main/cpp/test_items/driving_curve.cpp
index 447ef11..7ea497a 100644
--- a/lib/src/main/cpp/test_items/driving_curve.cpp
+++ b/lib/src/main/cpp/test_items/driving_curve.cpp
@@ -34,7 +34,7 @@
static bool UpdateStartLine(struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect);
static bool UpdateEndLine(bool mode, struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect);
-static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone, int &who);
+static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model_t *car, struct scan_window_t *zone, int &who);
void StartDrivingCurve(int index, int moveDirect, const struct RtkTime *rtkTime)
{
@@ -55,7 +55,54 @@
MA_EnterMap(mapIndex, MAP_TYPE_CURVE, 1);
}
-int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
+// 鏇茬嚎鍦哄湴鍏抽敭鐐�
+// 鍏ュ彛宸﹀彸涓ょ偣锛屽嚭鍙ｅ乏鍙充袱鐐癸紝鍓嶅崐閮ㄥぇ灏忓渾鍦嗗績鍜岀洿寰勶紝鍚庡崐閮ㄥぇ灏忓渾鍦嗗績鍜岀洿寰勶紝瀹為檯娴嬮噺澶у皬鍦嗗渾蹇冨亸宸湪20cm鍐�
+int TestCurve(const curve_map_t *map, const car_model_t *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
+{
+ // 鍒ゆ柇鏄惁鍦ㄨ寖鍥村唴锛屽鏋�4涓溅杞兘涓嶅湪鏇茬嚎鍐咃紝瑙嗕负杞﹁締绂诲紑鍦哄湴
+ vector<PointF> pxs = {car->carXY[car->left_front_tire[TIRE_OUTSIDE]],
+ car->carXY[car->left_rear_tire[TIRE_OUTSIDE]],
+ car->carXY[car->right_front_tire[TIRE_OUTSIDE]],
+ car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]};
+
+ int collide = 0;
+
+ for (auto px: pxs) {
+ if (IntersectionOfLine(map->front_half_small_circle_centre, map->back_half_small_circle_centre, px) == RELATION_RIGHT) {
+ // 浣嶄簬鍓嶅崐閮�,瀹為檯鍦哄湴宸﹀彸杈圭嚎璧风偣銆佺粓鐐瑰拰鍦嗗績鍩烘湰涓嶄細鍏辩嚎锛屽彲鑳藉樊寮傝繎涓�绫筹紝鎵�浠ヨ鍒嗗紑璁＄畻
+ // 鍒ゆ柇鐨勭偣瓒呰繃宸﹀彸璧风偣锛屽嵆鍦ㄧ嚎鐨勫乏渚�
+ if (AngleOf(map->front_half_small_circle_centre, map->left_start_point, px) < 180) {
+ if (DistanceOf(map->front_half_small_circle_centre, px) < map->front_half_small_circle_radius) {
+ collide = 1;
+ continue;
+ }
+ }
+ if (AngleOf(map->front_half_big_circle_centre, map->right_start_point, px) < 180) {
+ if (DistanceOf(map->front_half_big_circle_centre, px) > map->front_half_big_circle_radius) {
+ collide = 2;
+ continue;
+ }
+ }
+ } else {
+ // 鍒ゆ柇鐨勭偣涓嶈秴杩囧乏鍙崇粓鐐癸紝鍗冲湪绾跨殑宸︿晶
+ if (AngleOf(map->back_half_small_circle_centre, map->right_end_point, px) < 180) {
+ if (DistanceOf(map->back_half_small_circle_centre, px) < map->back_half_small_circle_radius) {
+ collide = 2;
+ continue;
+ }
+ }
+ if (AngleOf(map->back_half_big_circle_centre, map->left_end_point, px) < 180) {
+ if (DistanceOf(map->back_half_big_circle_centre, px) > map->back_half_big_circle_radius) {
+ collide = 1;
+ continue;
+ }
+ }
+ }
+ }
+
+}
+
+int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model_t *car, const car_model_t *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
Polygon tireRect;
int who = 0;
@@ -279,7 +326,7 @@
return crash;
}
-bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model *car)
+bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model_t *car)
{
// 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
bool ret = false;
@@ -316,7 +363,7 @@
}
// 杞﹁疆鏄惁鍘嬭竟绾�
-static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone, int &who)
+static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model_t *car, struct scan_window_t *zone, int &who)
{
bool ret = false;
diff --git a/lib/src/main/cpp/test_items/driving_curve.h b/lib/src/main/cpp/test_items/driving_curve.h
index e9f39fc..b841076 100644
--- a/lib/src/main/cpp/test_items/driving_curve.h
+++ b/lib/src/main/cpp/test_items/driving_curve.h
@@ -11,8 +11,8 @@
using namespace std;
void StartDrivingCurve(int index, int moveDirect, const struct RtkTime *rtkTime);
-bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model *car);
+bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model_t *car);
-int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime);
+int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model_t *car, const car_model_t *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime);
#endif //RTKDRIVERTEST_DRIVING_CURVE_H
diff --git a/lib/src/main/cpp/test_items/park_bottom.cpp b/lib/src/main/cpp/test_items/park_bottom.cpp
index d1bb652..421ecf5 100644
--- a/lib/src/main/cpp/test_items/park_bottom.cpp
+++ b/lib/src/main/cpp/test_items/park_bottom.cpp
@@ -1,6 +1,17 @@
//
// Created by YY on 2019/10/23.
//
+//
+// 0 ____________________________________7
+//
+//
+// 1 _____________2 5_________________6
+// 8| |9
+// | |
+// | |
+// | |
+// 3--------4
+//
#include "park_bottom.h"
#include "../common/apptimer.h"
@@ -35,19 +46,22 @@
TEST_FINISH
};
-static int testStatus;
+typedef enum {
+ NONE_CTRL_LINE,
+ LEFT_CTRL_LINE,
+ RIGHT_CTRL_LINE
+} ctrl_line_t;
+
static bool reverseCar = false;
-static int mapIndex = 0;
const uint32_t CHECK_PARK_DELAY = 400;
static uint32_t stopTimepoint;
-static int prevMoveDirect, storeMoveDirectBeforeStop;
+static int prevMoveDirect;
static bool occurCrashRedLine;
-static bool checkPartStatus;
+
static uint32_t firstReverseTimepoint;
-static bool reportExamTimeout, reportParkFail;
-static bool crossCtrlLineSw;
+
static int parkCount;
static char carray[3];
static int darray[3];
@@ -55,49 +69,44 @@
static int gearAtStop;
static int currGear;
-static char prevCrossedCtrlLine;
+static ctrl_line_t prevCrossedCtrlLine;
static double odo;
-static int exitAreaCfm;
-static char CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car);
-static bool EnterParking(const Polygon *map, const car_model *car);
-static bool CrashRedLine(const Polygon *map, const car_model *car, int &who);
-static bool ExitParkArea(const Polygon *map, const car_model *car);
-static bool AllTireExitParkArea(const Polygon *map, const car_model *car);
-void StartParkBottom(int index, int moveDirect, const struct RtkTime *rtkTime)
+static ctrl_line_t CrossCtrlLine(prime_t &prime);
+static bool EnterParking(prime_t &prime);
+static bool CrashRedLine(prime_t &prime);
+
+
+void StartParkBottom(prime_t &prime)
{
DEBUG("StartParkBottom");
- testStatus = TESTING;
reverseCar = false;
- mapIndex = index;
memset(carray, 0, sizeof(carray));
memset(darray, 0, sizeof(darray));
memset(parkStatus, 0, sizeof(parkStatus));
- prevMoveDirect = moveDirect;
+ prevMoveDirect = prime.pMotion->move;
- checkPartStatus = false;
firstReverseTimepoint = 0;
- reportExamTimeout = false;
+
parkCount = 0;
- crossCtrlLineSw = false;
- reportParkFail = false;
+
occurCrashRedLine = false;
currGear = ReadCarStatus(GEAR);
- prevCrossedCtrlLine = 0;
+ prevCrossedCtrlLine = NONE_CTRL_LINE;
stopTimepoint = 0;
- exitAreaCfm = 0;
+
odo = ReadOdo();
PlayTTS("鎮ㄥ凡杩涘叆鍊掕溅鍏ュ簱鍖哄煙", NULL);
}
-int TestParkBottom(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
+void TestParkBottom(prime_t &prime)
{
- char crossCtrlLine = 0;
- uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- int who = 0;
+ ctrl_line_t crossCtrlLine = NONE_CTRL_LINE;
+ uint32_t tp = AppTimer_GetTickCount();
+
vector<double> dtox;
vector<Line> line_set;
Line distance_line;
@@ -128,32 +137,32 @@
odo = ReadOdo();
}
- if (testStatus == TESTING && gear_change) {
+ if (gear_change) {
if (currGear == GEAR_R) {
// 鎸傚�掓尅,妫�娴嬫槸鍚﹁繃鎺у埗绾�
DEBUG("寮�濮嬫寕鍊掓尅");
if (!reverseCar) {
DEBUG("寮�濮嬮杞叆搴�");
reverseCar = true;
- MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 1);
+ MA_EnterMap(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].id, MAP_TYPE_PARK_BUTTOM, 1);
firstReverseTimepoint = tp; // 寮�濮�210绉掕鏃�
}
- crossCtrlLine = CrossCtrlLine(map, car, carPrev);
+ crossCtrlLine = CrossCtrlLine(prime);
if (parkCount >= 2) {
DEBUG("寮�濮嬫杞叆搴�");
parkCount = 0;
- MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 1);
+ MA_EnterMap(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].id, MAP_TYPE_PARK_BUTTOM, 1);
firstReverseTimepoint = tp; // 寮�濮�210绉掕鏃�
}
- if (crossCtrlLine == 0) {
+ if (crossCtrlLine == NONE_CTRL_LINE) {
// 鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎
- AddExamFault(20104, rtkTime);
+ AddExamFault(20104);
DEBUG("鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎");
} else if (crossCtrlLine == prevCrossedCtrlLine) {
// 閲嶅璺ㄨ秺鍚屼竴鎺у埗绾匡紝涓嶆寜瑙勫畾绾胯矾锛岄『搴忓舰寮忥紝涓嶅悎鏍�
- AddExamFault(20101, rtkTime);
+ AddExamFault(20101);
DEBUG("涓嶆寜瑙勫畾绾胯矾锛岄『搴忓舰寮忥紝 鍚� %c 渚�", prevCrossedCtrlLine);
} else {
prevCrossedCtrlLine = crossCtrlLine;
@@ -167,78 +176,45 @@
DEBUG("搴撲綅妫�鏌ユ鏁� = %d", parkCount);
- if (EnterParking(map, car)) {
+ if (EnterParking(prime)) {
DEBUG("鍊掑簱鎴愬姛");
} else {
- AddExamFault(20103, rtkTime);
+ AddExamFault(20103);
DEBUG("鍊掑簱涓嶅叆");
}
}
}
-
- if (testStatus == TESTING && parkCount < 2 && AllTireExitParkArea(map, car)) {
- if (++exitAreaCfm >= 4) { // 閬垮厤淇″彿婕傜Щ閫犳垚鐨勮鍒�
- testStatus = TEST_FAIL;
- AddExamFault(10103, rtkTime);
+
+ if (ExitParkArea(prime)) {
+ // 绂诲紑鍦哄湴
+ DEBUG("绂诲紑鍦哄湴");
+ if (parkCount < 2) {
+ AddExamFault(10103);
DEBUG("鐩存帴椹剁娴嬭瘯鍖�,涓嶆寜鑰冭瘯鍛樻寚浠ら┚椹�");
}
- } else {
- exitAreaCfm = 0;
- }
-
- if (ExitParkArea(map, car)) {
- DEBUG("绂诲紑鍦哄湴");
- // 绂诲紑鍦哄湴
- testStatus = TEST_FINISH;
- /*if ((parkStatus[0] != 1 || parkStatus[1] != 1) && !reportParkFail && reverseCar) {
- // 鍊掑簱涓嶅叆锛屼笉鍚堟牸
- reportParkFail = true;
- AddExamFault(20103, rtkTime);
- DEBUG("鍊掑簱涓嶅叆");
- }*/
+ prime.curr_exam_map.type = 0;
goto TEST_END;
}
- // 璺濈妫�娴�
- MakeLine(&distance_line, &map->point[0], &map->point[7]);
- line_set.push_back(distance_line);
- MakeLine(&distance_line, &map->point[1], &map->point[2]);
- line_set.push_back(distance_line);
- MakeLine(&distance_line, &map->point[2], &map->point[3]);
- line_set.push_back(distance_line);
- MakeLine(&distance_line, &map->point[3], &map->point[4]);
- line_set.push_back(distance_line);
- MakeLine(&distance_line, &map->point[4], &map->point[5]);
- line_set.push_back(distance_line);
- MakeLine(&distance_line, &map->point[5], &map->point[6]);
- line_set.push_back(distance_line);
- DistanceOfTire2X(dtox, car, line_set);
- MA_SendDistance(dtox[0], dtox[1]);
-
- if (CrashRedLine(map, car, who)) {
- if (!occurCrashRedLine /*&& reverseCar*/) {
+ if (BodyCollidingLine(prime) >= 0) {
+ if (!occurCrashRedLine) {
occurCrashRedLine = true;
// 杞﹁韩鍑虹嚎锛屼笉鍚堟牸
- AddExamFault(10116, rtkTime);
- DEBUG("杞﹁疆鍘嬬嚎");
- /*if (who == 1) {
- PlayTTS("鍘嬪乏搴撲綅绾�", NULL);
- } else if (who == 2) {
- PlayTTS("鍘嬪彸搴撲綅绾�", NULL);
- }*/
+ AddExamFault(10116);
+ DEBUG("杞﹁韩鍑虹嚎");
}
} else {
occurCrashRedLine = false;
}
- if (moveDirect != prevMoveDirect) {
- if (moveDirect == 0) {
+ if (prime.pMotion->move != prevMoveDirect) {
+ if (prime.pMotion->move == STOP) {
stopTimepoint = tp;
gearAtStop = (currGear == GEAR_R ? 1 : 0);
- DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
+ DEBUG("鍋滆溅浜�");
DEBUG("鍋滆溅鏃舵尅浣� = %d", gearAtStop);
- } else if (prevMoveDirect == 0 && stopTimepoint > 0) {
- DEBUG("缁х画琛岄┒ %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
+ } else if (prevMoveDirect == STOP && stopTimepoint > 0) {
+ DEBUG("缁х画琛岄┒");
DEBUG("鍋滆溅鏃堕棿 %ld", tp - stopTimepoint);
DEBUG("鍐嶆绉诲姩鏃舵尅浣� = %d", currGear == GEAR_R ? 1 : 0);
@@ -246,176 +222,122 @@
if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_bottom_pause_criteria)
&& gearAtStop == (currGear == GEAR_R ? 1 : 0)) {
// 鍋滆溅瓒�2绉掞紝姣忔鎵�5鍒�
- AddExamFault(20106, rtkTime);
+ AddExamFault(20106);
DEBUG("涓�斿仠杞�");
}
}
- prevMoveDirect = moveDirect;
+ prevMoveDirect = prime.pMotion->move;
}
-
-/* crossCtrlLine = CrossCtrlLine(map, car, carPrev);
- if (crossCtrlLine > 0 && !crossCtrlLineSw) {
- crossCtrlLineSw = true;
- if (parkCount == 0) {
- carray[0] = crossCtrlLine;
- } else if (parkCount == 1) {
- if (carray[0] == crossCtrlLine) {
- // 涓嶆寜瑙勫畾绾胯矾锛岄『搴忓舰寮忥紝涓嶅悎鏍�
- AddExamFault(20101, rtkTime);
- DEBUG("涓嶆寜瑙勫畾绾胯矾锛岄『搴忓舰寮�");
- }
- carray[1] = crossCtrlLine;
- } else if (parkCount == 2) {
- if (carray[0] != crossCtrlLine) {
- // 涓嶆寜瑙勫畾绾胯矾锛岄『搴忓舰寮忥紝涓嶅悎鏍�
- AddExamFault(20101, rtkTime);
- DEBUG("涓嶆寜瑙勫畾绾胯矾锛岄『搴忓舰寮�");
- } else {
- // 绂诲紑娴嬭瘯鍖猴紝鍋滄璁℃椂
- DEBUG("绂诲紑娴嬭瘯鍖猴紝鍋滄璁℃椂");
- testStatus = false;
- goto TEST_END;
- }
- carray[2] = crossCtrlLine;
- }
- }
-
- if (testStatus && darray[0] > 0 && tp - firstReverseTimepoint >= examParam.park_bottom_limit_time) {
- // 瀹屾垚瓒呮椂锛屼笉鍚堟牸
- if (!reportExamTimeout) {
- reportExamTimeout = true;
- AddExamFault(20105, rtkTime);
- DEBUG("椤圭洰瓒呮椂");
- }
- }
-
- if (moveDirect != prevMoveDirect) {
- if (moveDirect == 0) {
- stopTimepoint = tp;
- storeMoveDirectBeforeStop = prevMoveDirect;
- if (prevMoveDirect == -1) {
- checkPartStatus = true; // 姣忔鍊掕溅鍋滄锛岃Е鍙戝叆搴撴鏌�
- }
-
- DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
- } else {
- DEBUG("缁х画琛岄┒ %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
-
- DEBUG("鍋滆溅鏃堕棿 %ld", tp - stopTimepoint);
-
- if (moveDirect == storeMoveDirectBeforeStop) {
- // 鍚屾柟鍚戝啀鍚姩锛岀户缁垽鏂槸鍚﹀仠杞﹁秴鏃�
- if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_bottom_pause_criteria) && reverseCar) {
- // 鍋滆溅瓒�2绉掞紝姣忔鎵�5鍒�
- AddExamFault(20106, rtkTime);
- DEBUG("涓�斿仠杞�");
- }
- } else if (moveDirect == -1) {
- // 鍒囨崲涓哄�掕溅
- if (!reverseCar) {
- reverseCar = true;
- MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 1);
- }
- if (darray[parkCount] == 0) {
- if (!crossCtrlLineSw) {
- // 鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎
- AddExamFault(20104, rtkTime);
- DEBUG("鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎");
- }
- darray[parkCount] = 1;
- firstReverseTimepoint = tp; // 棣栨鍊掕溅锛屾墠鎰忓懗鐫�姝ら」鐩紑濮�
- }
- } else {
- // 鍒囨崲涓哄墠杩�
- DEBUG("鍒囨崲涓哄墠杩�");
-
- if (tp - stopTimepoint >= CHECK_PARK_DELAY) {
- if (crossCtrlLineSw) {
- if (parkStatus[parkCount] != 1) {
- // 鍊掑簱涓嶅叆锛屼笉鍚堟牸
- reportParkFail = true;
- AddExamFault(20103, rtkTime);
- DEBUG("鍊掑簱涓嶅叆");
- }
- }
-
- crossCtrlLineSw = false;
-
- if (parkCount < 2)
- parkCount++;
- }
- }
- }
- prevMoveDirect = moveDirect;
- } else if (moveDirect == -1) {
- if (darray[parkCount] == 0) {
- // 鍒囨崲涓哄�掕溅
- if (!crossCtrlLineSw) {
- // 鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎
- AddExamFault(20104, rtkTime);
- DEBUG("鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎");
- }
- darray[parkCount] = 1;
- firstReverseTimepoint = tp;
- }
- } else if (moveDirect == 0 && crossCtrlLineSw) {
- if (tp - stopTimepoint >= CHECK_PARK_DELAY && checkPartStatus) {
- if (EnterParking(map, car)) {
- parkStatus[parkCount] = 1;
- }
- checkPartStatus = false;
- }
- }*/
-
TEST_END:
- if (testStatus == TEST_FINISH) {
- DEBUG("鍊掑簱缁撴潫");
- MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 0);
- return 0;
+ DEBUG("鍊掑簱缁撴潫");
+ MA_EnterMap(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].id, MAP_TYPE_PARK_BUTTOM, 0);
+}
+
+static int BodyCollidingLine(prime_t &prime)
+{
+ vector<Line> lines;
+
+ Polygon car_body;
+
+ car_body.num = prime.pModel->body.size();
+ car_body.point = new PointF[car_body.num];
+
+ for (int i = 0; i < car_body.num; ++i) {
+ car_body.point[i] = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[i]];
}
- return 1;
+
+ Line line;
+ MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[0],
+ prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[7]);
+ lines.push_back(line);
+ MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[1],
+ prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2]);
+ lines.push_back(line);
+ MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2],
+ prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3]);
+ lines.push_back(line);
+ MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3],
+ prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4]);
+ lines.push_back(line);
+ MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4],
+ prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5]);
+ lines.push_back(line);
+ MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5],
+ prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[6]);
+ lines.push_back(line);
+
+ int idx = 0;
+
+ for (auto line: lines) {
+ if (IntersectionOf(line, &car_body) != GM_None) {
+ break;
+ }
+ idx++;
+ }
+ delete []car_body.point;
+ return idx < lines.size()? idx : -1;
}
// 妫�娴�2鍓嶈疆鏄惁姝ｅ悜瓒婅繃宸﹀彸鎺у埗绾�
-static char CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car)
+static ctrl_line_t CrossCtrlLine(prime_t &prime)
{
// 杩囧彸鎺у埗绾�
- if ((IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1) &&
- (IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->right_front_tire[TIRE_OUTSIDE]]) == -1) &&
- (IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->axial[AXIAL_REAR]]) == 1)) {
- return 'R';
+ Line left_trace, right_trace;
+ Line left_ctrl, right_ctrl;
+
+ MAKE_LINE(left_trace, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
+ prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]]);
+ MAKE_LINE(right_trace, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
+ prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]);
+
+ MAKE_LINE(left_ctrl, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[1], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[0]);
+ MAKE_LINE(right_ctrl, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[6], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[7]);
+
+
+ if (IntersectionOf(left_trace, left_ctrl) == GM_Intersection &&
+ IntersectionOf(right_trace, left_ctrl) == GM_Intersection &&
+ IntersectionOfLine(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], left_ctrl) == RELATION_LEFT) {
+ return LEFT_CTRL_LINE;
}
- // 杩囧乏鎺у埗绾�
- if ((IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == 1) &&
- (IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->right_front_tire[TIRE_OUTSIDE]]) == 1) &&
- (IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->axial[AXIAL_REAR]]) == -1)) {
- return 'L';
+ if (IntersectionOf(left_trace, right_ctrl) == GM_Intersection &&
+ IntersectionOf(right_trace, right_ctrl) == GM_Intersection &&
+ IntersectionOfLine(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], right_ctrl) == RELATION_RIGHT) {
+ return RIGHT_CTRL_LINE;
}
- return 0;
+ return NONE_CTRL_LINE;
}
-static bool EnterParking(const Polygon *map, const car_model *car) {
+// 闇�瑕佸簱鍏ュ彛绾垮璁＄畻鍦ㄥ唴
+static bool EnterParking(prime_t &prime) {
bool succ = false;
- Polygon parking;
+ Polygon park_area;
Polygon car_body;
- car_body.num = car->bodyNum;
- car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
+ car_body.num = prime.pModel->body.size();
+ car_body.point = new PointF[car_body.num];
for (int i = 0; i < car_body.num; ++i) {
- car_body.point[i] = car->carXY[car->body[i]];
+ car_body.point[i] = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[i]];
}
- MakePolygon(&parking, {map->point[2], map->point[3], map->point[4], map->point[5]});
+ PointF p8 = PointExtend(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2],
+ prime.pMap->park_button_map[prime.curr_exam_map.map_idx].line_width,
+ YawOf(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3]));
- if (IntersectionOf(&car_body, &parking) == GM_Containment) {
+ PointF p9 = PointExtend(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5],
+ prime.pMap->park_button_map[prime.curr_exam_map.map_idx].line_width,
+ YawOf(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4]));
+
+ MakePolygon(&park_area, {p8, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4], p9});
+
+ if (IntersectionOf(&car_body, &park_area) == GM_Containment) {
succ = true;
}
- CleanPolygon(&parking);
+ CleanPolygon(&park_area);
free(car_body.point);
DEBUG("妫�鏌ュ�掑簱鐘舵�� %s", succ ? "鎴愬姛" : "澶辫触");
@@ -423,84 +345,41 @@
return succ;
}
-static bool CrashRedLine(const Polygon *map, const car_model *car, int &who)
+// 4涓溅杞拰杞﹀ご鐐逛笉鍦ㄥ満鍦颁腑
+bool ExitParkArea(prime_t &prime)
{
- bool ret = false;
+ Polygon polygon;
- Line red_line;
- const int red_lines[][2] = {{0, 7}, {1, 2}, {2, 3}, {3, 4}, {4, 5}, {5, 6}};
-
- Polygon car_body;
-
- car_body.num = car->bodyNum;
- car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
- for (int i = 0; i < car_body.num; ++i) {
- car_body.point[i] = car->carXY[car->body[i]];
+ polygon.num = prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map.size();
+ polygon.point = new PointF[polygon.num];
+ for (int i = 0; i < polygon.num; ++i) {
+ polygon.point[i] = prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[i];
}
- for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
- MakeLine(&red_line, &(map->point[red_lines[i][0]]), &(map->point[red_lines[i][1]]));
- if (IntersectionOf(red_line, &car_body) != GM_None) {
- if (i == 2 || i == 1) {
- who = 1;
- } else if (i == 4 || i == 5) {
- who = 2;
- }
+ int num = 0;
- ret = true;
- break;
- }
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
+ &polygon) == GM_None) {
+ num++;
+ }
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
+ &polygon) == GM_None) {
+ num++;
+ }
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
+ &polygon) == GM_None) {
+ num++;
+ }
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]],
+ &polygon) == GM_None) {
+ num++;
+ }
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[prime.pModel->axial[AXIAL_FRONT]]],
+ &polygon) == GM_None) {
+ num++;
}
- free(car_body.point);
- return ret;
-}
+ delete []polygon.point;
-static bool ExitParkArea(const Polygon *map, const car_model *car)
-{
-// 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
- bool ret = false;
-
- Polygon carBody;
-
- carBody.num = car->bodyNum;
- carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
- for (int i = 0; i < carBody.num; ++i) {
- carBody.point[i] = car->carXY[car->body[i]];
- }
-
- if (IntersectionOf(&carBody, map) == GM_None) {
- ret = true;
- }
-
- free(carBody.point);
-
- return ret;
-}
-
-// 鍙屽墠杞拰鍙屽悗杞笉鍦ㄥ尯鍩�
-static bool AllTireExitParkArea(const Polygon *map, const car_model *car)
-{
- int tireExitNum = 0;
-
- if (IntersectionOf(car->carXY[ car->left_front_tire[TIRE_OUTSIDE] ], map) == GM_None) {
- tireExitNum++;
- }
-
- if (IntersectionOf(car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ], map) == GM_None) {
- tireExitNum++;
- }
-
- if (IntersectionOf(car->carXY[ car->left_rear_tire[TIRE_OUTSIDE] ], map) == GM_None) {
- tireExitNum++;
- }
-
- if (IntersectionOf(car->carXY[ car->right_rear_tire[TIRE_OUTSIDE] ], map) == GM_None) {
- tireExitNum++;
- }
-
- if (tireExitNum >= 4) {
- return true;
- }
- return false;
+ return num == 5? true : false;
}
diff --git a/lib/src/main/cpp/test_items/park_bottom.h b/lib/src/main/cpp/test_items/park_bottom.h
index 695ba04..35eb634 100644
--- a/lib/src/main/cpp/test_items/park_bottom.h
+++ b/lib/src/main/cpp/test_items/park_bottom.h
@@ -11,7 +11,8 @@
using namespace std;
-void StartParkBottom(int index, int moveStatus, const struct RtkTime *rtkTime);
-int TestParkBottom(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime);
+void StartParkBottom(prime_t &prime);
+void TestParkBottom(prime_t &prime);
+bool ExitParkArea(prime_t &prime);
#endif //RTKDRIVERTEST_PARK_BOTTOM_H
diff --git a/lib/src/main/cpp/test_items/park_edge.cpp b/lib/src/main/cpp/test_items/park_edge.cpp
index 4a4160c..9ac9060 100644
--- a/lib/src/main/cpp/test_items/park_edge.cpp
+++ b/lib/src/main/cpp/test_items/park_edge.cpp
@@ -1,6 +1,17 @@
//
// Created by YY on 2019/10/23.
//
+//
+// 4 ___________________________ 3
+// | |
+// | |
+// | |
+// ________________| |___________1
+// 6 5 2
+//
+//
+// 7______________________________________________________0
+//
#include "park_edge.h"
#include "../jni_log.h"
@@ -29,7 +40,6 @@
const uint32_t CHECK_PARK_DELAY = 400;
-static int mapIndex = 0;
static bool reportExamTimeout;
static bool reportParkFail;
@@ -45,24 +55,23 @@
static int currGear;
static double odo;
-static bool CrashRedLine1(const Polygon *map, const car_model *car);
-static bool CrashRedLine2(const Polygon *map, const car_model *car);
-static bool CrashRedLine3(const Polygon *map, const car_model *car);
-static bool EnterParking(const Polygon *map, const car_model *car);
-static bool ExitParkArea(const Polygon *map, const car_model *car);
-static bool ExitParkArea2(const Polygon *map, const car_model *car);
+static bool CrashRedLine1(const Polygon *map, const car_model_t *car);
+static bool CrashRedLine2(const Polygon *map, const car_model_t *car);
+static bool CrashRedLine3(const Polygon *map, const car_model_t *car);
+static bool EnterParking(const Polygon *map, const car_model_t *car);
+static bool ExitParkArea(const Polygon *map, const car_model_t *car);
+static bool ExitParkArea2(const Polygon *map, const car_model_t *car);
-void StartParkEdge(int index, int moveStatus, const struct RtkTime *rtkTime)
+void StartParkEdge(prime_t &prime)
{
DEBUG("杩涘叆渚ф柟鍋滆溅鍦哄湴");
testStatus = TESTING;
- mapIndex = index;
occurCrashRedLine1 = occurCrashRedLine2 = occurCrashRedLine3 = false; // 杩欎釜绉戠洰瑙勫畾鐗规畩鐐癸紝鍙戠敓涓�娆℃墸10鍒嗭紝鑰屼笉鐩存帴娣樻卑
reportExamTimeout = false;
reportParkFail = false;
- prevMoveStatus = moveStatus;
+ prevMoveStatus = prime.pMotion->move;
parkSuccess = false;
parkStatus = 0;
occurMoveBack = false;
@@ -82,7 +91,7 @@
}*/
}
-int TestParkEdge(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime)
+int TestParkEdge(prime_t &prime)
{
vector<double> dtox;
vector<Line> line_set;
@@ -128,7 +137,7 @@
occurMoveBack = true;
moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss,
rtkTime->mss * 10); // 寮�濮嬭鏃�
- MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 1);
+ MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 1);
}
} else {
if (occurMoveBack && !checkPark) {
@@ -307,7 +316,7 @@
DEBUG("寮�濮嬪�掕溅");
occurMoveBack = true;
moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 1);
+ MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 1);
}*/
}
@@ -328,14 +337,43 @@
TEST_END:
if (testStatus == TEST_FINISH) {
DEBUG("渚ф柟鍋滆溅缁撴潫");
- MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 0);
+ MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 0);
return 0;
}
return 1;
}
+int EnterParkEdge(prime_t &prime) {
+ if (prime.prev_modeling_index == -1 || prime.curr_modeling_index == -1) {
+ return -1;
+ }
+
+ PointF &lp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
+ PointF &lp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
+ PointF &rp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
+ PointF &rp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
+
+ Line left_trace, right_trace;
+
+ MAKE_LINE(left_trace, lp1, lp2);
+ MAKE_LINE(right_trace, rp1, rp2);
+
+ // 涓嶅悓鍦板尯宸﹀彸鍊掑簱娆″簭鍙兘涓嶅悓锛屾墍浠ヤ袱涓柟鍚戦兘鍙互杩涘叆
+ for (int i = 0; i < prime.pMap->park_button_map.size(); ++i) {
+ Line left_entrance_line;
+
+ MAKE_LINE(left_entrance_line, prime.pMap->park_button_map[i].map[1], prime.pMap->park_button_map[i].map[0]);
+
+ if (IntersectionOf(left_trace, left_entrance_line) == GM_Intersection
+ && IntersectionOfLine(lp1, left_entrance_line) == RELATION_RIGHT) {
+ return i;
+ }
+ }
+ return -1;
+}
+
// 杞﹁疆鏄惁鍘嬮亾璺竟绾�
-static bool CrashRedLine1(const Polygon *map, const car_model *car)
+static bool CrashRedLine1(const Polygon *map, const car_model_t *car)
{
bool ret = false;
@@ -360,7 +398,7 @@
}
// 杞﹁韩鏄惁鍘嬪簱浣嶇嚎
-static bool CrashRedLine2(const Polygon *map, const car_model *car)
+static bool CrashRedLine2(const Polygon *map, const car_model_t *car)
{
bool ret = false;
@@ -387,7 +425,7 @@
return ret;
}
-static bool CrashRedLine3(const Polygon *map, const car_model *car) {
+static bool CrashRedLine3(const Polygon *map, const car_model_t *car) {
bool ret = false;
if (!occurMoveBack) {
@@ -408,62 +446,6 @@
free(car_body.point);
}
-
- return ret;
-}
-
-static bool EnterParking(const Polygon *map, const car_model *car) {
- DEBUG("妫�鏌ュ仠杞﹀埌浣�...");
-
- bool succ = false;
-
- Polygon parking;
- Polygon car_body;
-
- car_body.num = car->bodyNum;
- car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
- for (int i = 0; i < car_body.num; ++i) {
- car_body.point[i] = car->carXY[car->body[i]];
- }
-
- MakePolygon(&parking, {map->point[2], map->point[3], map->point[4], map->point[5]});
-
- if (IntersectionOf(&car_body, &parking) == GM_Containment) {
- succ = true;
- }
-
- CleanPolygon(&parking);
- free(car_body.point);
-
- return succ;
-}
-
-// 杞﹀ご瑕侀┒杩囧墠搴撲綅绾�
-static bool ExitParkArea(const Polygon *map, const car_model *car)
-{
- if (IntersectionOfLine(map->point[6], map->point[7], car->carXY[ car->axial[AXIAL_FRONT] ]) == -1)
- return true;
- return false;
-}
-
-static bool ExitParkArea2(const Polygon *map, const car_model *car)
-{
-// 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
- bool ret = false;
-
- Polygon carBody;
-
- carBody.num = car->bodyNum;
- carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
- for (int i = 0; i < carBody.num; ++i) {
- carBody.point[i] = car->carXY[car->body[i]];
- }
-
- if (IntersectionOf(&carBody, map) == GM_None) {
- ret = true;
- }
-
- free(carBody.point);
return ret;
}
diff --git a/lib/src/main/cpp/test_items/park_edge.h b/lib/src/main/cpp/test_items/park_edge.h
index e918c33..6fc7e29 100644
--- a/lib/src/main/cpp/test_items/park_edge.h
+++ b/lib/src/main/cpp/test_items/park_edge.h
@@ -12,6 +12,6 @@
using namespace std;
void StartParkEdge(int index, int moveStatus, const struct RtkTime *rtkTime);
-int TestParkEdge(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime);
+int TestParkEdge(const Polygon *map, const car_model_t *car, const car_model_t *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime);
#endif //RTKDRIVERTEST_PARK_EDGE_H
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 7639775..0037350 100644
--- a/lib/src/main/cpp/test_items/stop_and_start.cpp
+++ b/lib/src/main/cpp/test_items/stop_and_start.cpp
@@ -2,6 +2,21 @@
// Created by YY on 2019/10/31.
//
+// |9
+// |
+// |
+// |
+// |
+// 7-----------------|8
+// 6_________________|5
+// 3_________________|4
+// |
+// 2-----------------|1
+// |
+// |
+// |
+// |0
+
#include <cstdlib>
#include <vector>
#include <cmath>
@@ -21,7 +36,7 @@
using namespace std;
-const uint32_t STOP_CAR_TIME = D_SEC(1);
+
const double EPSILON = 1e-3;
@@ -29,62 +44,66 @@
static PointF stopPoint;
-static int mapIndex = 0;
-static int prevMoveDirect;
-static uint32_t stopTimepoint = 0;
-static bool stopCar = false;
-static uint32_t stopCarTime;
+static bool check1 = false;
+
+static bool stopConfirm;
+static bool restartComplete = false;
static bool occurCrashRedLine = false;
-static bool slideLongDistance = false;
+
static bool slideNormalDistance = false;
static bool reportSlideFault = false;
-static bool reportStartTimeout = false;
static bool handBreakActive = false;
-static bool CrashRedLine(const Polygon *map, const car_model *car);
-static double DistanceOfHead2Stopline(const Polygon *map, const car_model *car);
-static double DistanceOfTire2Edge(const Polygon *map, const car_model *car);
-static bool ExitTestArea(const Polygon *map, const car_model *car);
-void StartSAS(int index, const Polygon *map, const car_model *car, int moveDirect, const struct RtkTime *rtkTime)
+static bool CrashRedLine(prime_t &prime);
+static double DistanceOfHead2Stopline(prime_t &prime);
+static double DistanceOfTire2Edge(prime_t &prime);
+static bool ExitTestArea(prime_t &prime);
+
+void StartSAS(prime_t &prime)
{
- 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;
-
- if (moveDirect == 0) {
- stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- }
-
+ stopConfirm = false;
+ restartComplete = false;
occurCrashRedLine = false;
- stopCar = false;
- slideLongDistance = false;
+ check1 = false;
slideNormalDistance = false;
reportSlideFault = false;
- reportStartTimeout = false;
handBreakActive = false;
- MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 1);
+ MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_UPHILL, 1);
}
-int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
+static void StoppingTimeout(apptimer_var_t val) {
+ // 璧锋鏃堕棿瓒呰繃30绉掞紝涓嶅悎鏍�
+ AddExamFault(20303);
+ DEBUG("璧锋鏃堕棿瓒呰繃30绉�");
+}
+
+static void StopConfirm(apptimer_var_t val) {
+ stopConfirm = true;
+}
+
+void MotionChange(move_status_t mv)
+{
+ AppTimer_delete(StopConfirm);
+
+ if (mv == STOP) {
+ AppTimer_add(StopConfirm, D_SEC(1));
+ } else {
+
+ }
+}
+
+int TestSAS(prime_t &prime)
{
static double distanceToStopLine = 0, distanceToEdge = 0;
- if (!testing)
- return 0;
- if (CrashRedLine(map, car)) {
+ if (CrashRedLine(prime)) {
// 杞﹁疆鍘嬬嚎锛屼笉鍚堟牸
if (!occurCrashRedLine) {
- AddExamFault(10116, rtkTime);
+ AddExamFault(10116);
DEBUG("杞﹁疆鍘嬬嚎");
}
occurCrashRedLine = true;
@@ -92,158 +111,125 @@
occurCrashRedLine = false;
}
- if (ExitTestArea(map, car)) {
- // 椹剁娴嬭瘯鍖�
- if (!stopCar) {
- // 涓嶅仠杞︾洿鎺ョ寮�
- AddExamFault(10103, rtkTime);
- }
- testing = false;
- }
+ // 妫�娴嬪埌鍋滆溅
+ if (prime.pMotion->move == STOP && stopConfirm) {
+ if (!check1) {
+ check1 = true;
+ // 瀛樺偍鍋滄鐐�
+ stopPoint = prime.pModeling->base_point;
+ // 寮�濮嬪仠杞﹁鏃�
+ AppTimer_delete(StoppingTimeout);
+ AppTimer_add(StoppingTimeout, examParam.ramp_start_car_limit_time);
- if (prevMoveDirect != moveDirect) {
- if (moveDirect == 0) {
- stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- } else if (stopCar && !handBreakActive) {
- // 妫�鏌ユ槸鍚︽媺浣忔墜鍒�
- handBreakActive = true;
- AddExamFault(20306, rtkTime);
- DEBUG("娌℃媺鎵嬪埞");
- }
- 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 && !stopCar) {
- // 杩欓噷鍒ゆ柇鍋滆溅鐘舵��
- stopCar = true;
- stopCarTime = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
-
- stopPoint = car->carXY[car->body[0]];
-
- distanceToStopLine = DistanceOfHead2Stopline(map, car);
- distanceToEdge = DistanceOfTire2Edge(map, car);
+ // 妫�鏌ヨ溅澶村拰鍋滆溅甯︾殑璺濈
+ distanceToStopLine = DistanceOfHead2Stopline(prime);
+ distanceToEdge = DistanceOfTire2Edge(prime);
DEBUG("DIS1 = %f DIS2 = %f", distanceToStopLine, distanceToEdge);
if (distanceToStopLine > examParam.ramp_stoppoint_red_distance) {
// 璺濈鍋滄绾垮墠鍚庤秴鍑�50鍘樼背
- AddExamFault(20301, rtkTime);
+ AddExamFault(20301);
DEBUG("璺濈鍋滄绾垮墠鍚庤秴鍑�50鍘樼背锛屼笉鍚堟牸");
} else if (fabs(distanceToStopLine) > EPSILON) {
// 鍓嶄繚闄╂病鏈変綅浜庡仠姝㈠甫鍐咃紝浣嗘病鏈夎秴鍑�50鍘樼背锛屾墸10鍒�
- AddExamFault(20304, rtkTime);
+ AddExamFault(20304);
DEBUG("鍓嶄繚闄╂病鏈変綅浜庡仠姝㈠甫鍐咃紝浣嗘病鏈夎秴鍑�50鍘樼背");
}
if (distanceToEdge > examParam.ramp_edge_red_distance) {
// 璺濈杈圭嚎瓒呭嚭50鍘樼背,涓嶅悎鏍�
- AddExamFault(20302, rtkTime);
+ AddExamFault(20302);
DEBUG("璺濈杈圭嚎瓒呭嚭50鍘樼背");
} else if (distanceToEdge > examParam.ramp_edge_yellow_distance) {
// 璺濈杈圭嚎瓒呭嚭30鍘樼背锛屾墸10鍒�
- AddExamFault(20305, rtkTime);
+ AddExamFault(20305);
DEBUG("璺濈杈圭嚎瓒呭嚭30鍘樼背");
}
}
- if (stopCar && !handBreakActive && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
+ // 鍋滆溅鍚庯紝妫�鏌ユ墜鍒规媺璧锋儏鍐�
+ if (!handBreakActive && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
handBreakActive = true;
}
}
- if (!stopCar) {
- // 璺濈妫�娴�
- vector<double> dtox;
- vector<Line> line_set;
- Line distance_line;
+ if (prime.pMotion->move != STOP && stopConfirm) {
+ // 杞﹁締浠庡仠姝㈢姸鎬佸啀娆＄Щ鍔�
+ double juli = DistanceOf(prime.pModeling->base_point, stopPoint);
- MakeLine(&distance_line, &map->point[0], &map->point[8]);
- line_set.push_back(distance_line);
- DistanceOfTire2X(dtox, car, line_set);
- MA_SendDistance(dtox[0], dtox[1]);
- } else {
- MA_SendDistance(distanceToStopLine, distanceToEdge);
- }
-
- // 鍒ゆ柇璧锋鍚庢粦鐘舵��
- if (stopCar) {
- if (IntersectionOfLine(map->point[4], stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) == 1) {
- // 鍙戠敓鍚庢粦
- double slideDistance = DistanceOf(stopPoint, car->carXY[car->axial[AXIAL_FRONT]]);
-
- if (slideDistance > examParam.ramp_slide_yellow_distance) {
+ if (juli > examParam.ramp_slide_yellow_distance) {
+ double deg = YawOf(stopPoint, prime.pModeling->base_point);
+ deg = fabs(prime.pModeling->yaw - deg);
+ if (deg > 180) {
+ deg = 360 - deg;
+ }
+ if (deg < 90) {
+ // 杞﹁締涓婂潯
+ if (!restartComplete) {
+ restartComplete = true;
+ AppTimer_delete(StoppingTimeout);
+ if (slideNormalDistance && !reportSlideFault) {
+ // 鍚庢粦瓒呰繃10鍘樼背锛屼絾娌¤秴杩�30鍘樼背
+ DEBUG("鍚庢粦瓒呰繃10鍘樼背锛屼絾娌¤秴杩�30鍘樼背");
+ reportSlideFault = true;
+ AddExamFault(10204);
+ }
+ if (!handBreakActive) {
+ // 妫�鏌ユ槸鍚︽媺浣忔墜鍒�
+ DEBUG("娌℃媺鎵嬪埞");
+ handBreakActive = true;
+ AddExamFault(20306);
+ }
+ }
+ } else {
+ // 杞﹁締鍚庢粦
slideNormalDistance = true;
- }
-
- if (slideDistance > examParam.ramp_slide_red_distance && !slideLongDistance && !reportSlideFault) {
- // 鍚庢粦瓒呰繃30鍘樼背, 涓嶅悎鏍�
- AddExamFault(10106, rtkTime);
- DEBUG("鍚庢粦瓒呰繃30鍘樼背");
- slideLongDistance = true;
- reportSlideFault = true;
- }
- }
-
-
- if (!reportStartTimeout && (IntersectionOfLine(map->point[4], stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) != -1 ||
- DistanceOf(stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) < 0.1)) {
- if (TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10) - stopCarTime > examParam.ramp_start_car_limit_time) {
- // 璧锋鏃堕棿瓒呰繃30绉掞紝涓嶅悎鏍�
- AddExamFault(20303, rtkTime);
- DEBUG("璧锋鏃堕棿瓒呰繃30绉�");
- reportStartTimeout = true;
- }
- }
-
- if (IntersectionOfLine(map->point[5], map->point[6], car->carXY[car->axial[AXIAL_REAR]]) == -1) {
- // 杞﹀熬椹惰繃鍋滄鏉�
- if (slideNormalDistance && !slideLongDistance && !reportSlideFault) {
- reportSlideFault = true;
- // 鍚庢粦瓒呰繃10鍘樼背锛屼絾娌¤秴杩�30鍘樼背
- AddExamFault(10204, rtkTime);
- DEBUG("鍚庢粦瓒呰繃10鍘樼背锛屼絾娌¤秴杩�30鍘樼背");
+ if (juli > examParam.ramp_slide_red_distance && !reportSlideFault) {
+ // 鍚庢粦瓒呰繃30鍘樼背, 涓嶅悎鏍�
+ DEBUG("鍚庢粦瓒呰繃30鍘樼背");
+ reportSlideFault = true;
+ AddExamFault(10106);
+ }
}
}
}
- if (!testing) {
- MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 0);
+ if (ExitTestArea(prime)) {
+ // 椹剁娴嬭瘯鍖�
+ if (!stopConfirm) {
+ // 涓嶅仠杞︾洿鎺ョ寮�
+ AddExamFault(10103);
+ }
+ MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_UPHILL, 0);
}
return testing ? 1 : 0;
}
// 杞﹁疆鏄惁鍘嬭竟绾�
-static bool CrashRedLine(const Polygon *map, const car_model *car)
+static bool CrashRedLine(prime_t &prime)
{
bool ret = false;
- Line red_line;
- int red_lines[2][2];
- int red_line_num = 0;
+ int red_lines[][2] = {{0, 1}, {1, 4}, {4, 5}, {5, 8}, {8, 9}};
- if (map->num == 10) {
- red_lines[0][0] = 0;
- red_lines[0][1] = 8;
- red_lines[1][0] = 8;
- red_lines[1][1] = 9;
- red_line_num = 2;
- } else {
- red_lines[0][0] = 0;
- red_lines[0][1] = 8;
- red_line_num = 1;
- }
Line frontAxle, rearAxle;
- MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
- MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
+ // 浠呯湅杞﹁疆澶栦晶
+ MAKE_LINE(frontAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
+ prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]]);
+ MAKE_LINE(rearAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
+ prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]);
- for (int i = 0; i < red_line_num; ++i) {
- MakeLine(&red_line, &map->point[red_lines[i][0]], &map->point[red_lines[i][1]]);
+ for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
+ Line red_line;
+ MAKE_LINE(red_line, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[red_lines[i][0]],
+ prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[red_lines[i][1]]);
+
if (IntersectionOf(red_line, frontAxle) == GM_Intersection ||
- IntersectionOf(red_line, rearAxle) == GM_Intersection) {
+ IntersectionOf(red_line, rearAxle) == GM_Intersection) {
ret = true;
break;
}
@@ -252,28 +238,21 @@
return ret;
}
-static double DistanceOfHead2Stopline(const Polygon *map, const car_model *car)
+static double DistanceOfHead2Stopline(prime_t &prime)
{
double dis = 0.0;
- int rel1 = IntersectionOfLine(map->point[4], map->point[3], car->carXY[car->body[0]]);
- int rel2 = IntersectionOfLine(map->point[5], map->point[6], car->carXY[car->body[0]]);
+ Line upper_edge, lower_edge;
- DEBUG("%d %d %f, %f", car->body[0], car->axial[AXIAL_FRONT], car->carXY[car->body[0]].X, car->carXY[car->body[0]].Y);
- DEBUG("rel1 %d rel2 %d", rel1, rel2);
+ MAKE_LINE(upper_edge, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[5],
+ prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[6]);
+ MAKE_LINE(lower_edge, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[4],
+ prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[3]);
- if (rel1 == 1) {
- Line line1;
-
- MakeLine(&line1, &map->point[4], &map->point[3]);
-
- dis = DistanceOf(car->carXY[car->body[0]], line1);
- } else if (rel2 == -1) {
- Line line2;
-
- MakeLine(&line2, &map->point[5], &map->point[6]);
-
- dis = DistanceOf(car->carXY[car->body[0]], line2);
+ if (IntersectionOfLine(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], upper_edge) == REL_POS_RIGHT) {
+ dis = DistanceOf(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], upper_edge);
+ } else if (IntersectionOfLine(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], lower_edge) == REL_POS_LEFT) {
+ dis = DistanceOf(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], lower_edge);
}
DEBUG("DistanceOfHead2Stopline dis %f", dis);
@@ -281,28 +260,27 @@
return dis;
}
-static double DistanceOfTire2Edge(const Polygon *map, const car_model *car)
+static double DistanceOfTire2Edge(prime_t &prime)
{
Line edge;
- MakeLine(&edge, &map->point[0], &map->point[8]);
+ MAKE_LINE(edge, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[0],
+ prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[8]);
- double l1 = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], edge);
+ double l1 = DistanceOf(prime.pModeling->points[prime.pModel->right_front_tire[TIRE_OUTSIDE]], edge);
- double l2 = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], edge);
-
-// return (l1+l2)/2.0;
+ double l2 = DistanceOf(prime.pModeling->points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]], edge);
return MAX(l1, l2); // 鍙栨渶杩滅殑
}
// 鏁翠釜杞﹁締閮借椹剁璇ユ祴璇曞尯鍩�
-static bool ExitTestArea(const Polygon *map, const car_model *car)
+static bool ExitTestArea(prime_t &prime)
{
// 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
bool ret = false;
- Polygon carBody, map2;
+ /*Polygon carBody, map2;
PointF vPoint = Calc3Point(map->point[8], map->point[0], DistanceOf(map->point[8], map->point[7]), 'R');
PointF vPoint2;
@@ -329,6 +307,6 @@
}
free(carBody.point);
- free(map2.point);
+ free(map2.point);*/
return ret;
}
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 0697693..220d1a8 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, 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);
+void StartSAS(int index, const Polygon *map, const car_model_t *car, int moveDirect, const struct RtkTime *rtkTime);
+int TestSAS(const Polygon *map, const car_model_t *car, const car_model_t *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime);
#endif //RTKDRIVERTEST_STOP_AND_START_H
diff --git a/lib/src/main/cpp/test_items/turn_a90.cpp b/lib/src/main/cpp/test_items/turn_a90.cpp
index a2eafb1..724c69d 100644
--- a/lib/src/main/cpp/test_items/turn_a90.cpp
+++ b/lib/src/main/cpp/test_items/turn_a90.cpp
@@ -2,6 +2,17 @@
// Created by YY on 2019/11/4.
//
+// 4----------------------|5
+// |
+// |
+// 3----------|2 |
+// | |
+// | |
+// | |
+// | |
+// | |
+// |1 |0
+
#include "turn_a90.h"
#include "../test_common/Geometry.h"
#include "../driver_test.h"
@@ -21,90 +32,60 @@
using namespace std;
static bool testing;
-static int mapIndex;
static int enterAreaHeading;
static bool turnLeftFinished;
-static uint32_t stopTimepoint = 0;
-
-static bool reportStopCarTimeout;
-static int prevMoveDirect;
static bool crashRedLine;
-static bool CrashRedLine(const Polygon *map, const car_model *car);
-static bool ExitTestArea(const Polygon *map, const car_model *car);
+static bool CrashRedLine(prime_t &prime);
+static bool ExitTestArea(prime_t &prime);
-void StartTurnA90(int index, int moveDirect, double heading, const struct RtkTime *rtkTime)
+void StartTurnA90(prime_t &prime)
{
DEBUG("杩涘叆鐩磋杞集鍦哄湴");
testing = true;
- enterAreaHeading = (int) heading;
- prevMoveDirect = moveDirect;
- if (moveDirect == 0) {
- stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- }
- reportStopCarTimeout = false;
+ enterAreaHeading = (int) prime.pModeling->yaw;
crashRedLine = false;
turnLeftFinished = false;
- mapIndex = index;
- MA_EnterMap(mapIndex, MAP_TYPE_TURN_90, 1);
+ MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_TURN_90, 1);
}
-int TestTurnA90(const Polygon *map, const car_model *car, const car_model *carPrev, double heading, double speed, int moveDirect, const struct RtkTime *rtkTime)
+static void StoppingTimeout(apptimer_var_t val) {
+ DEBUG("涓�斿仠杞�");
+ AddExamFault(20703);
+}
+
+void MotionChange(move_status_t mv)
{
- int az = (int) heading;
+ if (!testing)
+ return;
+
+ AppTimer_delete(StoppingTimeout);
+
+ if (mv == STOP) {
+ AppTimer_add(StoppingTimeout, D_SEC(2));
+ } else {
+
+ }
+}
+
+void TestTurnA90(prime_t &prime)
+{
+ int az = (int) prime.pModeling->yaw;
vector<double> dtox;
vector<Line> line_set;
Line distance_line;
- if (ExitTestArea(map, car)) {
- testing = false;
- goto TEST_END;
- }
-
- // 璺濈妫�娴�
- MakeLine(&distance_line, &map->point[0], &map->point[5]);
- line_set.push_back(distance_line);
- MakeLine(&distance_line, &map->point[5], &map->point[4]);
- line_set.push_back(distance_line);
- MakeLine(&distance_line, &map->point[1], &map->point[2]);
- line_set.push_back(distance_line);
- MakeLine(&distance_line, &map->point[2], &map->point[3]);
- line_set.push_back(distance_line);
- DistanceOfTire2X(dtox, car, line_set);
- MA_SendDistance(dtox[0], dtox[1]);
-
- if (CrashRedLine(map, car)) {
+ if (CrashRedLine(prime)) {
if (!crashRedLine) {
crashRedLine = true;
// 纰惧帇閬撹矾杈圭紭,涓嶅悎鏍�
- AddExamFault(20701, rtkTime);
+ AddExamFault(20701);
DEBUG("纰惧帇閬撹矾杈圭紭");
}
} else {
crashRedLine = false;
- }
-
- if (moveDirect != prevMoveDirect) {
- if (moveDirect == 0) {
- stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- reportStopCarTimeout = false;
-
- DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
- } else {
-
- }
- prevMoveDirect = moveDirect;
- } else if (moveDirect == 0) {
- uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
-
- if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.turn_a90_pause_criteria) && !reportStopCarTimeout) {
- // 鍋滆溅瓒�2绉掞紝姣忔鎵�5鍒�
- AddExamFault(20703, rtkTime);
- DEBUG("涓�斿仠杞�");
- reportStopCarTimeout = true;
- }
}
// 妫�鏌ヨ浆鍚戠姸鎬�
@@ -117,7 +98,7 @@
if (az >= 30) {
if (!turnLeftFinished) {
char turn_direct;
- if((( ((int)heading) + 360 - enterAreaHeading) % 360) < 180) {
+ if((( az + 360 - enterAreaHeading) % 360) < 180) {
DEBUG("鍙宠浆");
turn_direct = 'R';
} else {
@@ -128,39 +109,38 @@
if ((turn_direct == 'R' && ReadCarStatus(TURN_SIGNAL_LAMP) != RIGHT_TURN_LIGHT) ||
(turn_direct == 'L' && ReadCarStatus(TURN_SIGNAL_LAMP) != LEFT_TURN_LIGHT)) {
DEBUG("杞悜鐏湭寮�鍚�");
- AddExamFault(20702, rtkTime);
+ AddExamFault(20702);
}
}
turnLeftFinished = true;
}
- if (turnLeftFinished) {
-
+ if (ExitTestArea(prime)) {
+ testing = false;
+ MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_TURN_90, 0);
}
-
-TEST_END:
- if (!testing) {
- MA_EnterMap(mapIndex, MAP_TYPE_TURN_90, 0);
- }
- return testing? 1:0;
}
// 杞﹁疆鏄惁鍘嬭竟绾�
-static bool CrashRedLine(const Polygon *map, const car_model *car)
+static bool CrashRedLine(prime_t &prime)
{
bool ret = false;
- Line red_line;
const int red_lines[][2] = {{0, 5}, {5, 4}, {1, 2}, {2, 3}};
Line frontAxle, rearAxle;
// 浠呯湅杞﹁疆澶栦晶
- MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
- MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
+ MAKE_LINE(frontAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
+ prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]]);
+ MAKE_LINE(rearAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
+ prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]);
for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
- MakeLine(&red_line, &map->point[red_lines[i][0]], &map->point[red_lines[i][1]]);
+ Line red_line;
+ MAKE_LINE(red_line, prime.pMap->turn_a90_map[prime.curr_exam_map.map_idx].map[red_lines[i][0]],
+ prime.pMap->turn_a90_map[prime.curr_exam_map.map_idx].map[red_lines[i][1]]);
+
if (IntersectionOf(red_line, frontAxle) == GM_Intersection ||
IntersectionOf(red_line, rearAxle) == GM_Intersection) {
ret = true;
@@ -171,25 +151,41 @@
return ret;
}
-// 鏁翠釜杞﹁締閮借椹剁璇ユ祴璇曞尯鍩�
-static bool ExitTestArea(const Polygon *map, const car_model *car)
+// 4涓溅杞拰杞﹀ご鐐逛笉鍦ㄥ満鍦颁腑
+static bool ExitTestArea(prime_t &prime)
{
- bool ret = false;
+ Polygon polygon;
- // 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
- Polygon carBody;
-
- carBody.num = car->bodyNum;
- carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
- for (int i = 0; i < carBody.num; ++i) {
- carBody.point[i] = car->carXY[car->body[i]];
+ polygon.num = prime.pMap->turn_a90_map[prime.curr_exam_map.map_idx].map.size();
+ polygon.point = new PointF[polygon.num];
+ for (int i = 0; i < polygon.num; ++i) {
+ polygon.point[i] = prime.pMap->turn_a90_map[prime.curr_exam_map.map_idx].map[i];
}
- if (IntersectionOf(&carBody, map) == GM_None) {
- ret = true;
+ int num = 0;
+
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
+ &polygon) == GM_None) {
+ num++;
+ }
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
+ &polygon) == GM_None) {
+ num++;
+ }
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
+ &polygon) == GM_None) {
+ num++;
+ }
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]],
+ &polygon) == GM_None) {
+ num++;
+ }
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[prime.pModel->axial[AXIAL_FRONT]]],
+ &polygon) == GM_None) {
+ num++;
}
- free(carBody.point);
+ delete []polygon.point;
- return ret;
+ return num == 5? true : false;
}
diff --git a/lib/src/main/cpp/test_items/turn_a90.h b/lib/src/main/cpp/test_items/turn_a90.h
index 857285d..f82df8b 100644
--- a/lib/src/main/cpp/test_items/turn_a90.h
+++ b/lib/src/main/cpp/test_items/turn_a90.h
@@ -11,6 +11,6 @@
using namespace std;
void StartTurnA90(int index, int moveDirect, double heading, const struct RtkTime *rtkTime);
-int TestTurnA90(const Polygon *map, const car_model *car, const car_model *carPrev, double heading, double speed, int moveDirect, const struct RtkTime *rtkTime);
+int TestTurnA90(const Polygon *map, const car_model_t *car, const car_model_t *carPrev, double heading, double speed, int moveDirect, const struct RtkTime *rtkTime);
#endif //RTKDRIVERTEST_TURN_A90_H
diff --git a/lib/src/main/cpp/test_items2/car_start.cpp b/lib/src/main/cpp/test_items2/car_start.cpp
deleted file mode 100644
index d111adf..0000000
--- a/lib/src/main/cpp/test_items2/car_start.cpp
+++ /dev/null
@@ -1,102 +0,0 @@
-//
-// Created by YY on 2020/8/20.
-//
-
-#include "car_start.h"
-#include "../driver_test.h"
-#include "../test_common/odo_graph.h"
-#include "../native-lib.h"
-#include "../test_common/car_sensor.h"
-#include "road_exam.h"
-#include "../jni_log.h"
-#include "../common/apptimer.h"
-
-#define DEBUG(fmt, args...) LOGD("<road_exam car_start> <%s>: " fmt, __func__, ##args)
-
-static double startCarMoveDistance;
-static bool checkEngineRPM, checkStartCarSignal, checkDoor, handBreakActive;
-
-void cb(int seq)
-{
- DEBUG("璇煶缁撴潫 %d", seq);
-}
-
-void CarStartInit(void)
-{
- startCarMoveDistance = ReadOdo();
- checkEngineRPM = false;
- checkStartCarSignal = false;
- checkDoor = false;
- handBreakActive = false;
- PlayTTS(examParam.start_car_begin_tts, cb);
-
- DEBUG("杞﹁締璧锋");
-}
-
-bool TestCarStart(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
-{
- car_sensor_value_t sensor;
- double moveDistance = ReadOdo() - startCarMoveDistance;
-
-// DEBUG("璧锋琛岄┒璺濈 %f", moveDistance);
-
- if (!checkStartCarSignal && moveDirect == 1) {
- checkStartCarSignal = true;
-
- sensor = ReadCarSensorValue(TURN_SIGNAL_LAMP);
- if (sensor.name == TURN_SIGNAL_LAMP) {
- if (sensor.value != LEFT_TURN_LIGHT) {
- DEBUG("鍙樿皟鏈墦鐏�!!");
- // 娌℃墦鐏紝涓嶅悎鏍�
- AddExamFault(30121, rtkTime);
- } else if (TimeGetDiff(rtkTime, &sensor.time) < D_SEC(3)) {
- DEBUG("杞悜鐏椂闂翠笉瓒�");
- // 涓嶈冻3绉掞紝涓嶅悎鏍�
- AddExamFault(30122, rtkTime);
- }
- }
- }
-
- if (moveDistance > examParam.start_car_limit_distance) {
- /*sensor = ReadCarSensorValue(HAND_BREAK);
- if (sensor.name == HAND_BREAK && sensor.value == 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);
- }*/
-
- PlayTTS(examParam.start_car_end_tts, NULL);
- DEBUG("############# 瀹屾垚璧锋 ############");
-
- return false;
- } else if (moveDistance >= examParam.open_door_drive_allow_distance) {
- if (!checkDoor) {
- checkDoor = true;
-
- sensor = ReadCarSensorValue(DOOR);
- if (sensor.name == DOOR && sensor.value == DOOR_OPEN) {
- // 杞﹂棬鏈畬鍏ㄥ叧闂紝涓嶅悎鏍�
- DEBUG("杞﹂棬鏈叧闂�");
- AddExamFault(40202, rtkTime);
- }
-
- sensor = ReadCarSensorValue(HAND_BREAK);
- if (sensor.name == HAND_BREAK && sensor.value == BREAK_ACTIVE) {
- handBreakActive = true;
- }
- }
- }
-
- if (ReadCarStatus(ENGINE_RPM) > examParam.start_car_max_rpm && !checkEngineRPM) {
- // 杞�熻秴鏍囷紝涓嶅悎鏍�
- DEBUG("杞�熻秴鏍�");
- AddExamFault(40210, rtkTime);
- checkEngineRPM = true;
- }
-
- return true;
-}
diff --git a/lib/src/main/cpp/test_items2/car_start.h b/lib/src/main/cpp/test_items2/car_start.h
deleted file mode 100644
index 5c9d99a..0000000
--- a/lib/src/main/cpp/test_items2/car_start.h
+++ /dev/null
@@ -1,13 +0,0 @@
-//
-// Created by YY on 2020/8/20.
-//
-
-#ifndef MYAPPLICATION2_CAR_START_H
-#define MYAPPLICATION2_CAR_START_H
-
-#include "../driver_test.h"
-
-void CarStartInit(void);
-bool TestCarStart(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime);
-
-#endif //MYAPPLICATION2_CAR_START_H
diff --git a/lib/src/main/cpp/test_items2/change_lane.cpp b/lib/src/main/cpp/test_items2/change_lane.cpp
deleted file mode 100644
index c9f305c..0000000
--- a/lib/src/main/cpp/test_items2/change_lane.cpp
+++ /dev/null
@@ -1,51 +0,0 @@
-//
-// Created by YY on 2020/8/21.
-//
-
-#include "change_lane.h"
-#include "../test_common/odo_graph.h"
-#include "../native-lib.h"
-#include "../jni_log.h"
-#include "../driver_test.h"
-
-#define DEBUG(fmt, args...) LOGD("<road_exam change_lane> <%s>: " fmt, __func__, ##args)
-
-static double maxMoveDistance;
-static int originalLane;
-static bool start;
-
-static void TtsBack(int seq)
-{
- maxMoveDistance = ReadOdo();
- start = true;
- DEBUG("鍙橀亾寮�濮嬮噷绋� %f", maxMoveDistance);
-}
-
-void StartChaneLaneExam(int ori_lane)
-{
- DEBUG("鍙樻洿杞﹂亾");
-
- start = false;
- originalLane = ori_lane;
- PlayTTS(examParam.change_lane_begin_tts, TtsBack);
-}
-
-bool TestChangeLane(int currLane, const struct RtkTime *rtkTime)
-{
- if (!start)
- return true;
- if (originalLane != currLane) {
- DEBUG("瀹屾垚鍙橀亾");
- PlayTTS(examParam.change_lane_end_tts, NULL);
- return false;
- }
-
- if (ReadOdo() - maxMoveDistance > examParam.change_lane_limit_distance) {
- // 瓒呰溅鏈畬鎴�
- DEBUG("鍙橀亾鍥哄畾璺濈鍐呮湭瀹屾垚 褰撳墠閲岀▼ %f", ReadOdo());
- AddExamFault(30103, rtkTime);
- return false;
- }
-
- return true;
-}
diff --git a/lib/src/main/cpp/test_items2/change_lane.h b/lib/src/main/cpp/test_items2/change_lane.h
deleted file mode 100644
index efa7d95..0000000
--- a/lib/src/main/cpp/test_items2/change_lane.h
+++ /dev/null
@@ -1,12 +0,0 @@
-//
-// Created by YY on 2020/8/21.
-//
-
-#ifndef MYAPPLICATION2_CHANGE_LANE_H
-#define MYAPPLICATION2_CHANGE_LANE_H
-
-void StartChaneLaneExam(int ori_lane);
-
-bool TestChangeLane(int currLane, const struct RtkTime *rtkTime);
-
-#endif //MYAPPLICATION2_CHANGE_LANE_H
diff --git a/lib/src/main/cpp/test_items2/common_check.cpp b/lib/src/main/cpp/test_items2/common_check.cpp
deleted file mode 100644
index 3dde24c..0000000
--- a/lib/src/main/cpp/test_items2/common_check.cpp
+++ /dev/null
@@ -1,38 +0,0 @@
-//
-// Created by fctom on 2020/2/27.
-//
-
-#include "common_check.h"
-#include "../test_common/car_sensor.h"
-#include "../driver_test.h"
-
-// 鐢变汉宸ョ‘璁�
-// 2 閬尅杞﹀唴鐩戞帶璁惧
-// 3 涓嶆寜鑰冭瘯鍛樻寚浠よ椹�
-// 9 涓嶆寜浜ら�氫俊鍙枫�佹爣绾胯椹�
-// 28 璧锋鏃惰溅杈嗗彂鐢熼棷鍔�
-
-static int prevMoveDirect;
-
-void StartCheck(int moveDirect, double speed, const struct RtkTime *rtkTime)
-{
- if (prevMoveDirect != moveDirect) {
-
- if (prevMoveDirect == 0) {
- // 璧锋鎸傞敊鎸★紝涓嶅悎鏍�
-// if (ReadSensor(SENSOR_SHIFT_1) != 1) {
-// AddExamFault(19, rtkTime);
-// }
-
- // 璧锋娌℃湁鍏抽棴杞﹂棬锛屼笉鍚堟牸
-// if (ReadSensor(SENSOR_DOOR) != 1) {
-// AddExamFault(23, rtkTime);
-// }
-
- // 鍚庢粦鍒ゆ柇
-
- }
-
- prevMoveDirect = moveDirect;
- }
-}
diff --git a/lib/src/main/cpp/test_items2/common_check.h b/lib/src/main/cpp/test_items2/common_check.h
deleted file mode 100644
index 5860628..0000000
--- a/lib/src/main/cpp/test_items2/common_check.h
+++ /dev/null
@@ -1,8 +0,0 @@
-//
-// Created by fctom on 2020/2/27.
-//
-
-#ifndef MYAPPLICATION2_COMMON_CHECK_H
-#define MYAPPLICATION2_COMMON_CHECK_H
-
-#endif //MYAPPLICATION2_COMMON_CHECK_H
diff --git a/lib/src/main/cpp/test_items2/drive_straight.cpp b/lib/src/main/cpp/test_items2/drive_straight.cpp
deleted file mode 100644
index 21156f2..0000000
--- a/lib/src/main/cpp/test_items2/drive_straight.cpp
+++ /dev/null
@@ -1,96 +0,0 @@
-//
-// Created by YY on 2020/3/23.
-//
-
-#include "drive_straight.h"
-#include "../driver_test.h"
-#include "../native-lib.h"
-#include "../jni_log.h"
-#include "road_exam.h"
-#include "../master/comm_if.h"
-#include "../common/apptimer.h"
-#include "../test_common/odo_graph.h"
-#include <cmath>
-
-#define DEBUG(fmt, args...) LOGD("<road_exam drive_straight> <%s>: " fmt, __func__, ##args)
-
-static int setup;
-static double beginOdo;
-static int yaw_stat;
-static double offsetBase;
-
-static void TtsBack(int seq)
-{
- setup = 1;
-}
-
-void StartDriveStraightExam(void) {
- DEBUG("寮�濮嬬洿绾胯椹�");
- setup = 0;
- yaw_stat = 0;
- PlayTTS(examParam.straight_begin_tts, TtsBack);
-}
-
-bool TestDriveStraight(road_exam_map &RoadMap, int roadIndex, const car_model *car, const struct RtkTime *rtkTime) {
- static double yaw;
- static Line baseLine;
- static double offset1, offset2;
-
- if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
- return false;
-
- if (setup == 1) {
- // 鍋忚埅瑙掑钩鍧囧��
- static vector<double> yaws;
-
- if (yaw_stat == 0) {
- yaws.clear();
- }
- yaws.push_back(car->yaw);
- yaw_stat++;
-
- if (yaw_stat == 15) {
- yaw = AvgYaw(yaws);
- vector<double>().swap(yaws);
-
- DEBUG("鍒涘缓鍩虹嚎 yaw %f", yaw);
- PointF extPoint = PointExtend(car->basePoint, 100, yaw);
- MakeLine(&baseLine, &car->basePoint, &extPoint);
- beginOdo = ReadOdo();
-
- PointF px = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, car->carXY[car->axial[AXIAL_FRONT]]);
- offsetBase = DistanceOf(px, car->carXY[car->axial[AXIAL_FRONT]]);
- DEBUG("璧风偣鍋忕 %f", offsetBase);
-
- setup = 2;
- }
- } else if (setup == 2) {
- offset1 = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine);
-
- if (offset1 > examParam.straight_max_offset) {
- DEBUG("铏氭嫙鐩寸嚎鍋忕Щ澶т簬30鍘樼背 offset1 = %f", offset1);
-// // 鍋忕Щ澶т簬30鍘樼背锛屼笉鍚堟牸
-// AddExamFault(30, rtkTime);
-// return false;
- }
-
- PointF px = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, car->carXY[car->axial[AXIAL_FRONT]]);
- offset2 = DistanceOf(px, car->carXY[car->axial[AXIAL_FRONT]]);
-
- if (fabs(offset2 - offsetBase) > examParam.straight_max_offset) {
- DEBUG("鐩寸嚎鍋忕Щ澶т簬30鍘樼背 offset2 = %f", fabs(offset2 - offsetBase));
- // 鍋忕Щ澶т簬30鍘樼背锛屼笉鍚堟牸
- AddExamFault(40301, rtkTime);
- return false;
- }
- }
-
- if (setup == 2 && ReadOdo() - beginOdo > examParam.straight_limit_distance) {
- DEBUG("鐩寸嚎琛岄┒缁撴潫 offset1 = %f offset2 = %f", offset1, fabs(offset2 - offsetBase));
-
- PlayTTS(examParam.straight_end_tts, NULL);
- return false;
- }
-
- return true;
-}
diff --git a/lib/src/main/cpp/test_items2/drive_straight.h b/lib/src/main/cpp/test_items2/drive_straight.h
deleted file mode 100644
index 9d463f1..0000000
--- a/lib/src/main/cpp/test_items2/drive_straight.h
+++ /dev/null
@@ -1,13 +0,0 @@
-//
-// Created by YY on 2020/3/23.
-//
-
-#ifndef MYAPPLICATION2_DRIVE_STRAIGHT_H
-#define MYAPPLICATION2_DRIVE_STRAIGHT_H
-
-#include "../driver_test.h"
-
-void StartDriveStraightExam(void);
-bool TestDriveStraight(road_exam_map &RoadMap, int roadIndex, const car_model *car, const struct RtkTime *rtkTime);
-
-#endif //MYAPPLICATION2_DRIVE_STRAIGHT_H
diff --git a/lib/src/main/cpp/test_items2/dummy_light.cpp b/lib/src/main/cpp/test_items2/dummy_light.cpp
deleted file mode 100644
index 411e37c..0000000
--- a/lib/src/main/cpp/test_items2/dummy_light.cpp
+++ /dev/null
@@ -1,142 +0,0 @@
-//
-// Created by YY on 2020/3/9.
-//
-
-#include <cstdlib>
-#include "../common/apptimer.h"
-#include "dummy_light.h"
-#include "../native-lib.h"
-#include "../jni_log.h"
-#include "../test_common/car_sensor.h"
-
-#define DEBUG(fmt, args...) LOGD("<road_exam dummy_light> <%s>: " fmt, __func__, ##args)
-
-static struct RtkTime currRtkTime;
-static struct dummy_light_exam *content;
-static int contentNum;
-
-static int question;
-
-static vector<int> process;
-
-static bool testing;
-
-static void CheckSolution(apptimer_var_t val);
-static void ExamDummyLight(void);
-
-void StartDummyLightExam(struct dummy_light_exam *ptr, int num, const struct RtkTime* rtkTime)
-{
- DEBUG("StartDummyLightExam 椤圭洰鏁伴噺 %d", num);
- content = ptr;
- contentNum = num;
- question = 0;
-
- if (content != NULL && num > 0) {
- DEBUG("鍚姩鐏厜");
- currRtkTime = *rtkTime;
-
- for (int i = 0; i < contentNum; ++i) {
- DEBUG("鐏厜椤圭洰 <%d> item %d, TTS %s", i, content[i].item, content[i].tts.c_str());
- for (int j = 0; j < content[i].process.size(); ++j) {
- DEBUG("鐏厜杩囩▼瑙�<%d> %d - %d", i, (content[i].process[j] >> 8) & 0xFF, content[i].process[j] & 0xFF);
- }
- for (int j = 0; j < content[i].solution.size(); ++j) {
- DEBUG("鐏厜鏈�缁堣В<%d> %d - %d", i, (content[i].solution[j] >> 8) & 0xFF, content[i].solution[j] & 0xFF);
- }
- }
- testing = true;
-
- AppTimer_delete(CheckSolution);
- ExamDummyLight();
- } else {
- DEBUG("鏃犵伅鍏夎�冭瘯鍐呭");
- testing = false;
- }
-}
-
-bool ExecuteDummyLightExam(const struct RtkTime* rtkTime)
-{
- currRtkTime = *rtkTime;
- return testing;
-}
-
-void DummyLightTTSDone(int id)
-{
- // 绛夎闊虫挱鎶ュ畬姣曞悗璁℃椂
- if (testing) {
- vector<int>().swap(process);
-
- // 棰勮鍙栨湁涓棿鎿嶄綔姝ラ鐨勭伅鍏�
- for (int i = 0; i < content[question].process.size(); ++i) {
- if (ReadCarStatus((content[question].process[i]>>8) & 0xFF) == content[question].process[i] & 0xFF) {
- process.push_back(content[question].process[i]);
- DEBUG("鐏厜鎿嶄綔杩囩▼瑙�1<%d> %d - %d", question, (content[question].process[i]>>8) & 0xFF, content[question].process[i] & 0xFF);
- }
- }
-
- AppTimer_delete(CheckSolution);
- AppTimer_add(CheckSolution, D_SEC(5));
- }
-}
-
-void TerminateDummyLightExam(void)
-{
- testing = false;
- AppTimer_delete(CheckSolution);
-}
-
-// 璁板綍涓棿杩囩▼
-void handleLigthExam(uint16_t id, int value)
-{
- if (testing) {
- for (int i = 0; i < content[question].process.size(); ++i) {
- if (id == ((content[question].process[i] >> 8) & 0xFF) && value == (content[question].process[i] & 0xFF)) {
- DEBUG("鐏厜鎿嶄綔杩囩▼瑙�2<%d> %d - %d", question, (content[question].process[i]>>8) & 0xFF, content[question].process[i] & 0xFF);
- if (process.size() == 0 || process.back() != content[question].process[i]) {
- process.push_back(content[question].process[i]);
- }
- }
- }
- }
-}
-
-// 妫�鏌ユ渶缁堢姸鎬�
-static void CheckSolution(apptimer_var_t val)
-{
- if (content[question].process.size() > 0) {
- if (content[question].process.size() != process.size()) {
- AddExamFault(content[question].wrongCode, &currRtkTime);
- goto NEXT_LIGHT;
- } else {
- // 鎿嶄綔椤哄簭涔熻婊¤冻
- for (int i = 0; i < content[question].process.size(); ++i) {
- if (process[i] != content[question].process[i]) {
- AddExamFault(content[question].wrongCode, &currRtkTime);
- goto NEXT_LIGHT;
- }
- }
- }
- }
-
- for (int i = 0; i < content[question].solution.size(); ++i) {
- if (ReadCarStatus((content[question].solution[i]>>8)&0xFF) != (content[question].solution[i] & 0xFF)) {
- DEBUG("鐏厜纭涓嶅悎鏍�<%d> %d != %d", question, (content[question].solution[i]>>8) & 0xFF, content[question].solution[i] & 0xFF);
- AddExamFault(content[question].wrongCode, &currRtkTime);
- break;
- }
- }
-
-NEXT_LIGHT:
- question++;
- ExamDummyLight();
-}
-
-static void ExamDummyLight(void)
-{
- if (testing && question < contentNum) {
- DEBUG("鐏厜棰樼洰锛� %s", content[question].tts.c_str());
- PlayTTS(content[question].tts, DummyLightTTSDone);
- } else {
- testing = false;
- }
-}
diff --git a/lib/src/main/cpp/test_items2/dummy_light.h b/lib/src/main/cpp/test_items2/dummy_light.h
deleted file mode 100644
index 129a0f1..0000000
--- a/lib/src/main/cpp/test_items2/dummy_light.h
+++ /dev/null
@@ -1,41 +0,0 @@
-//
-// Created by YY on 2020/3/9.
-//
-
-#ifndef MYAPPLICATION2_DUMMY_LIGHT_H
-#define MYAPPLICATION2_DUMMY_LIGHT_H
-
-#include "../driver_test.h"
-#include <vector>
-#include <string>
-
-enum {
- TURN_ON_DIPPED_LAMP = 1,
- DRIVE_AT_NIGHT,
- BRIDGE_MEET_CAR,
- TURN_ON_MAIN_BEAM_LAMP,
- FOLLOW_CAR,
- DRIVE_IN_FOG,
- THROUGE_CROSSWALK,
- CAR_FAULT,
- THROUGE_CURVE,
- THROUGE_CROSSROADS,
- PARK_CAR_TEMP,
- OVERTAKE
-};
-
-struct dummy_light_exam {
- int item;
- int wrongCode;
- std::vector<int> process;
- std::vector<int> solution;
- std::string tts;
-};
-
-void StartDummyLightExam(struct dummy_light_exam *ptr, int num, const struct RtkTime* rtkTime);
-bool ExecuteDummyLightExam(const struct RtkTime* rtkTime);
-void DummyLightTTSDone(int id);
-void TerminateDummyLightExam(void);
-void handleLigthExam(uint16_t id, int value);
-
-#endif //MYAPPLICATION2_DUMMY_LIGHT_H
diff --git a/lib/src/main/cpp/test_items2/operate_gear.cpp b/lib/src/main/cpp/test_items2/operate_gear.cpp
deleted file mode 100644
index f9e9fba..0000000
--- a/lib/src/main/cpp/test_items2/operate_gear.cpp
+++ /dev/null
@@ -1,148 +0,0 @@
-//
-// Created by YY on 2020/3/25.
-//
-
-#include "operate_gear.h"
-#include "../driver_test.h"
-#include "../common/apptimer.h"
-#include "../native-lib.h"
-#include "../test_common/car_sensor.h"
-#include "../jni_log.h"
-#include "../test_common/odo_graph.h"
-#include "road_exam.h"
-
-#define DEBUG(fmt, args...) LOGD("<road_exam operate_gear> <%s>: " fmt, __func__, ##args)
-
-static int expectGear;
-static int upDownShift;
-static int oldGear;
-
-static int setup;
-
-static double maxMoveDistance;
-
-static void TtsBack(int seq)
-{
- maxMoveDistance = ReadOdo();
- setup = 1;
-}
-
-void StartOperateGearExam(void) {
- DEBUG("寮�濮嬪姞鍑忔尅鎿嶄綔");
-
- setup = 0;
-
- PlayTTS(examParam.shift_begin_tts, TtsBack);
-}
-
-bool TestOperateGear(const struct RtkTime *rtkTime)
-{
- static struct RtkTime shiftTime;
-
- car_sensor_value_t sensor = ReadCarSensorValue(GEAR);
-
- if (sensor.name != GEAR)
- return false;
-
- if (setup == 0) {
- return true;
- } else if (setup == 1) {
- if (sensor.value != GEAR_N) {
- oldGear = sensor.value;
- DEBUG("鍑嗗棣栨鎹㈡尅 GEAR = %d", oldGear);
- }
- switch (sensor.value) {
- case GEAR_1: {
- expectGear = GEAR_2;
- upDownShift = 1;
- PlayTTS("璇峰姞鍒颁簩鎸�", NULL);
- setup = 2;
- break;
- }
- case GEAR_2: {
- expectGear = GEAR_3;
- upDownShift = 1;
- PlayTTS("璇峰姞鍒颁笁鎸�", NULL);
- setup = 2;
- break;
- }
- case GEAR_3: {
- expectGear = GEAR_2;
- upDownShift = -1;
- PlayTTS("璇峰噺鍒颁簩鎸�", NULL);
- setup = 2;
- break;
- }
- case GEAR_4: {
- expectGear = GEAR_3;
- upDownShift = -1;
- PlayTTS("璇峰噺鍒颁笁鎸�", NULL);
- setup = 2;
- break;
- }
- case GEAR_5: {
- upDownShift = -1;
- expectGear = GEAR_4;
- PlayTTS("璇峰噺鍒板洓鎸�", NULL);
- setup = 2;
- break;
- }
- }
- } else if (setup == 2) {
- if (sensor.value == GEAR_N || sensor.value == oldGear) {
- DEBUG("绛夊緟棣栨鎹㈡尅");
- } else if (sensor.value != expectGear) {
- // 鏈寜鎸囦护鎿嶄綔鎸′綅锛屼笉鍚堟牸
- DEBUG("棣栨鎹㈡尅閿欒 GEAR %d 甯屾湜 %d", sensor.value, expectGear);
- AddExamFault(40401, rtkTime);
- return false;
- } else {
- // 鍦ㄦ鎸′綅琛岄┒涓�瀹氳窛绂伙紝鍐嶆墽琛屼笅涓�涓�
- DEBUG("棣栨鎹㈡尅鎴愬姛锛屼笅涓�涓�...");
- oldGear = expectGear;
- shiftTime = *rtkTime;
- setup = 3;
- }
- } else if (setup == 3) {
- if (TimeGetDiff(rtkTime, &shiftTime) >= examParam.shift_hold_time) {
- setup = 4;
- char buff[128];
- expectGear += 0 - upDownShift;
- sprintf(buff, "璇�%s鍒�%d鎸�", upDownShift > 0 ? "鍑�": "鍔�", expectGear - GEAR_N);
- PlayTTS(buff, NULL);
-
- DEBUG("%s", buff);
- DEBUG("鍑嗗浜屾鎹㈡尅 甯屾湜 %d", expectGear);
- }
- }
- else if (setup == 4) {
- if (sensor.value == GEAR_N || sensor.value == oldGear) {
- DEBUG("绛夊緟浜屾鎹㈡尅");
- } else if (sensor.value != expectGear) {
- // 鏈寜鎸囦护鎿嶄綔鎸′綅锛屼笉鍚堟牸
- DEBUG("浜屾鎹㈡尅閿欒 GEAR %d 甯屾湜 %d", sensor.value, expectGear);
- AddExamFault(40401, rtkTime);
- return false;
- } else {
- // 鍦ㄦ鎸′綅琛岄┒涓�瀹氳窛绂伙紝鍐嶆墽琛屼笅涓�涓�
- shiftTime = *rtkTime;
- setup = 5;
- DEBUG("浜屾鎹㈡尅鎴愬姛锛屼笅涓�涓�...");
- }
- } else if (setup == 5) {
- if (TimeGetDiff(rtkTime, &shiftTime) >= examParam.shift_hold_time) {
- PlayTTS(examParam.shift_end_tts, NULL);
- DEBUG("鍔犲噺鎸′綅鎿嶄綔缁撴潫");
- return false;
- }
- }
-
- if (ReadOdo() - maxMoveDistance > 120) {
- // 鏈寜鎸囦护鎿嶄綔鎸′綅锛屼笉鍚堟牸
- DEBUG("鏈寜鎸囦护鎿嶄綔鎸′綅锛岃秴鏃�");
- AddExamFault(40401, rtkTime);
- return false;
- }
-
- return true;
-}
diff --git a/lib/src/main/cpp/test_items2/operate_gear.h b/lib/src/main/cpp/test_items2/operate_gear.h
deleted file mode 100644
index 52c1daa..0000000
--- a/lib/src/main/cpp/test_items2/operate_gear.h
+++ /dev/null
@@ -1,13 +0,0 @@
-//
-// Created by YY on 2020/3/25.
-//
-
-#ifndef MYAPPLICATION2_OPERATE_GEAR_H
-#define MYAPPLICATION2_OPERATE_GEAR_H
-
-#include "../driver_test.h"
-
-void StartOperateGearExam(void);
-bool TestOperateGear(const struct RtkTime *rtkTime);
-
-#endif //MYAPPLICATION2_OPERATE_GEAR_H
diff --git a/lib/src/main/cpp/test_items2/overtake.cpp b/lib/src/main/cpp/test_items2/overtake.cpp
deleted file mode 100644
index 74a7b97..0000000
--- a/lib/src/main/cpp/test_items2/overtake.cpp
+++ /dev/null
@@ -1,71 +0,0 @@
-//
-// Created by YY on 2020/8/21.
-//
-
-#include "overtake.h"
-#include "../native-lib.h"
-#include "../jni_log.h"
-#include "../test_common/odo_graph.h"
-#include "../driver_test.h"
-
-#define DEBUG(fmt, args...) LOGD("<road_exam overtake> <%s>: " fmt, __func__, ##args)
-
-static double maxMoveDistance;
-static int setup;
-static int originalLane;
-
-static void TtsBack(int seq)
-{
- maxMoveDistance = ReadOdo();
- setup = 1;
- DEBUG("瓒呰溅寮�濮嬮噷绋� %f", maxMoveDistance);
-}
-
-void StartOvertakeExam(int ori_lane)
-{
- DEBUG("瓒呰秺鍓嶆柟杞﹁締");
-
- PlayTTS(examParam.overtake_begin_tts, TtsBack);
-
- setup = 0;
- originalLane = ori_lane;
-}
-
-bool TestOvertake(int currLane, const struct RtkTime *rtkTime)
-{
- if (setup == 0) {
- return true;
- }
- if (setup == 1) {
- if (originalLane == currLane) {
-
- } else if (currLane + 1 == originalLane) {
- setup = 2;
- originalLane = currLane;
- } else {
- DEBUG("鍙充晶瓒呰溅");
- AddExamFault(30103, rtkTime);
- return false;
- }
- } else if (setup == 2) {
- if (originalLane == currLane) {
-
- } else if (currLane == originalLane + 1) {
- PlayTTS(examParam.overtake_end_tts, NULL);
- return false;
- } else {
- DEBUG("瓒呰溅杩濊鍙橀亾");
- AddExamFault(30103, rtkTime);
- return false;
- }
- }
-
- if (ReadOdo() - maxMoveDistance > examParam.overtake_limit_distance) {
- // 瓒呰溅鏈畬鎴�
- DEBUG("瓒呰溅鍥哄畾璺濈鍐呮湭瀹屾垚 褰撳墠閲岀▼ %f", ReadOdo());
- AddExamFault(30103, rtkTime);
- return false;
- }
-
- return true;
-}
diff --git a/lib/src/main/cpp/test_items2/overtake.h b/lib/src/main/cpp/test_items2/overtake.h
deleted file mode 100644
index 4ac10f9..0000000
--- a/lib/src/main/cpp/test_items2/overtake.h
+++ /dev/null
@@ -1,12 +0,0 @@
-//
-// Created by YY on 2020/8/21.
-//
-
-#ifndef MYAPPLICATION2_OVERTAKE_H
-#define MYAPPLICATION2_OVERTAKE_H
-
-void StartOvertakeExam(int ori_lane);
-
-bool TestOvertake(int currLane, const struct RtkTime *rtkTime);
-
-#endif //MYAPPLICATION2_OVERTAKE_H
diff --git a/lib/src/main/cpp/test_items2/prepare.cpp b/lib/src/main/cpp/test_items2/prepare.cpp
deleted file mode 100644
index 4cb5f34..0000000
--- a/lib/src/main/cpp/test_items2/prepare.cpp
+++ /dev/null
@@ -1,105 +0,0 @@
-//
-// Created by YY on 2020/12/16.
-//
-
-#include "prepare.h"
-#include "../native-lib.h"
-#include "../driver_test.h"
-#include "../test_common/car_sensor.h"
-#include "../common/apptimer.h"
-#include "../jni_log.h"
-
-#define DEBUG(fmt, args...) LOGD("<prepare> <%s>: " fmt, __func__, ##args)
-
-static int touch[4];
-static int hint_cnt;
-static bool exec = false;
-
-static void TtsBack(int seq);
-static void PrepareTimeout(apptimer_var_t val);
-static void PrepareTimeout2(apptimer_var_t val);
-
-void StartPrepare(void)
-{
- exec = true;
- hint_cnt = 0;
- memset(touch, 0, sizeof(touch));
- PlayTTS(examParam.prepare_tts, TtsBack);
- DEBUG("寮�濮嬩笂杞﹀噯澶�");
-}
-
-void TerminatePrepare(void)
-{
- exec = false;
- AppTimer_delete(PrepareTimeout);
- AppTimer_delete(PrepareTimeout2);
-}
-
-void handlePrepare(uint16_t sensor_id, int value)
-{
- if (!exec)
- return;
- if (sensor_id == SEATBELT) {
- if (value == INSERT_SEATBELT) {
- AppTimer_delete(PrepareTimeout);
- // 妫�鏌ョ粫杞︿紶鎰熷櫒鎯呭喌
- if (touch[0] + touch[1] + touch[2] + touch[3] != 4) {
- exec = false;
- PrepareOver(-1);
- } else {
- exec = false;
- PrepareOver(0);
- }
- } else {
- // 瀹夊叏甯﹁В寮�
- AppTimer_delete(PrepareTimeout2);
- }
- }
-
- if (sensor_id == SURROUND_CAR_1 && value == SURROUND_CAR_ACTIVE && touch[0] != 1) {
- touch[0] = 1;
- PlayTTS(examParam.touch_leftfront_tts, NULL);
- DEBUG("瑙︽懜浼犳劅鍣�1");
- }
- if (sensor_id == SURROUND_CAR_2 && value == SURROUND_CAR_ACTIVE && touch[1] != 1) {
- touch[1] = 1;
- PlayTTS(examParam.touch_leftrear_tts, NULL);
- DEBUG("瑙︽懜浼犳劅鍣�2");
- }
- if (sensor_id == SURROUND_CAR_3 && value == SURROUND_CAR_ACTIVE && touch[2] != 1) {
- touch[2] = 1;
- PlayTTS(examParam.touch_rightrear_tts, NULL);
- DEBUG("瑙︽懜浼犳劅鍣�3");
- }
- if (sensor_id == SURROUND_CAR_4 && value == SURROUND_CAR_ACTIVE && touch[3] != 1) {
- touch[3] = 1;
- PlayTTS(examParam.touch_rightfront_tts, NULL);
- DEBUG("瑙︽懜浼犳劅鍣�4");
- }
-}
-
-static void TtsBack(int seq)
-{
- if (hint_cnt == 0) {
- if (ReadCarStatus(SEATBELT) == INSERT_SEATBELT) {
- // 鍐嶆鎻愮ず
- AppTimer_add(PrepareTimeout2, D_SEC(10));
- DEBUG("璁剧疆鍐嶆鎻愰啋");
- }
- AppTimer_add(PrepareTimeout, D_MIN(2));
- DEBUG("璁剧疆2鍒嗛挓瓒呮椂");
- }
- hint_cnt++;
-}
-
-static void PrepareTimeout(apptimer_var_t val)
-{
- DEBUG("涓婅溅鍑嗗瓒呮椂");
- exec = false;
- PrepareOver(-2);
-}
-
-static void PrepareTimeout2(apptimer_var_t val)
-{
- PlayTTS(examParam.prepare_tts, TtsBack);
-}
diff --git a/lib/src/main/cpp/test_items2/prepare.h b/lib/src/main/cpp/test_items2/prepare.h
deleted file mode 100644
index 0f197df..0000000
--- a/lib/src/main/cpp/test_items2/prepare.h
+++ /dev/null
@@ -1,15 +0,0 @@
-//
-// Created by YY on 2020/12/16.
-//
-
-#ifndef MYAPPLICATION2_PREPARE_H
-#define MYAPPLICATION2_PREPARE_H
-
-#include <cstdint>
-
-void StartPrepare(void);
-void TerminatePrepare(void);
-void handlePrepare(uint16_t sensor_id, int value);
-void PrepareOver(int res);
-
-#endif //MYAPPLICATION2_PREPARE_H
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
deleted file mode 100644
index ad015dd..0000000
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ /dev/null
@@ -1,2150 +0,0 @@
-//
-// Created by YY on 2020/3/17.
-//
-
-#include "road_exam.h"
-#include "../driver_test.h"
-#include "../utils/xconvert.h"
-#include "../common/apptimer.h"
-#include "../jni_log.h"
-#include "../defs.h"
-#include "../test_common/car_sensor.h"
-#include "../native-lib.h"
-#include "through_something.h"
-#include "../master/comm_if.h"
-#include "drive_straight.h"
-#include "stop_car.h"
-#include "operate_gear.h"
-#include "../test_common/odo_graph.h"
-#include "car_start.h"
-#include "overtake.h"
-#include "change_lane.h"
-#include "prepare.h"
-
-#include <cmath>
-#include <vector>
-#include <list>
-#include <map>
-#include <string>
-#include <cstdlib>
-#include <algorithm>
-
-#define DEBUG(fmt, args...) LOGD("<road_exam> <%s>: " fmt, __func__, ##args)
-
-using namespace std;
-
-
-enum {
- CROSSING_NOT_HINT,
- CROSSING_HAS_HINT,
-};
-
-typedef struct {
- int road;
- int sep;
- int no;
- int total;
- int guide;
-} lane_t;
-
-typedef struct {
- int edgeIndex;
- int pointIndex;
- PointF point;
-} projection_t;
-
-typedef struct {
- int gain;
- struct RtkTime time;
-} change_lane_t;
-
-typedef struct {
- int road_index;
- int stop_line_index;
- int sep;
- double distance;
-} road_end_point_t;
-
-static const int INVALID_ROAD = -1;
-
-static const int CROSSING_TURN_THRESHOLD = 35;
-static const int TURN_THRESHOLD = 3;
-
-static bool occurOverSpeed;
-static bool occurSecondBreak;
-
-
-static int prevMoveDirect;
-
-static bool StopCarOnRedArea;
-static PointF stopPoint;
-static bool prevGearError = false;
-static int GearNSlideStatus = 0;
-
-static bool slideLongDistance;
-static bool slideNormalDistance;
-static bool occurSlide;
-
-static struct RtkTime crashGreenRunTime, crashGreenStartTime, stopTimepoint;
-static struct RtkTime gearChangeTimePoint;
-static struct RtkTime gearErrorTimePoint;
-static struct RtkTime gearNSlideTimePoint;
-
-static int gearChange;
-
-static car_sensor_value_t turnSignalStatus;
-
-static int gearErrorTime;
-
-static int currExamMapIndex;
-
-static lane_t Lane;
-static change_lane_t ChangeLane;
-
-static int CrashLineType;
-
-static map<int, car_sensor_value_t> CarSensorValue;
-static map<int, int> CrossingHint;
-static map<int, bool> ErrorLaneReport;
-
-typedef struct {
- int road_id;
- double distance;
-} trigger_detect_t;
-static map<int, trigger_detect_t> TriggerDetect; // 璺濈鏈矾娈靛悇瑙﹀彂鐐圭殑璺濈
-
-#define ROAD_EXAM_READY_NEXT 0
-#define ROAD_EXAM_FREE_RUN 1
-#define ROAD_EXAM_ITEM_CAR_START 2
-#define ROAD_EXAM_ITEM_STRAIGHT 3
-#define ROAD_EXAM_ITEM_OP_GEAR 4
-#define ROAD_EXAM_ITEM_CHANGE_LANE 5
-#define ROAD_EXAM_ITEM_OVER_TAKE 6
-#define ROAD_EXAM_ITEM_CAR_STOP 7
-
-#define ROAD_EXAM_ITEM_NOT_EXEC 0
-#define ROAD_EXAM_ITEM_EXECING 1
-#define ROAD_EXAM_ITEM_EXECED 2
-
-
-typedef struct {
- int name;
- int status;
-} RoadExamItem_t;
-static map<int, int> RoadExamItem;
-static int RoadExamStatus;
-static bool win;
-
-static struct RtkTime beginTurnTime, prevDetectTurnTime;
-static int startTurnYaw, prevYaw;
-static int turnCnt, turnTimeCnt;
-static int prevTurnWise;
-
-static void ItemExam(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end);
-static void ItemExam2(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, int forward, int moveDirect);
-static int isTurn(int currYaw, int prevYaw, int thres);
-static void ResetTurnDetect(const car_model *car);
-static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime);
-static bool StopOnRedArea(road_exam_map &RoadMap, const car_model *car);
-
-static int NearbyCrossingGuide(road_exam_map &RoadMap, int &stopLineIndex, int roadIndex, road_t &road, const car_model *car);
-
-static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, int forward, int moveDirect);
-
-static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car);
-
-static double AnalysisRoad(road_exam_map &RoadMap, int roadIndex, lane_t lane, const car_model *car);
-
-void InitRoadExam(road_exam_map &RoadMap)
-{
- DEBUG("Start road_exam");
-
- occurOverSpeed = false;
-
- occurSecondBreak = false;
-
- prevMoveDirect = 0;
-
- StopCarOnRedArea = false;
- occurSlide = false;
- slideLongDistance = false;
- slideNormalDistance = false;
- prevGearError = false;
- gearErrorTime = 0;
- GearNSlideStatus = 0;
-
- currExamMapIndex = -1;
-
- Lane.road = Lane.sep = Lane.no = -1;
- Lane.guide = 0;
-
-
- CrashLineType = -1;
- turnCnt = -1;
-
- ResetOdo();
- ResetTarget(RoadMap);
- // 鍒濆鍖栬�冮」
- RoadExamItem.clear();
- RoadExamItem[ROAD_EXAM_ITEM_CAR_START] = ROAD_EXAM_ITEM_NOT_EXEC;
- RoadExamItem[ROAD_EXAM_ITEM_STRAIGHT] = ROAD_EXAM_ITEM_NOT_EXEC;
- RoadExamItem[ROAD_EXAM_ITEM_OP_GEAR] = ROAD_EXAM_ITEM_NOT_EXEC;
- RoadExamItem[ROAD_EXAM_ITEM_CHANGE_LANE] = ROAD_EXAM_ITEM_NOT_EXEC;
- RoadExamItem[ROAD_EXAM_ITEM_OVER_TAKE] = ROAD_EXAM_ITEM_NOT_EXEC;
- RoadExamItem[ROAD_EXAM_ITEM_CAR_STOP] = ROAD_EXAM_ITEM_NOT_EXEC;
-
- RoadExamStatus = ROAD_EXAM_READY_NEXT;
-
- TriggerDetect.clear();
-
- win = false;
-}
-
-void TerminateRoadExam(void)
-{
- TerminateDummyLightExam();
- TerminatePrepare();
-}
-
-/*********************************************************************
- * 璁＄畻鏌愮偣鍒伴亾璺竟绾跨殑鎶曞奖鐐�
- * @param edge
- * @param road
- * @param point
- * @return
- */
-PointF CalcProjectionWithRoadEdge(vector<edge_t> &edge, PointF point)
-{
- PointF p1, p2;
- PointF proj;
-
- vector<PointF> projArray;
-
- Line line;
-
- // 璁＄畻鍨傜偣
- for (int i = 0; i < edge.size(); ++i) {
- p1 = edge[i].points[0];
- for (int j = 1; j < edge[i].points.size(); ++j) {
- p2 = edge[i].points[j];
- MakeLine(&line, &p1, &p2);
-
- PointF vp;
- if (VerticalPointOnLine(point, line, vp)) {
- projArray.push_back(vp);
- }
-
- p1 = p2;
- }
- }
-
- if (projArray.size() == 0) {
- // 娌℃湁浠讳綍鍨傜偣锛屾壘鍒版渶杩戠殑鐐�
- proj = edge[0].points[0];
- for (int i = 0; i < edge.size(); ++i) {
- for (int j = 1; j < edge[i].points.size(); ++j) {
- if (DistanceOf(point, edge[i].points[j]) < DistanceOf(point, proj)) {
- proj = edge[i].points[j];
- }
- }
- }
- } else {
- // 鏈�杩戠殑鍨傜偣
- proj = projArray[0];
- for (int i = 1; i < projArray.size(); ++i) {
- if (DistanceOf(point, projArray[i]) < DistanceOf(point, proj)) {
- proj = projArray[i];
- }
- }
- }
-
- return proj;
-}
-
-static projection_t CalcProjectionWithRoadEdgeEx(vector<edge_t> &edge, PointF point)
-{
- PointF p1, p2;
- Line line;
- projection_t theProj;
-
- vector<projection_t> projSet;
-
- // 璁＄畻鍨傜偣
- for (int i = 0; i < edge.size(); ++i) {
- p1 = edge[i].points[0];
- for (int j = 1; j < edge[i].points.size(); ++j) {
- p2 = edge[i].points[j];
- MakeLine(&line, &p1, &p2);
-
- PointF vp;
- if (VerticalPointOnLine(point, line, vp)) {
- projection_t proj;
- proj.edgeIndex = i;
- proj.pointIndex = j - 1;
- proj.point = vp;
- projSet.push_back(proj);
- }
-
- p1 = p2;
- }
- }
-
- if (projSet.size() == 0) {
- // 娌℃湁浠讳綍鍨傜偣锛屾壘鍒版渶杩戠殑鐐�
- theProj.edgeIndex = 0;
- theProj.pointIndex = 0;
- theProj.point = edge[0].points[0];
-
- for (int i = 0; i < edge.size(); ++i) {
- for (int j = 1; j < edge[i].points.size(); ++j) {
- if (DistanceOf(point, edge[i].points[j]) < DistanceOf(point, theProj.point)) {
- theProj.edgeIndex = i;
- theProj.pointIndex = j - 1;
- theProj.point = edge[i].points[j];
- }
- }
- }
- } else {
- // 鏈�杩戠殑鍨傜偣
- theProj = projSet[0];
- for (int i = 1; i < projSet.size(); ++i) {
- if (DistanceOf(point, projSet[i].point) < DistanceOf(point, theProj.point)) {
- theProj = projSet[i];
- }
- }
- }
-
- return theProj;
-}
-
-double CalcDistanceReference(PointF point, PointF refPoint, vector<edge_t> &edge)
-{
- double distance = 0;
-
- projection_t projection, refProjection;
-
- projection = CalcProjectionWithRoadEdgeEx(edge, point);
- refProjection = CalcProjectionWithRoadEdgeEx(edge, refPoint);
-
- if (projection.edgeIndex != refProjection.edgeIndex || projection.pointIndex != refProjection.pointIndex) {
- bool ahead = false;
- if (projection.edgeIndex != refProjection.edgeIndex) {
- if (projection.edgeIndex > refProjection.edgeIndex) {
- projection_t temp;
- temp = projection;
- projection = refProjection;
- refProjection = temp;
- ahead = true;
- }
- int a = projection.edgeIndex;
- int b = projection.pointIndex + 1;
-
- if (b < edge[a].points.size()) {
- distance += DistanceOf(projection.point, edge[projection.edgeIndex].points[b]);
- }
-
- for (; a < refProjection.edgeIndex; ++a) {
- for (; b < edge[a].points.size() - 1; ++b) {
- distance += DistanceOf(edge[a].points[b], edge[a].points[b+1]);
- }
- b = 0;
- }
-
- for (int i = 0; i < refProjection.pointIndex; i++) {
- distance += DistanceOf(edge[a].points[i], edge[a].points[i+1]);
- }
-
- distance += DistanceOf(refProjection.point, edge[refProjection.edgeIndex].points[refProjection.pointIndex]);
- } else {
- if (projection.pointIndex > refProjection.pointIndex) {
- projection_t temp;
- temp = projection;
- projection = refProjection;
- refProjection = temp;
- ahead = true;
- }
- int a = projection.edgeIndex;
- int b = projection.pointIndex + 1;
-
- if (b < edge[a].points.size()) {
- distance += DistanceOf(projection.point, edge[projection.edgeIndex].points[b]);
- }
-
- for (; b < refProjection.pointIndex; b++) {
- distance += DistanceOf(edge[a].points[b], edge[a].points[b+1]);
- }
-
- distance += DistanceOf(refProjection.point, edge[refProjection.edgeIndex].points[refProjection.pointIndex]);
- }
- if (ahead) {
- distance *= -1.0;
- }
- } else {
- distance = DistanceOf(projection.point, refProjection.point);
-
- if (DistanceOf(projection.point, edge[projection.edgeIndex].points[projection.pointIndex]) >
- DistanceOf(refProjection.point, edge[refProjection.edgeIndex].points[refProjection.pointIndex])) {
- distance *= -1.0;
- }
- }
-
- return distance;
-}
-
-/***************************************************************
- * 杞﹁締鎵�鍦ㄩ亾璺紝鏍规嵁杞﹁締鐨勪腑杞村墠鐐�
- * @param currRoadIndex, 浼樺厛妫�娴嬪綋鍓嶉亾璺�
- * @param RoadMap
- * @param car
- * @return
- */
-static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car)
-{
- int n = 0;
- int index = currRoadIndex;
-
- if (index < 0) {
- index = 0;
- }
-
- while (n < RoadMap.roads.size()) {
- bool changeSegment = false;
- vector<PointF> roadOutLine;
- Polygon area;
-
-// DEBUG("CalcRoadIndex %d 鍋氱偣 %d 鏈夌偣 %d", index, RoadMap.roads[index].leftEdge.size(), RoadMap.roads[index].rightEdge.size());
-
- for (int i = 0; i < RoadMap.roads[index].leftEdge.size(); ++i) {
- for (int j = 0; j < RoadMap.roads[index].leftEdge[i].points.size(); ++j) {
- if (changeSegment && roadOutLine.size() > 0 && IsSamePoint(roadOutLine.back(), RoadMap.roads[index].leftEdge[i].points[j])) {
-// DEBUG("閲嶅鐐�");
- continue;
- }
- changeSegment = false;
- roadOutLine.push_back(RoadMap.roads[index].leftEdge[i].points[j]);
-// DEBUG("鍋氱偣鍔犲叆 point (%f, %f)", RoadMap.roads[index].leftEdge[i].points[j].X, RoadMap.roads[index].leftEdge[i].points[j].Y);
- }
- changeSegment = true;
- }
-
- for (int i = 0; i < RoadMap.roads[index].rightEdge.size(); ++i) {
- for (int j = RoadMap.roads[index].rightEdge[i].points.size() - 1; j >= 0; --j) {
- if (changeSegment && roadOutLine.size() > 0 && IsSamePoint(roadOutLine.back(), RoadMap.roads[index].rightEdge[i].points[j])) {
-// DEBUG("閲嶅鐐�");
- continue;
- }
- changeSegment = false;
- roadOutLine.push_back(RoadMap.roads[index].rightEdge[i].points[j]);
-
-// DEBUG("鏈夌偣鍔犲叆 point (%f, %f)", RoadMap.roads[index].rightEdge[i].points[j].X, RoadMap.roads[index].rightEdge[i].points[j].Y);
- }
- changeSegment = true;
- }
-
- area.num = roadOutLine.size();
- area.point = (PointF *) malloc(area.num * sizeof(PointF));
-
- for (int i = 0; i < area.num; ++i) {
- area.point[i] = roadOutLine[i];
- }
-
-// {
-// for (int x = 0; x < area.num; ++x) {
-// DEBUG("road %d point %d (%f, %f)", index, x, area.point[x].X, area.point[x].Y);
-// }
-// }
-
- if (IntersectionOf(car->carXY[car->axial[AXIAL_FRONT]], &area) == GM_Containment) {
- free(area.point);
- break;
- }
-
- free(area.point);
-
- index = (index + 1) % RoadMap.roads.size();
- n++;
- }
-
- if (n >= RoadMap.roads.size()) {
- index = -1;
- }
-
- return index;
-}
-
-/************************************************
- * 杞﹁疆鍘嬪疄绾匡紝鍓嶅悗杞翠氦鍙夛紝鍓嶅悗杞抗浜ゅ弶
- * @param currCrashLineType
- * @param RoadMap
- * @param car
- * @param CarModelList
- * @return
- */
-static int CrashRedLine(int currCrashLineType, int roadIndex, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
-{
- Line frontTireAxle, rearTireAxle;
- Line frontLeftTireTrack, frontRightTireTrack;
- Line rearLeftTireTrack, rearRightTireTrack;
- bool track = false;
- int lineType = -1;
- PointF p11, p12, p21, p22, p31, p32, p41, p42;
-
- MakeLine(&frontTireAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
- MakeLine(&rearTireAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
-
- if (CarModelList.size() >= 2) {
- list<car_model *>::iterator iter = CarModelList.begin();
- p11 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]];
- p21 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_front_tire[TIRE_OUTSIDE]];
- p31 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_rear_tire[TIRE_OUTSIDE]];
- p41 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_rear_tire[TIRE_OUTSIDE]];
-
- struct RtkTime time1 = ((car_model *)(*iter))->tm;
-
- ++iter;
-
- p12 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]];
- p22 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_front_tire[TIRE_OUTSIDE]];
- p32 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_rear_tire[TIRE_OUTSIDE]];
- p42 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_rear_tire[TIRE_OUTSIDE]];
-
- struct RtkTime time2 = ((car_model *)(*iter))->tm;
-
- if (TimeGetDiff(&time1, &time2) <= D_SEC(1)) {
- MakeLine(&frontLeftTireTrack, &p11, &p12);
- MakeLine(&frontRightTireTrack, &p21, &p22);
- MakeLine(&rearLeftTireTrack, &p31, &p32);
- MakeLine(&rearRightTireTrack, &p41, &p42);
- track = true;
- }
- }
-
- PointF p1, p2;
- Line redLine;
-
- // 宸﹀彸璺竟瀹炵嚎
- for (int n = 0; n < RoadMap.roads.size(); ++n) {
- for (int m = 0; m < RoadMap.roads[n].leftEdge.size(); ++m) {
- if (RoadMap.roads[n].leftEdge[m].character == LINE_SOLID && RoadMap.roads[n].leftEdge[m].points.size() >= 2) {
- p1 = RoadMap.roads[n].leftEdge[m].points[0];
- for (int l = 1; l < RoadMap.roads[n].leftEdge[m].points.size(); ++l) {
- p2 = RoadMap.roads[n].leftEdge[m].points[l];
-
- MakeLine(&redLine, &p1, &p2);
-
- if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
- IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
- lineType = LINE_SOLID;
- goto CRASH_LINE_CONFIRM;
- }
-
- if (track &&
- (IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
- IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
- IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
- IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
- lineType = LINE_SOLID;
- goto CRASH_LINE_CONFIRM;
- }
- p1 = p2;
- }
- }
- }
-
- for (int m = 0; m < RoadMap.roads[n].rightEdge.size(); ++m) {
- if (RoadMap.roads[n].rightEdge[m].character == LINE_SOLID && RoadMap.roads[n].rightEdge[m].points.size() >= 2) {
- p1 = RoadMap.roads[n].rightEdge[m].points[0];
- for (int l = 1; l < RoadMap.roads[n].rightEdge[m].points.size(); ++l) {
- p2 = RoadMap.roads[n].rightEdge[m].points[l];
-
- MakeLine(&redLine, &p1, &p2);
-
- if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
- IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
- lineType = LINE_SOLID;
- goto CRASH_LINE_CONFIRM;
- }
-
- if (track &&
- (IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
- IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
- IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
- IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
- lineType = LINE_SOLID;
- goto CRASH_LINE_CONFIRM;
- }
- p1 = p2;
- }
- }
- }
-
- // 鍒嗛亾瀹炵嚎
-// DEBUG("%d 鍒嗛亾娈� %d", n, RoadMap.roads[n].separate.size());
-
- for (int m = 0; m < RoadMap.roads[n].separate.size(); ++m) {
- // 涓�缁勮溅閬�
-// DEBUG("%d %d 杞﹂亾鏁� %d", n, m, RoadMap.roads[n].separate[m].lines.size());
-
- for (int l = 0; l < RoadMap.roads[n].separate[m].lines.size(); ++l) {
- // 澶氭牴鍒嗛亾绾�
-
-// DEBUG("%d %d %d 绾挎鏍� %d", n, m, l, RoadMap.roads[n].separate[m].lines[l].size());
- for (int a = 0; a < RoadMap.roads[n].separate[m].lines[l].size(); ++a) {
- // 涓�鏍瑰垎閬撶嚎涓嚎鍨嬬浉鍚岀殑
- int character = RoadMap.roads[n].separate[m].lines[l][a].character;
-
- if (RoadMap.roads[n].separate[m].lines[l][a].points.size() < 2)
- continue;
-
- p1 = RoadMap.roads[n].separate[m].lines[l][a].points[0];
-
- for (int b = 1; b < RoadMap.roads[n].separate[m].lines[l][a].points.size(); ++b) {
- p2 = RoadMap.roads[n].separate[m].lines[l][a].points[b];
- MakeLine(&redLine, &p1, &p2);
- if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
- IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
- lineType = character;
- goto CRASH_LINE_CONFIRM;
- }
- if (track &&
- (IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
- IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
- IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
- IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
- lineType = character;
- goto CRASH_LINE_CONFIRM;
- }
- p1 = p2;
- }
-
- }
- }
- }
- }
-
- for (int n = 0; n < RoadMap.forbidLines.size(); ++n) {
- // 鍦板浘鍥存爮
- if (RoadMap.forbidLines[n].type == LINE_BOUNDARY && track && RoadMap.forbidLines[n].points.size() >= 2) {
- p1 = RoadMap.forbidLines[n].points[0];
- for (int m = 1; m < RoadMap.forbidLines[n].points.size(); ++m) {
- p2 = RoadMap.forbidLines[n].points[m];
- MakeLine(&redLine, &p1, &p2);
- if (IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
- IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection) {
- lineType = LINE_BOUNDARY;
- goto CRASH_LINE_CONFIRM;
- }
- p1 = p2;
- }
- }
- // 鍏跺畠绾㈢嚎
- if (RoadMap.forbidLines[n].type == LINE_SOLID && RoadMap.forbidLines[n].points.size() >= 2) {
- p1 = RoadMap.forbidLines[n].points[0];
- for (int m = 1; m < RoadMap.forbidLines[n].points.size(); ++m) {
- p2 = RoadMap.forbidLines[n].points[m];
- MakeLine(&redLine, &p1, &p2);
-
- if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
- IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
- lineType = LINE_SOLID;
- goto CRASH_LINE_CONFIRM;
- }
-
- if (track &&
- (IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
- IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
- IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
- IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
- lineType = LINE_SOLID;
- goto CRASH_LINE_CONFIRM;
- }
- p1 = p2;
- }
- }
- }
-
-CRASH_LINE_CONFIRM:
- if (lineType == LINE_HALF_SOLID_LEFT && currCrashLineType != lineType && track) {
- if (IntersectionOfLine(p21, redLine) == -1 && IntersectionOfLine(p22, redLine) == 1) {
- // 闈炴硶璺ㄧ嚎
- lineType += 100;
- }
- } else if (lineType == LINE_HALF_SOLID_RIGHT && currCrashLineType != lineType && track) {
- if (IntersectionOfLine(p11, redLine) == 1 && IntersectionOfLine(p12, redLine) == -1) {
- // 闈炴硶璺ㄧ嚎
- lineType += 100;
- }
- }
- return lineType;
-}
-
-static int GetGuideDirect(road_exam_map &RoadMap, PointF point, int roadIndex, int sepIndex, int laneNo)
-{
- for (int i = 0; i < RoadMap.roads[roadIndex].separate[sepIndex].lane_direct.size(); ++i) {
- double d1 = CalcDistanceReference(point, RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].start, RoadMap.roads[roadIndex].rightEdge);
- double d2 = CalcDistanceReference(point, RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].end, RoadMap.roads[roadIndex].rightEdge);
- if (d1 < -1e-3 && d2 > 1e-3 && laneNo < RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].direct.size()) {
- return RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].direct[laneNo];
- }
- }
- return 0;
-}
-
-/********************************************
- * 璁＄畻鏌愮偣鍦ㄥ摢涓溅閬�
- * @param point
- * @param RoadMap
- * @param roadIndex
- * @return
- */
-static bool GetLane(lane_t &lane, PointF point, road_exam_map &RoadMap, int roadIndex)
-{
- Line leftProjLine, rightProjLine;
- Line sep;
- PointF p0, p1, p2;
-
- if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
- return false;
-
- p1 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].leftEdge, point);
- p0 = p2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, point);
-
- MakeLine(&leftProjLine, &point, &p1);
- MakeLine(&rightProjLine, &point, &p2);
-
- for (int i = 0; i < RoadMap.roads[roadIndex].separate.size(); ++i) {
- struct car_lane_pos {
- int lane_no;
- char pos; // 鐐瑰湪杞﹂亾鐨勫乏鍙充綅缃�
-
- bool operator == (const int &x) {
- return (this->lane_no == x);
- }
- };
-
- vector<struct car_lane_pos> CLP;
- bool oneMoreIntersection = false;
-
- for (int j = 0; j < RoadMap.roads[roadIndex].separate[i].lines.size(); ++j) { // 姣忎竴鏍圭嚎鍒嗙粍
- bool intersection = false;
- for (int k = 0; !intersection && k < RoadMap.roads[roadIndex].separate[i].lines[j].size(); ++k) { // 鍚屼竴绾垮瀷鐨勭嚎鍒嗙粍
-
- p1 = RoadMap.roads[roadIndex].separate[i].lines[j][k].points[0];
- for (int m = 1; m < RoadMap.roads[roadIndex].separate[i].lines[j][k].points.size(); ++m) {
- p2 = RoadMap.roads[roadIndex].separate[i].lines[j][k].points[m];
- MakeLine(&sep, &p1, &p2);
-
- if (IntersectionOf(sep, leftProjLine) == GM_Intersection) {
- intersection = true;
- struct car_lane_pos temp;
-
- temp.lane_no = j;
- temp.pos = 'R';
- CLP.push_back(temp);
- break;
- }
- if (IntersectionOf(sep, rightProjLine) == GM_Intersection) {
- intersection = true;
- struct car_lane_pos temp;
-
- temp.lane_no = j;
- temp.pos = 'L';
- CLP.push_back(temp);
- break;
- }
- p1 = p2;
- }
- }
- if (!intersection) {
- struct car_lane_pos temp;
- temp.lane_no = j;
- temp.pos = 0;
- CLP.push_back(temp);
- } else {
- oneMoreIntersection = true;
- }
- }
-
- if (oneMoreIntersection) {
- // 鑰冭檻鍒伴噰闆嗗亸宸彲鑳芥瘡涓�鏍圭嚎鏃犳硶瀵归綈锛屽彧瑕佸叾涓竴鏍圭浉浜わ紝鍏跺畠涓嶇浉浜ょ殑涔熷弬涓�
- for (vector<struct car_lane_pos>::iterator it = CLP.begin(); it != CLP.end(); ++it) {
- if ((*it).pos == 0) {
- int m = RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no].size();
- int n = RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][m-1].points.size();
-
- if (DistanceOf(RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][0].points[0], point) >
- DistanceOf(RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][m-1].points[n-1], point)) {
- // 杞﹁締鎺ヨ繎杞﹂亾灏鹃儴
- MakeLine(&sep, &RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][m-1].points[n-2], &RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][m-1].points[n-1]);
- } else {
- // 杞﹁締鎺ヨ繎杞﹂亾澶撮儴
- MakeLine(&sep, &RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][0].points[0], &RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][0].points[1]);
- }
-
- struct car_lane_pos temp;
-
- temp.lane_no = (*it).lane_no;
-
- if (IntersectionOfLine(point, sep) == -1) {
- temp.pos = 'R';
- } else {
- temp.pos = 'L';
- }
- (*it) = temp;
- }
- }
-
- lane.road = roadIndex;
- lane.sep = i;
- lane.no = CLP.size();
- lane.total = CLP.size() + 1;
-
- for (int x = 0; x < CLP.size(); ++x) {
- if (CLP[x].pos == 'L') {
- lane.no = x;
- break;
- }
- }
- lane.guide = GetGuideDirect(RoadMap, p0, lane.road, lane.sep, lane.no);
- return true;
- }
- }
-
- lane.road = roadIndex;
- lane.sep = 0;
- lane.no = 0;
- lane.total = 0;
- lane.guide = 0;
-
- return false;
-}
-
-static void WriteCarSensorValue(int name, int value, const struct RtkTime *rtkTime)
-{
- car_sensor_value_t nv;
-
- nv.name = name;
- nv.value = value;
- nv.time = *rtkTime;
-
- auto it = CarSensorValue.find(name);
-
- if (it != CarSensorValue.end()) {
- if (it->second.value != value || name == ENGINE_RPM) {
- CarSensorValue[name] = nv;
- }
- } else {
- CarSensorValue.insert(pair<int, car_sensor_value_t>(name, nv));
- }
-}
-
-car_sensor_value_t ReadCarSensorValue(int name)
-{
- car_sensor_value_t nv = {.name = -1};
-
- auto it = CarSensorValue.find(name);
-
- if (it != CarSensorValue.end()) {
- nv = it->second;
- }
-
- return nv;
-}
-
-static void UpdateCarSensor(const struct RtkTime *rtkTime)
-{
- WriteCarSensorValue(TURN_SIGNAL_LAMP, ReadCarStatus(TURN_SIGNAL_LAMP), rtkTime);
- WriteCarSensorValue(HAND_BREAK, ReadCarStatus(HAND_BREAK), rtkTime);
- WriteCarSensorValue(DOOR, ReadCarStatus(DOOR), rtkTime);
- WriteCarSensorValue(ENGINE_RPM, ReadCarStatus(ENGINE_RPM), rtkTime);
- WriteCarSensorValue(SECOND_BREAK, ReadCarStatus(SECOND_BREAK), rtkTime);
- WriteCarSensorValue(GEAR, ReadCarStatus(GEAR), rtkTime);
- WriteCarSensorValue(BREAK, ReadCarStatus(BREAK), rtkTime);
-}
-
-static void DetectLine(int roadIndex, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, int moveDirect, const struct RtkTime *rtkTime)
-{
- static int checkCrashGreenTimeout = 0;
- static bool crashDottedLine = false;
- int newLineType = CrashRedLine(CrashLineType, roadIndex, RoadMap, car, CarModelList);
-
- if (newLineType >= 100) {
- newLineType -= 100;
- DEBUG("闈炴硶璺ㄨ秺鍒嗛亾绾�");
- AddExamFault(30119, rtkTime);
- }
- if (newLineType == LINE_SOLID && CrashLineType != LINE_SOLID) {
- // 杞﹁締琛岄┒涓獞杞ц溅閬撲腑蹇冨疄绾挎垨鑰呰溅閬撹竟缂樺疄绾匡紝涓嶅悎鏍�
- DEBUG("鎾為亾璺竟缂樼嚎");
- AddExamFault(30119, rtkTime);
- }
- if (newLineType == LINE_BOUNDARY && CrashLineType != LINE_BOUNDARY) {
- // 杞﹁締瓒婄晫锛岄�冭窇浜嗭紝涓嶅悎鏍�
- DEBUG("杞﹁締瓒婄晫");
- AddExamFault(30103, rtkTime);
- }
-
- if (newLineType == LINE_DOTTED || newLineType == LINE_HALF_SOLID_LEFT || newLineType == LINE_HALF_SOLID_RIGHT) {
- if (!crashDottedLine) {
- checkCrashGreenTimeout = 0;
- // 璁板綍寮�濮嬪帇绾跨殑鏃堕棿锛屼笉纭畾鏄惁鏈夊彉閬撴剰鍥撅紝寰呯‘璁ゅ彉閬撳悗鍐嶅鐞嗕箣
- crashGreenStartTime = *rtkTime;
- turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
- DEBUG("寮�濮嬪帇铏氱嚎 %02d-%02d-%02d %02d:%02d:%02d", crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD, crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss, crashGreenStartTime.mss * 10);
- }
- crashDottedLine = true;
- } else {
- crashDottedLine = false;
- }
-
- if (crashDottedLine) {
- // 鍘嬭櫄绾�
- if (moveDirect != 0) {
- if (checkCrashGreenTimeout == 0) {
- checkCrashGreenTimeout = 1;
- crashGreenRunTime = *rtkTime; // 杩愬姩涓帇铏氱嚎鐨勫紑濮嬫椂闂寸偣
- } else if (checkCrashGreenTimeout == 1) {
- if (TimeGetDiff(rtkTime, &crashGreenRunTime) >= examParam.crash_dotted_line_cumulative_time) {
- DEBUG("闀挎椂闂村帇铏氱嚎");
- checkCrashGreenTimeout = 2;
- // 闀挎椂闂撮獞杞ц溅閬撳垎鐣岀嚎琛岄┒锛屼笉鍚堟牸
- AddExamFault(30120, rtkTime);
- }
- }
- } else {
- // 鍋滆溅灏变笉璁℃椂浜�
- checkCrashGreenTimeout = 0;
- }
- } else {
- // 涓嶅啀鍘嬭櫄绾�
- checkCrashGreenTimeout = 0;
- }
-
- CrashLineType = newLineType;
-}
-
-int DetectLane(road_exam_map &RoadMap, const car_model *car, int roadIndex, const struct RtkTime *rtkTime)
-{
- lane_t newLane;
- int gain = 0;
-
- if (CrashLineType == LINE_DOTTED || CrashLineType == LINE_HALF_SOLID_LEFT || CrashLineType == LINE_HALF_SOLID_RIGHT) {
- // 鎺掗櫎璺ㄧ嚎涓�
- return 0;
- }
- if (GetLane(newLane, car->carXY[car->axial[AXIAL_FRONT]], RoadMap, roadIndex)) {
- if (newLane.road == Lane.road && newLane.sep == Lane.sep) {
- gain = newLane.no - Lane.no;
- } else {
- DEBUG("杞﹂亾鍒嗘鍒囨崲 Old Lane road = %d sep = %d no = %d New Lane road = %d sep = %d no = %d", Lane.road, Lane.sep, Lane.no, newLane.road, newLane.sep, newLane.no);
- ChangeLane.gain = 0;
- }
-
- // 妫�鏌ヨ浆鍚戠伅
- if (gain != 0) {
- DEBUG("鍙橀亾 new lane %d, gain %d", newLane.no, gain);
- if (gain < 0) {
- if (turnSignalStatus.value != LEFT_TURN_LIGHT) {
- DEBUG("鍙樿皟鏈墦鐏�!!");
- // 娌℃墦鐏紝涓嶅悎鏍�
- AddExamFault(30121, rtkTime);
- } else if (TimeGetDiff(&crashGreenStartTime, &turnSignalStatus.time) <
- examParam.turn_signal_min_advance) {
- DEBUG("杞悜鐏椂闂翠笉瓒� %02d-%02d-%02d %02d:%02d:%02d.%03d -> %02d-%02d-%02d %02d:%02d:%02d.%03d",
- crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD,
- crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss,
- crashGreenStartTime.mss * 10,
- turnSignalStatus.time.YY, turnSignalStatus.time.MM, turnSignalStatus.time.DD, turnSignalStatus.time.hh,
- turnSignalStatus.time.mm, turnSignalStatus.time.ss, turnSignalStatus.time.mss * 10);
-
- // 涓嶈冻3绉掞紝涓嶅悎鏍�
- AddExamFault(30122, rtkTime);
- }
- } else {
- if (turnSignalStatus.value != RIGHT_TURN_LIGHT) {
- DEBUG("鍙樿皟鏈墦鐏�!!");
- // 娌℃墦鐏紝涓嶅悎鏍�
- AddExamFault(30121, rtkTime);
- } else if (TimeGetDiff(&crashGreenStartTime, &turnSignalStatus.time) <
- examParam.turn_signal_min_advance) {
- DEBUG("杞悜鐏椂闂翠笉瓒� %02d-%02d-%02d %02d:%02d:%02d.%03d -> %02d-%02d-%02d %02d:%02d:%02d.%03d",
- crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD,
- crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss,
- crashGreenStartTime.mss * 10,
- turnSignalStatus.time.YY, turnSignalStatus.time.MM, turnSignalStatus.time.DD, turnSignalStatus.time.hh,
- turnSignalStatus.time.mm, turnSignalStatus.time.ss, turnSignalStatus.time.mss * 10);
- // 涓嶈冻3绉掞紝涓嶅悎鏍�
- AddExamFault(30122, rtkTime);
- }
- }
-
- if (((ChangeLane.gain < 0 && gain < 0) || (ChangeLane.gain > 0 && gain > 0)) && TimeGetDiff(rtkTime, &ChangeLane.time) < examParam.continuous_change_lane_min_time) {
- DEBUG("杩炵画鍙橀亾");
- AddExamFault(30125, rtkTime);
- }
- ChangeLane.gain = gain;
- ChangeLane.time = *rtkTime;
- }
-
- Lane = newLane;
- }
- return gain;
-}
-
-static void ChangeCrossingStatus(int roadIndex, int index, int status)
-{
- int key = roadIndex * 100 + index;
-
- auto it = CrossingHint.find(key);
-
- if (it != CrossingHint.end()) {
- CrossingHint.erase(it);
- }
- CrossingHint.insert(pair<int, int>(key, status));
-}
-
-static int GetCrossingStatus(int roadIndex, int index)
-{
- int key = roadIndex * 100 + index;
-
- auto it = CrossingHint.find(key);
-
- if (it != CrossingHint.end()) {
- return it->second;
- }
- return CROSSING_NOT_HINT;
-}
-
-/*****************************************************
- * 浠ユ甯搁�氳繃璺彛鎴栬嚜鐢遍┚椹剁寮�璺鍚�
- * @param roadIndex
- */
-static void ResetCrossingStatus(road_exam_map &RoadMap, int roadIndex)
-{
- while (true) {
- bool w = false;
- for (auto it = CrossingHint.begin(); it != CrossingHint.end(); ++it) {
- if (it->first / 100 == roadIndex && it->second != CROSSING_NOT_HINT) {
- ChangeCrossingStatus(it->first / 100, it->first % 100, CROSSING_NOT_HINT);
- CrossingChange(RoadMap.roads[roadIndex].id, it->first % 100, 0);
- w = true;
- break;
- }
- }
- if (!w)
- break;
- }
-}
-
-static void SetErrorLaneRpt(int roadIndex, int index, bool status)
-{
- int key = roadIndex * 100 + index;
-
- auto it = ErrorLaneReport.find(key);
-
- if (it != ErrorLaneReport.end()) {
- ErrorLaneReport.erase(it);
- }
- ErrorLaneReport.insert(pair<int, bool>(key, status));
-}
-
-static bool GetErrorLaneRpt(int roadIndex, int index)
-{
- int key = roadIndex * 100 + index;
-
- auto it = ErrorLaneReport.find(key);
-
- if (it != ErrorLaneReport.end()) {
- return it->second;
- }
- return false;
-}
-
-static void ResetErrorLaneRpt(int roadIndex)
-{
- while (true) {
- bool w = false;
- for (auto it = ErrorLaneReport.begin(); it != ErrorLaneReport.end(); ++it) {
- if (it->first / 100 == roadIndex && it->second == true) {
- SetErrorLaneRpt(it->first / 100, it->first % 100, false);
- w = true;
- break;
- }
- }
- if (!w)
- break;
- }
-}
-
-/*********************************************************
- * 璁＄畻褰撳墠鎵�鍦ㄥ垎閬撶嚎鐨勫墿浣欓暱搴�
- * @param RoadMap
- * @param lane
- * @param car
- * @return
- */
-static double SeparateLength(road_exam_map &RoadMap, lane_t lane, const car_model *car)
-{
- double distance = 0;
-
- if (lane.road < 0 || lane.total == 0 || lane.sep < 0)
- return distance;
-
- int n = RoadMap.roads[lane.road].separate[lane.sep].lines[0].size();
- int m = RoadMap.roads[lane.road].separate[lane.sep].lines[0][n-1].points.size();
-
- return CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]],
- RoadMap.roads[lane.road].separate[lane.sep].lines[0][n-1].points[m-1],
- RoadMap.roads[lane.road].rightEdge);
-}
-
-/***********************************************************
- * 鍓嶅悜鏈�杩戠殑璺彛鍋滄绾挎垨瀵煎悜绾胯捣鐐癸紝涓�瀹氳窛绂诲唴鍋氬嚭璺彛鎻愮ず锛屾垨椤圭洰鑷姩瑙﹀彂
- * @param roadIndex
- * @param RoadMap
- * @param car
- */
-static road_end_point_t NearbyRoadEndPoint(int roadIndex, road_exam_map &RoadMap, const car_model *car)
-{
- road_end_point_t rec;
-
- rec.road_index = roadIndex;
- rec.stop_line_index = 0;
- rec.distance = 0;
-
- if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
- return rec;
-
- // 鏈�杩戠殑璺彛璺濈
- int stop_line_index;
- PointF nearbyStopPoint;
-
- NearbyCrossingGuide(RoadMap, stop_line_index, roadIndex, RoadMap.roads[roadIndex], car);
- nearbyStopPoint.X = RoadMap.roads[roadIndex].stopLine[stop_line_index].line.X1;
- nearbyStopPoint.Y = RoadMap.roads[roadIndex].stopLine[stop_line_index].line.Y1;
-
- double dis1 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], nearbyStopPoint, RoadMap.roads[roadIndex].rightEdge);
-
- struct ep_ {
- int sep;
- double distance;
- } ep;
-
- vector<struct ep_> dset;
-
- ep.sep = -1;
- ep.distance = dis1;
- dset.push_back(ep);
-
- // 鏈�杩戠殑鍒嗛亾绾胯窛绂伙紝涓斾笉瓒呰繃鏈�杩戣矾鍙�
- for (int i = 0; i < RoadMap.roads[roadIndex].separate.size(); ++i) {
- for (int j = 0; j < RoadMap.roads[roadIndex].separate[i].lines.size(); ++j) {
- for (int k = 0; k < RoadMap.roads[roadIndex].separate[i].lines[j].size(); ++k) {
- if (RoadMap.roads[roadIndex].separate[i].lines[j][k].character != LINE_DOTTED &&
- RoadMap.roads[roadIndex].separate[i].lines[j][k].points.size() > 0) {
- double dis2 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.roads[roadIndex].separate[i].lines[j][k].points[0], RoadMap.roads[roadIndex].rightEdge);
-
- if (dis2 < -1e-3) {
- continue;
- }else if (dis2 < dis1) {
- // 鎵惧嚭鏈�鐭殑鐐�
- ep.sep = i;
- ep.distance = dis2;
- dset.push_back(ep);
- continue;
- }
- goto FIND_END_POINT;
- }
- }
- }
- }
-
- FIND_END_POINT:
- sort(dset.begin(), dset.end(), [=](struct ep_ x, struct ep_ y)->bool {return x.distance < y.distance;});
-
- rec.stop_line_index = stop_line_index;
- rec.distance = dset[0].distance;
- rec.sep = dset[0].sep;
-
- return rec;
-}
-
-int ExamSchemeCrossing(road_exam_map &RoadMap, int roadIndex, int crossIndex)
-{
- if (roadIndex < 0 || roadIndex >= RoadMap.roads.size() || RoadMap.examScheme.size() == 0) {
- return -1;
- }
- for (int i = 0; i < RoadMap.examScheme[0].crossingActive.size(); ++i) {
- if (RoadMap.examScheme[0].crossingActive[i].road_id == RoadMap.roads[roadIndex].id &&
- RoadMap.examScheme[0].crossingActive[i].index == crossIndex) {
- return RoadMap.examScheme[0].crossingActive[i].active;
- }
- }
- return -1;
-}
-
-static void HintCrossing(road_exam_map &RoadMap, int roadIndex, int stopIndex, double distance) {
- if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
- return;
-
- if (distance > 5 && distance < 60) {
- // 鎻愮ず璺彛鎬庝箞璧�
- if (GetCrossingStatus(roadIndex, stopIndex) == CROSSING_NOT_HINT) {
- // 椤圭洰鏍囧畾妯″紡涓嬶紝杩欓噷鎻愮ず鏍囧畾鏈矾鍙ｅ姩浣�
- if (RoadMap.calibrate) {
- CrossingChange(RoadMap.roads[roadIndex].id, stopIndex, 1);
- } else {
- int act = ExamSchemeCrossing(RoadMap, roadIndex, stopIndex);
-
- switch (act) {
- case ROAD_ACTIVE_FORWARD:
- DEBUG("璺彛鎻愮ず 鐩磋");
- PlayTTS(examParam.crossing_go_straight_tts, NULL);
- break;
- case ROAD_ACTIVE_TURN_LEFT:
- DEBUG("璺彛鎻愮ず 宸﹁浆");
- PlayTTS(examParam.crossing_turn_left_tts, NULL);
- break;
- case ROAD_ACTIVE_TURN_RIGHT:
- DEBUG("璺彛鎻愮ず 鍙宠浆");
- PlayTTS(examParam.crossing_turn_right_tts, NULL);
- break;
- case ROAD_ACTIVE_TURN_BACKWARD:
- DEBUG("璺彛鎻愮ず 鎺夊ご");
- PlayTTS(examParam.crossing_turn_back_tts, NULL);
- break;
- default:
- DEBUG("璺彛鎻愮ず 鏈厤缃�");
- //PlayTTS(examParam.crossing_turn_unknown_tts, NULL);
- break;
- }
- }
- ChangeCrossingStatus(roadIndex, stopIndex, CROSSING_HAS_HINT);
- }
- } else if (distance > 65 && GetCrossingStatus(roadIndex, stopIndex) != CROSSING_NOT_HINT) {
- ChangeCrossingStatus(roadIndex, stopIndex, CROSSING_NOT_HINT);
-
- if (RoadMap.calibrate) {
- CrossingChange(RoadMap.roads[roadIndex].id, stopIndex, 0);
- }
- }
-}
-
-static int NearbyCrossingGuide(road_exam_map &RoadMap, int &stopLineIndex, int roadIndex, road_t &road, const car_model *car)
-{
- int guide = 0, stopLine = 0;
- double distance;
-
- struct nearby_crossing {
- int stopLine;
- int guide;
- double distance;
- };
-
- vector<struct nearby_crossing> set;
-
- for (int i = 0; i < road.stopLine.size(); ++i) {
- PointF point;
-
- point.X = road.stopLine[i].line.X1;
- point.Y = road.stopLine[i].line.Y1;
-
- distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point,
- road.rightEdge);
-
- if (distance > 1e-3) {
- struct nearby_crossing temp;
-
- temp.stopLine = i;
- temp.guide = ExamSchemeCrossing(RoadMap, roadIndex, i);
- temp.distance = distance;
-
- set.push_back(temp);
- }
- }
-
- if (set.size() > 0) {
- distance = set[0].distance;
- guide = set[0].guide;
-
- for (int i = 1; i < set.size(); ++i) {
- if (distance > set[i].distance) {
- distance = set[i].distance;
- guide = set[i].guide;
- stopLine = set[i].stopLine;
- }
- }
- }
-
- stopLineIndex = stopLine;
- return guide;
-}
-
-static void RingBreak(void)
-{
- static int oldValue = BREAK_INACTIVE;
- car_sensor_value_t brk = ReadCarSensorValue(BREAK);
-
- if (brk.name == BREAK) {
- if (brk.value == BREAK_ACTIVE && oldValue != BREAK_ACTIVE) {
- PlayRing();
- }
- oldValue = brk.value;
- }
-}
-
-void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
-{
- double BigStraightRoadFree = 0, RoadCrossingFree = 0, TargetFree = 0;
- int forward = 0; // 杞﹁締鐩稿褰撳墠閬撹矾鐨�
- UpdateCarSensor(rtkTime);
-
- UpdataOdo(speed, moveDirect, rtkTime);
-
- if (!win) {
- win = true;
- turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
- }
-
- if (RoadMap.calibrate == 0) {
- // 鍒硅溅鎻愮ず闊�
- RingBreak();
-
- // 瓒呴�熸娴�
- if (ConvertMs2KMh(speed) > examParam.road_max_speed) {
- if (!occurOverSpeed) {
- occurOverSpeed = true;
- // 瓒呴�燂紝涓嶅悎鏍�
- DEBUG("瓒呴�� %f", ConvertMs2KMh(speed));
- AddExamFault(30118, rtkTime);
- }
- } else if (ConvertMs2KMh(speed) < examParam.road_max_speed - 5) {
- occurOverSpeed = false;
- }
-
- // 鍓埞杞︽娴�
- if (ReadCarStatus(SECOND_BREAK) == BREAK_ACTIVE) {
- // 鍓埞杞﹁俯涓嬶紝涓嶅悎鏍�
- if (!occurSecondBreak) {
- DEBUG("鍓埞杞﹀姩浣滀簡");
- occurSecondBreak = true;
- AddExamFault(30132, rtkTime);
- }
- } else {
- occurSecondBreak = false;
- }
-
- // 涓ゆ鎹㈡尅涓嶈繘
- static int prevGear = -1;
-
- int currGear = ReadCarStatus(GEAR);
- if (currGear != GEAR_N && prevGear == GEAR_N) {
- // 涓�娆℃崲鎸�
- if (gearChange == currGear && TimeGetDiff(rtkTime, &gearChangeTimePoint) < D_SEC(5)) {
- AddExamFault(30113, rtkTime);
- }
-
- gearChange = currGear;
- gearChangeTimePoint = *rtkTime;
- }
- prevGear = currGear;
-
- // 鎸′綅鍖归厤妫�娴�
- bool currGearError = false;
- bool currGearNSlide = false;
-
- switch (ReadCarStatus(GEAR)) {
- case GEAR_N:
- if (moveDirect != 0) {
- // 绌烘。婊戣
- currGearNSlide = true;
- }
- break;
- case GEAR_1:
- if (ConvertMs2KMh(speed) < examParam.gear_speed_table[0][0] ||
- ConvertMs2KMh(speed) > examParam.gear_speed_table[0][1]) {
- currGearError = true;
- }
- break;
- case GEAR_2:
- if (ConvertMs2KMh(speed) < examParam.gear_speed_table[1][0] ||
- ConvertMs2KMh(speed) > examParam.gear_speed_table[1][1]) {
- currGearError = true;
- }
- break;
- case GEAR_3:
- if (ConvertMs2KMh(speed) < examParam.gear_speed_table[2][0] ||
- ConvertMs2KMh(speed) > examParam.gear_speed_table[2][1]) {
- currGearError = true;
- }
- break;
- case GEAR_4:
- if (ConvertMs2KMh(speed) < examParam.gear_speed_table[3][0] ||
- ConvertMs2KMh(speed) > examParam.gear_speed_table[3][1]) {
- currGearError = true;
- }
- break;
- case GEAR_5:
- if (ConvertMs2KMh(speed) < examParam.gear_speed_table[4][0] ||
- ConvertMs2KMh(speed) > examParam.gear_speed_table[4][1]) {
- currGearError = true;
- }
- break;
- default:
- break;
- }
- // 绌烘。婊戣瓒呮椂
- if (currGearNSlide) {
- if (GearNSlideStatus == 0) {
- DEBUG("妫�娴嬪埌绌烘尅婊戣");
- GearNSlideStatus = 1;
- gearNSlideTimePoint = *rtkTime;
- }
- if (GearNSlideStatus == 1 &&
- TimeGetDiff(rtkTime, &gearNSlideTimePoint) > examParam.gear_n_allow_time) {
- // 绌烘。婊戣瓒�5绉掞紝涓嶅悎鏍�
- DEBUG("鎸′綅婊戣锛岃秴杩�5绉�");
- AddExamFault(30114, rtkTime);
- GearNSlideStatus = 2;
- }
- } else if (GearNSlideStatus != 0) {
- GearNSlideStatus = 0;
- DEBUG("绌烘尅婊戣缁撴潫");
- }
-
- // 鎸′綅涓嶅尮閰嶈秴鏃�
- if (currGearError && prevGearError) {
-// DEBUG("鎸′綅閿欒澧炲姞 %ld姣 褰撳墠鎸′綅 %d 鏃堕�� %f", TimeGetDiff(rtkTime, &gearErrorTimePoint),
-// ReadCarStatus(GEAR), ConvertMs2KMh(speed));
- gearErrorTime += TimeGetDiff(rtkTime, &gearErrorTimePoint);
- }
- if (gearErrorTime > examParam.gear_speed_error_cumulative_time) {
- // 绱15绉掞紝鎸′綅-杞﹂�熶笉鍖归厤锛屼笉鍚堟牸
- DEBUG("鎸′綅閿欒瓒呰繃15绉�");
- AddExamFault(30112, rtkTime);
- gearErrorTime = 0;
- }
-
- prevGearError = currGearError;
- if (prevGearError) {
- gearErrorTimePoint = *rtkTime;
- }
-
- // 璧锋鍚庢粦
- if (moveDirect != prevMoveDirect) {
- if (moveDirect == 0) {
- stopTimepoint = *rtkTime;
- StopCarOnRedArea = false;
-
- DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD,
- rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
-
- if (slideNormalDistance) {
- // 鍚庢粦锛屾墸10鍒�
- AddExamFault(30202, rtkTime);
- DEBUG("鍚庢粦瓒呰繃10鍘樼背, 浣嗕笉瓒呰繃30鍘樼背");
- }
-
- slideNormalDistance = false;
- slideLongDistance = false;
- occurSlide = false;
- } else if (moveDirect == -1 && prevMoveDirect == 0) {
- DEBUG("寮�濮嬪悗婊�");
- stopPoint = car->basePoint;
- occurSlide = true;
- }
- prevMoveDirect = moveDirect;
- } else if (moveDirect == 0) {
- // 鎸佺画鍋滆溅
- if (TimeGetDiff(rtkTime, &stopTimepoint) >= CorrectPauseCriteria(examParam.road_pause_criteria) && !StopCarOnRedArea &&
- StopOnRedArea(RoadMap, car)) {
- // 鍋滆溅瓒�2绉掞紝鍋滃湪绾㈠尯锛屼笉鍚堟牸
- AddExamFault(30128, rtkTime);
- DEBUG("绂佸仠鍖哄仠杞�");
- StopCarOnRedArea = true;
- }
- } else if (moveDirect == -1) {
- // 鎸佺画鍚庢粦
- if (occurSlide) {
- double slideDistance = DistanceOf(stopPoint, car->basePoint);
-
- if (slideDistance > examParam.road_slide_yellow_distance) {
- slideNormalDistance = true;
- }
-
- if (slideDistance > examParam.road_slide_red_distance && !slideLongDistance) {
- // 鍚庢粦瓒呰繃30鍘樼背, 涓嶅悎鏍�
- AddExamFault(30107, rtkTime);
- DEBUG("鍚庢粦瓒呰繃30鍘樼背");
- slideLongDistance = true;
- slideNormalDistance = false;
- occurSlide = false;
- }
- }
- } else {
- // 鍓嶈繘
- }
- }
-
- // 妫�娴嬬寮�姝よ矾娈碉紝鍏ㄨ溅闇�涓嶅湪鑼冨洿鍐�
- int oldid = currExamMapIndex;
- currExamMapIndex = CalcRoadIndex(currExamMapIndex, RoadMap, car);
- if (oldid != currExamMapIndex) {
- DEBUG("閬撹矾ID鍒囨崲 %d ======> %d", oldid, currExamMapIndex);
-
- if (oldid >= 0) {
- ResetCrossingStatus(RoadMap, oldid);
- ResetErrorLaneRpt(oldid);
- }
-
- if (RoadMap.calibrate) {
- if (currExamMapIndex < 0 && oldid >= 0) {
- RoadChange(RoadMap.roads[oldid].id, 0);
- } else if (currExamMapIndex >= 0) {
- RoadChange(RoadMap.roads[currExamMapIndex].id, 1);
- }
- }
- Lane.guide = 0;
- }
-
- if (currExamMapIndex >= 0) {
- car_sensor_value_t brk = ReadCarSensorValue(BREAK);
- // 妫�娴嬮�氳繃璺彛銆佷汉琛岄亾绛夊尯鍩熸椂锛岄噴鏀惧埞杞︽垨鍑忛��
- TargetFree = ApproachTarget(RoadMap, car, currExamMapIndex, (brk.value == BREAK_ACTIVE), speed, moveDirect, rtkTime);
- // 妫�鏌�
- projection_t projection;
-
- uint32_t tm1 = AppTimer_GetTickCount();
-
- projection = CalcProjectionWithRoadEdgeEx(RoadMap.roads[currExamMapIndex].rightEdge, car->carXY[car->axial[AXIAL_FRONT]]);
-
- if (projection.edgeIndex < RoadMap.roads[currExamMapIndex].rightEdge.size()) {
- if (projection.pointIndex < RoadMap.roads[currExamMapIndex].rightEdge[projection.edgeIndex].points.size() - 1) {
- double roadYaw = YawOf(RoadMap.roads[currExamMapIndex].rightEdge[projection.edgeIndex].points[projection.pointIndex + 1],
- RoadMap.roads[currExamMapIndex].rightEdge[projection.edgeIndex].points[projection.pointIndex]);
-
- int deltaAng;
-
- if (ABS((int)roadYaw - (int)car->yaw) > 180) {
- deltaAng = 360 - ABS((int)roadYaw - (int)car->yaw);
- } else {
- deltaAng = ABS((int)roadYaw - (int)car->yaw);
- }
-
- if (deltaAng < 90) {
- forward = 1;
- } else {
- forward = -1;
- }
-// DEBUG("璺鏂瑰悜 %f 杞﹁締鏂瑰悜 %f 宸�� %d 鑰楁椂 %ld", roadYaw, car->yaw, deltaAng, AppTimer_GetTickCount() - tm1);
- }
- }
- }
-
- ExitTarget(RoadMap, car, CarModelList, rtkTime);
-
- if (RoadMap.calibrate == 0) {
- oldid = CrashLineType;
- // 妫�娴嬪帇绾跨姸鎬�
- DetectLine(currExamMapIndex, RoadMap, car, CarModelList, moveDirect, rtkTime);
-
- if (oldid != CrashLineType) {
- DEBUG("鍘嬬嚎绫诲瀷鍒囨崲 %d", CrashLineType);
- }
-
- oldid = Lane.guide;
- DetectLane(RoadMap, car, currExamMapIndex, rtkTime);
-
-// DEBUG("Lane淇℃伅 road %d sep %d total %d no %d guide %d", Lane.road, Lane.sep, Lane.total, Lane.no, Lane.guide);
- if (Lane.guide > 0 && currExamMapIndex >= 0) {
- int stop_line_index;
- int act = NearbyCrossingGuide(RoadMap, stop_line_index, currExamMapIndex,
- RoadMap.roads[currExamMapIndex], car);
-
- if (act != 0 && !(act & Lane.guide)) {
- if (!GetErrorLaneRpt(currExamMapIndex, stop_line_index)) {
- DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囧悜琛岄┒ %d: %d 鏈熸湜 = %d guide = %d", currExamMapIndex,
- stop_line_index, act, Lane.guide);
- AddExamFault(30117, rtkTime);
- SetErrorLaneRpt(currExamMapIndex, stop_line_index, true);
- }
- }
- }
-
- if (Lane.guide != oldid) {
- DEBUG("瀵煎悜绫诲瀷鍒囨崲 %d", Lane.guide);
- }
- }
-// DEBUG("currExamMapIndex = %d Lane.no = %d Lane.guide = %d (%f, %f)", currExamMapIndex, Lane.no, Lane.guide, car->carXY[car->axial[AXIAL_FRONT]].X, car->carXY[car->axial[AXIAL_FRONT]].Y);
-
- if (currExamMapIndex >= 0 && Lane.guide == 0) {
- BigStraightRoadFree = AnalysisRoad(RoadMap, currExamMapIndex, Lane, car);
-
- road_end_point_t ep = NearbyRoadEndPoint(currExamMapIndex, RoadMap, car);
- // 鎻愮ず璺彛鎬庝箞璧�
- if (forward == 1) {
- HintCrossing(RoadMap, ep.road_index, ep.stop_line_index, ep.distance);
- }
-
- double freeSepDis = SeparateLength(RoadMap, Lane, car);
-
- // 鍓╀綑璺濈灏辨槸瀵煎悜绾胯捣鐐圭殑璺濈
- if (Lane.sep >= 0 && Lane.sep == ep.sep) {
- freeSepDis = ep.distance;
- }
-
- RoadCrossingFree = freeSepDis;
-
-// DEBUG("杞﹂�� %f, 绱琛岄┒璺濈 %f, 鐩撮亾鍓╀綑璺濈 %f, 杞﹂亾鍓╀綑璺濈 %f 鐗规畩鍖哄煙璺濈 %f", ConvertMs2KMh(speed), ReadOdo(), BigStraightRoadFree, RoadCrossingFree, TargetFree);
- }
-
- // 棰濆鐨勮浆鍚戞娴�
-
- if (RoadMap.calibrate == 0) {
- DetectTurn(car, moveDirect, rtkTime);
-
- ItemExam(RoadMap, currExamMapIndex, car, CarModelList, speed, moveDirect, rtkTime,
- BigStraightRoadFree,
- TargetFree > RoadCrossingFree ? RoadCrossingFree : TargetFree);
- ItemExam2(RoadMap, currExamMapIndex, car, CarModelList, forward, moveDirect);
- }
-}
-
-static void ItemExam(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end)
-{
- static double freeRunDistance;
- static double totalRunDistance;
- static double freeRunExceptDistance = 50.0;
-
- if (RoadExamStatus == ROAD_EXAM_READY_NEXT) {
- if (RoadExamItem[ROAD_EXAM_ITEM_CAR_START] == ROAD_EXAM_ITEM_NOT_EXEC) {
- CarStartInit();
- totalRunDistance = ReadOdo();
- RoadExamStatus = ROAD_EXAM_ITEM_CAR_START;
- } else {
- bool not_complete = false;
-
- if (RoadExamItem[ROAD_EXAM_ITEM_STRAIGHT] == ROAD_EXAM_ITEM_NOT_EXEC) {
- not_complete = true;
-// if (straight > 170 && road_end > 170) {
-// StartDriveStraightExam();
-// RoadExamStatus = ROAD_EXAM_ITEM_STRAIGHT;
-// return;
-// }
- }
- if (RoadExamItem[ROAD_EXAM_ITEM_OP_GEAR] == ROAD_EXAM_ITEM_NOT_EXEC) {
- not_complete = true;
-// if (road_end > 170) {
-// StartOperateGearExam();
-// RoadExamStatus = ROAD_EXAM_ITEM_OP_GEAR;
-// return;
-// }
- }
- if (RoadExamItem[ROAD_EXAM_ITEM_CHANGE_LANE] == ROAD_EXAM_ITEM_NOT_EXEC) {
- not_complete = true;
-// if (road_end > 150 && Lane.total > 1) {
-// StartChaneLaneExam(Lane.no);
-// RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
-// return;
-// }
- }
- if (RoadExamItem[ROAD_EXAM_ITEM_OVER_TAKE] == ROAD_EXAM_ITEM_NOT_EXEC) {
- not_complete = true;
-// if (road_end > 200 && Lane.total > 1) {
-// if (Lane.no == 0) {
-// // 宸插湪鏈�宸﹁溅閬擄紝鍏堝彉鏇磋溅閬擄紵
-// StartChaneLaneExam(Lane.no);
-// RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
-// } else {
-// StartOvertakeExam(Lane.no);
-// RoadExamStatus = ROAD_EXAM_ITEM_OVER_TAKE;
-// }
-// return;
-// }
- }
-
- if (!not_complete) {
-// if (road_end > 200 && ReadOdo() > examParam.road_total_distance) {
-// RoadExamStatus = ROAD_EXAM_ITEM_CAR_STOP;
-// StartStopCarExam();
-// return;
-// }
- }
- }
- } else if (RoadExamStatus == ROAD_EXAM_FREE_RUN) {
- if (ReadOdo() - freeRunDistance > freeRunExceptDistance) {
- DEBUG("瀵绘壘涓嬩竴涓瓙椤圭洰");
- RoadExamStatus = ROAD_EXAM_READY_NEXT;
- }
- } else {
- bool testing = false;
-
- switch (RoadExamStatus) {
- case ROAD_EXAM_ITEM_CAR_START:
- testing = TestCarStart(car, speed, moveDirect, rtkTime);
- break;
- case ROAD_EXAM_ITEM_OP_GEAR:
- testing = TestOperateGear(rtkTime);
- break;
- case ROAD_EXAM_ITEM_CHANGE_LANE:
- testing = TestChangeLane(Lane.no, rtkTime);
- break;
- case ROAD_EXAM_ITEM_OVER_TAKE:
- testing = TestOvertake(Lane.no, rtkTime);
- break;
- case ROAD_EXAM_ITEM_STRAIGHT:
- testing = TestDriveStraight(RoadMap, roadIndex, car, rtkTime);
- break;
- case ROAD_EXAM_ITEM_CAR_STOP:
- testing = TestStopCar(RoadMap, roadIndex, car, moveDirect, rtkTime);
- break;
- default:break;
- }
-
- if (!testing) {
- DEBUG("褰撳墠瀛愰」缁撴潫");
- RoadExamItem[RoadExamStatus] = ROAD_EXAM_ITEM_EXECED;
- if (RoadExamStatus == ROAD_EXAM_ITEM_CAR_START) {
- freeRunExceptDistance = 60.0;
- } else if (RoadExamStatus == ROAD_EXAM_ITEM_CAR_STOP) {
- // 鑰冭瘯缁撴潫
- MA_SendExamStatus(0, 0);
- PlayTTS("鑰冭瘯缁撴潫", NULL);
- } else {
- freeRunExceptDistance = 2;//250.0;
- }
-
- RoadExamStatus = ROAD_EXAM_FREE_RUN;
- freeRunDistance = ReadOdo();
- }
- }
-}
-
-static void ItemExam2(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, int forward, int moveDirect)
-{
- int item = 0;
-
- if (RoadExamStatus == ROAD_EXAM_READY_NEXT) {
- item = EntryItem(roadIndex, RoadMap, car, CarModelList, forward, moveDirect);
- }
-
- if (RoadExamStatus == RoadExamStatus && item == 6) {
- PlayTTS("鍓嶆柟浼氳溅", NULL);
- }
- else if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 3) {
- StartDriveStraightExam();
- RoadExamStatus = ROAD_EXAM_ITEM_STRAIGHT;
- } else if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 4) {
- StartOperateGearExam();
- RoadExamStatus = ROAD_EXAM_ITEM_OP_GEAR;
- } else if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 1) {
- StartChaneLaneExam(Lane.no);
- RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
- } else if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 2) {
- StartOvertakeExam(Lane.no);
- RoadExamStatus = ROAD_EXAM_ITEM_OVER_TAKE;
- } else if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 5) {
- if (RoadExamItem[ROAD_EXAM_ITEM_STRAIGHT] == ROAD_EXAM_ITEM_EXECED &&
- RoadExamItem[ROAD_EXAM_ITEM_OP_GEAR] == ROAD_EXAM_ITEM_EXECED &&
- RoadExamItem[ROAD_EXAM_ITEM_CHANGE_LANE] == ROAD_EXAM_ITEM_EXECED &&
- RoadExamItem[ROAD_EXAM_ITEM_OVER_TAKE] == ROAD_EXAM_ITEM_EXECED) {
- StartStopCarExam();
- RoadExamStatus = ROAD_EXAM_ITEM_CAR_STOP;
- }
- }
-}
-
-/*******************************************************************
- * 杞﹁締瓒婅繃鍋滄绾跨殑鏂瑰紡閫氳繃璺彛
- * @param RoadMap
- * @param road
- * @param stop_line
- * @param active
- * @param car
- */
-void CrossRoadCallback(road_exam_map &RoadMap, int road, int stop_line, int active, const car_model *car)
-{
- if (RoadMap.calibrate) {
- CrossingChange(RoadMap.roads[road].id, stop_line, 0);
- }
- SetErrorLaneRpt(road, stop_line, false);
- if (active != ROAD_ACTIVE_FORWARD) {
- ResetTurnDetect(car);
- }
-}
-
-static int isTurn(int currYaw, int prevYaw, int thres)
-{
-// DEBUG("currYaw %d prevYaw %d", currYaw, prevYaw);
- int deltaAng = 0;
-
- if (ABS(currYaw - prevYaw) > 180) {
- deltaAng = 360 - ABS(currYaw-prevYaw);
- } else {
- deltaAng = ABS(currYaw - prevYaw);
- }
-
-
-// DEBUG("瑙掑害宸�� %d", deltaAng);
-
- if (deltaAng >= thres) {
- if((( currYaw + 360 - prevYaw) % 360) < 180) {
-// DEBUG("鍙宠浆");
- return deltaAng;
- } else {
-// DEBUG("宸﹁浆");
- return 0 - deltaAng;
- }
- }
-
- return 0;
-}
-
-static void ResetTurnDetect(const car_model *car)
-{
- DEBUG("鍚姩杞悜妫�娴�");
- turnCnt = 0;
- turnTimeCnt = 0;
- prevDetectTurnTime = car->tm;
- prevYaw = car->yaw;
-}
-
-static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime)
-{
- int angle;
-
- if (turnCnt < 0 || TimeGetDiff(rtkTime, &prevDetectTurnTime) < 500) {
- return;
- }
-
- angle = isTurn((int) car->yaw, prevYaw, TURN_THRESHOLD);
-
- prevYaw = (int) car->yaw;
- prevDetectTurnTime = *rtkTime;
-
- if (turnCnt == 0) {
- if (angle != 0 && moveDirect != 0) {
- DEBUG("鏈夎浆鍚戣抗璞�");
- turnCnt++;
- beginTurnTime = *rtkTime;
- startTurnYaw = (int) car->yaw;
-
- turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
- }
- } else if (turnCnt == 1) {
- if (angle != 0 && moveDirect != 0) {
- if (angle * prevTurnWise > 0) {
- DEBUG("纭杞悜");
- // 鍚屽悜杞姩
- turnCnt++;
- turnTimeCnt = 0;
- } else {
- beginTurnTime = *rtkTime;
- startTurnYaw = (int) car->yaw;
-
- turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
- }
- } else {
- turnCnt = 0;
- }
- } else if (turnCnt >= 2) {
- if (moveDirect == 0) {
- // 鏆傚仠
- } else {
- turnTimeCnt += TimeGetDiff(rtkTime, &prevDetectTurnTime);
- int wise = isTurn((int) car->yaw, startTurnYaw, TURN_THRESHOLD);
- DEBUG("杞姩瑙掑害 %d", wise);
- if (ABS(wise) >= CROSSING_TURN_THRESHOLD) {
- // 纭杞集琛屼负锛屾娴嬪紑濮嬪垰杞集鏃惰浆鍚戠伅鎯呭喌
- turnCnt = -1;
-
- if (wise < 0) {
- if (turnSignalStatus.value != LEFT_TURN_LIGHT) {
- DEBUG("鍙樿皟鏈墦鐏�!!");
- // 娌℃墦鐏紝涓嶅悎鏍�
- AddExamFault(30121, rtkTime);
- } else if (TimeGetDiff(&beginTurnTime, &turnSignalStatus.time) <
- examParam.turn_signal_min_advance) {
- DEBUG("杞悜鐏椂闂翠笉瓒�");
- // 涓嶈冻3绉掞紝涓嶅悎鏍�
- AddExamFault(30122, rtkTime);
- }
- } else {
- if (turnSignalStatus.value != RIGHT_TURN_LIGHT) {
- DEBUG("鍙樿皟鏈墦鐏�!!");
- // 娌℃墦鐏紝涓嶅悎鏍�
- AddExamFault(30121, rtkTime);
- } else if (TimeGetDiff(&beginTurnTime, &turnSignalStatus.time) <
- examParam.turn_signal_min_advance) {
- DEBUG("杞悜鐏椂闂翠笉瓒�");
- // 涓嶈冻3绉掞紝涓嶅悎鏍�
- AddExamFault(30122, rtkTime);
- }
- }
- } else if (turnTimeCnt > D_SEC(10)) {
- // 鍙栨秷杞悜妫�娴�
- DEBUG("鍙栨秷杞悜妫�娴�1");
- turnCnt = -1;
- }
- }
- }
-
- if (turnCnt < 2 && moveDirect != 0) {
- turnTimeCnt += TimeGetDiff(rtkTime, &prevDetectTurnTime);
- if (turnTimeCnt > D_SEC(15)) {
- // 鍙栨秷杞悜妫�娴�
- DEBUG("鍙栨秷杞悜妫�娴�2");
- turnCnt = -1;
- }
- }
-
- prevTurnWise = angle;
-}
-
-/**********************************************************
- * 鎸夋暣涓溅韬槸鍚﹁鐩栬绠�
- * @param RoadMap
- * @param car
- * @return
- */
-static bool StopOnRedArea(road_exam_map &RoadMap, const car_model *car)
-{
- for (int i = 0; i < RoadMap.specialAreas.size(); ++i) {
- Polygon redArea;
- redArea.num = 0;
-
- if (RoadMap.specialAreas[i].type == GRID_AREA && RoadMap.specialAreas[i].area.size() == 4) {
- redArea.num = 4;
- redArea.point = (PointF *)malloc(4 * sizeof(PointF));
-
- redArea.point[0] = RoadMap.specialAreas[i].area[0];
- redArea.point[1] = RoadMap.specialAreas[i].area[1];
- redArea.point[2] = RoadMap.specialAreas[i].area[2];
- redArea.point[3] = RoadMap.specialAreas[i].area[3];
- } else if (RoadMap.specialAreas[i].type == ZEBRA_CROSSING) {
- int j = 0;
- for (; j < RoadMap.roads.size(); ++j) {
- if (RoadMap.specialAreas[i].road == RoadMap.roads[j].id) {
- break;
- }
- }
-
- if (j < RoadMap.roads.size()) {
-
- PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[j].leftEdge,
- RoadMap.specialAreas[i].area[0]);
- PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[j].leftEdge,
- RoadMap.specialAreas[i].area[1]);
-
- redArea.num = 4;
- redArea.point = (PointF *) malloc(4 * sizeof(PointF));
-
- redArea.point[0] = RoadMap.specialAreas[i].area[0];
- redArea.point[1] = RoadMap.specialAreas[i].area[1];
- redArea.point[2] = p2;
- redArea.point[3] = p1;
- }
- }
-
- if (redArea.num > 0) {
- for (int i = 0; i < car->bodyNum; ++i) {
- if (IntersectionOf(car->carXY[car->body[i]], &redArea) == GM_Containment) {
- free(redArea.point);
- return true;
- }
- }
- free(redArea.point);
- }
- }
-
- return false;
-}
-
-bool CrashTheLine(Line line, const car_model *car, LIST_CAR_MODEL &CarModelList)
-{
- if (CarModelList.size() < 2)
- return false;
-
- list<car_model *>::iterator iter = CarModelList.begin();
-
- Line trace;
- PointF p1, p2;
-
- p1 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->axial[AXIAL_FRONT]];
- ++iter;
- p2 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->axial[AXIAL_FRONT]];
- MakeLine(&trace, &p1, &p2);
-
- if (IntersectionOf(trace, line) == GM_Intersection &&
- IntersectionOfLine(p1, line) == -1) {
- return true;
- }
- return false;
-}
-
-static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, int forward, int moveDirect)
-{
- if (index < 0 || index >= RoadMap.roads.size() || RoadMap.examScheme.size() == 0 || CarModelList.size() < 2)
- return -1;
-
- list<car_model *>::iterator iter = CarModelList.begin();
-
- Line trace;
- PointF p1, p2;
-
- p1 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->axial[AXIAL_FRONT]];
- ++iter;
- p2 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->axial[AXIAL_FRONT]];
- MakeLine(&trace, &p1, &p2);
-
- double trace_length = DistanceOf(p1, p2);
-
- if (TriggerDetect.size() > 0) {
- auto it = TriggerDetect.begin();
-
- if (it->second.road_id != RoadMap.roads[index].id) {
- TriggerDetect.clear();
- }
- }
-
-// DEBUG("妫�鏌ヨЕ鍙戠偣 閬� %d 璺� %d", RoadMap.examScheme[0].triggerLines[0].road, RoadMap.roads[index].id);
-
- for (int j = 0; j < RoadMap.examScheme[0].triggerLines.size(); ++j) {
- if (RoadMap.examScheme[0].triggerLines[j].road == RoadMap.roads[index].id) {
-
-
- /*Line triggerLine;
-
- PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.triggerLines[i].points[0]);
-
- MakeLine(&triggerLine, &RoadMap.triggerLines[i].points[0], &p2);
-
- if (CrashTheLine(triggerLine, car, CarModelList)) {
- DEBUG("瑙﹀彂椤圭洰 %d %s (%0.4f, %0.4f)-(%0.4f, %0.4f)", RoadMap.triggerLines[i].active, RoadMap.triggerLines[i].tts.c_str(), triggerLine.X1, triggerLine.Y1, triggerLine.X2, triggerLine.Y2);
- return RoadMap.triggerLines[i].active;
- }*/
-
- /* vector<double> vec;
-
- PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[index].rightEdge,
- car->carXY[car->axial[AXIAL_FRONT]]);
- PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].rightEdge,
- RoadMap.examScheme[0].triggerLines[j].points[0]);
-
- vec.push_back(DistanceOf(car->carXY[car->axial[AXIAL_FRONT]],
- RoadMap.examScheme[0].triggerLines[j].points[0]));
- vec.push_back(DistanceOf(p2, RoadMap.examScheme[0].triggerLines[j].points[0]));
- vec.push_back(DistanceOf(p1, car->carXY[car->axial[AXIAL_FRONT]]));
-
- sort(vec.begin(), vec.end());
-
- if (fabs(vec[0] + vec[1] - vec[2]) < 0.1) {
- DEBUG("瑙﹀彂椤圭洰 %d", RoadMap.examScheme[0].triggerLines[j].active);
- return RoadMap.examScheme[0].triggerLines[j].active;
- }*/
-
-// double angleOf = DeltaYaw(YawOf(RoadMap.examScheme[0].triggerLines[j].points[0], car->carXY[car->axial[AXIAL_FRONT]]), car->yaw);
-// auto it = TriggerDetect.find(j);
-//
-// if (it != TriggerDetect.end()) {
-// if (fabs(angleOf - it->second.angleOf) > 2.0) {
-// bool trigger = false;
-//
-// trigger_detect_t up;
-//
-// if (moveDirect == 1 && forward == 1 && it->second.angleOf <= 90 && angleOf > 90) {
-// trigger = true;
-// }
-// DEBUG("瑙傚療瀛�<%d> 鏇存柊 %f -> %f", j, it->second.angleOf, angleOf);
-//
-// up.angleOf = angleOf;
-// up.road_id = RoadMap.roads[index].id;
-//
-// TriggerDetect[j] = up;
-//
-//
-//
-// if (trigger) {
-// DEBUG("pos %d 瑙﹀彂椤圭洰 %d", j, RoadMap.examScheme[0].triggerLines[j].active);
-// return RoadMap.examScheme[0].triggerLines[j].active;
-// }
-// }
-// } else {
-// trigger_detect_t up;
-//
-// up.road_id = RoadMap.roads[index].id;
-// up.angleOf = angleOf;
-//
-// TriggerDetect.insert(pair<int, trigger_detect_t>(j, up));
-//
-// DEBUG("瑙傚療瀛�<%d> 鍔犲叆 %f", j, angleOf);
-// }
-
-// double dist = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]],
-// RoadMap.examScheme[0].triggerLines[j].points[0]);
- auto it = TriggerDetect.find(j);
-
- if (it != TriggerDetect.end()) {
- PointF verticalPoint;
-
- if (moveDirect == 1 &&
- forward == 1 &&
- trace_length > 0.01 &&
- VerticalPointOnLine(RoadMap.examScheme[0].triggerLines[j].points[0], trace, verticalPoint) &&
- DistanceOf(verticalPoint, RoadMap.examScheme[0].triggerLines[j].points[0]) < it->second.distance) {
- DEBUG("pos %d 瑙﹀彂椤圭洰 %d", j, RoadMap.examScheme[0].triggerLines[j].active);
-
- DEBUG("(%f, %f) ON (%f, %f)-(%f, %f) IS (%f, %f) trace %f",
- RoadMap.examScheme[0].triggerLines[j].points[0].X,
- RoadMap.examScheme[0].triggerLines[j].points[0].Y,
- trace.X1,
- trace.Y1,
- trace.X2,
- trace.Y2,
- verticalPoint.X,
- verticalPoint.Y,
- trace_length);
-
- return RoadMap.examScheme[0].triggerLines[j].active;
- }
- } else {
- PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.examScheme[0].triggerLines[j].points[0]);
- PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].rightEdge, RoadMap.examScheme[0].triggerLines[j].points[0]);
-
- double dist1 = DistanceOf(p1, RoadMap.examScheme[0].triggerLines[j].points[0]);
- double dist2 = DistanceOf(p2, RoadMap.examScheme[0].triggerLines[j].points[0]);
-
- trigger_detect_t up;
-
- up.road_id = RoadMap.roads[index].id;
- up.distance = MAX(dist1, dist2);
-
- TriggerDetect.insert(pair<int, trigger_detect_t>(j, up));
-
- DEBUG("瑙傚療瀛�<%d> road_id %d 鍔犲叆 (%f,%f)", j, RoadMap.roads[index].id, RoadMap.examScheme[0].triggerLines[j].points[0].X, RoadMap.examScheme[0].triggerLines[j].points[0].Y);
- }
- }
- }
-
- return -1;
-}
-
-static double AnalysisRoad(road_exam_map &RoadMap, int roadIndex, lane_t lane, const car_model *car)
-{
- double distance = 0;
-
- if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
- return 0;
-
- for (int i = 0; i < RoadMap.roads[roadIndex].rightEdge.size(); ++i) {
- for (int j = 1; j < RoadMap.roads[roadIndex].rightEdge[i].points.size(); ++j) {
- PointF point = RoadMap.roads[roadIndex].rightEdge[i].points[j];
- distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, RoadMap.roads[roadIndex].leftEdge);
- if (distance > 1e-3) {
- return distance;
- }
- }
- }
-
- return 0;
-}
diff --git a/lib/src/main/cpp/test_items2/road_exam.h b/lib/src/main/cpp/test_items2/road_exam.h
deleted file mode 100644
index eeea8e7..0000000
--- a/lib/src/main/cpp/test_items2/road_exam.h
+++ /dev/null
@@ -1,43 +0,0 @@
-//
-// Created by YY on 2020/3/17.
-//
-
-#ifndef MYAPPLICATION2_ROAD_EXAM_H
-#define MYAPPLICATION2_ROAD_EXAM_H
-
-#include "../driver_test.h"
-
-
-#define GENERAL_MAP 100
-#define DRIVE_STRAIGHT_MAP 101
-#define OP_GEAER_MAP 102
-#define STOP_CAR_MAP 103
-#define THROUGH_INTERSECTION_MAP 104
-#define TURN_LEFT_MAP 105
-#define TURN_RIGHT_MAP 106
-#define THROUGH_ZEBRA_CROSSING_MAP 107
-#define THROUGH_SCHOOL_MAP 108
-#define THROUGH_BUS_STATION_MAP 109
-#define TURN_AROUND_MAP 110
-
-typedef struct
-{
- int name;
- int value;
- struct RtkTime time;
-} car_sensor_value_t;
-
-car_sensor_value_t ReadCarSensorValue(int name);
-
-void CrossRoadCallback(road_exam_map &RoadMap, int road, int stop_line, int active, const car_model *car);
-int ExamSchemeCrossing(road_exam_map &RoadMap, int roadIndex, int crossIndex);
-
-void InitRoadExam(road_exam_map &RoadMap);
-void TerminateRoadExam(void);
-void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);
-
-bool CrashTheLine(Line line, const car_model *car, LIST_CAR_MODEL &CarModelList);
-double CalcDistanceReference(PointF point, PointF refPoint, vector<edge_t> &edge);
-PointF CalcProjectionWithRoadEdge(vector<edge_t> &edge, PointF point);
-
-#endif //MYAPPLICATION2_ROAD_EXAM_H
diff --git a/lib/src/main/cpp/test_items2/smart_item.cpp b/lib/src/main/cpp/test_items2/smart_item.cpp
deleted file mode 100644
index 72e98ef..0000000
--- a/lib/src/main/cpp/test_items2/smart_item.cpp
+++ /dev/null
@@ -1,8 +0,0 @@
-//
-// Created by YY on 2020/8/18.
-//
-
-#include "smart_item.h"
-#include "../driver_test.h"
-
-
diff --git a/lib/src/main/cpp/test_items2/smart_item.h b/lib/src/main/cpp/test_items2/smart_item.h
deleted file mode 100644
index 9eb32a8..0000000
--- a/lib/src/main/cpp/test_items2/smart_item.h
+++ /dev/null
@@ -1,8 +0,0 @@
-//
-// Created by YY on 2020/8/18.
-//
-
-#ifndef MYAPPLICATION2_SMART_ITEM_H
-#define MYAPPLICATION2_SMART_ITEM_H
-
-#endif //MYAPPLICATION2_SMART_ITEM_H
diff --git a/lib/src/main/cpp/test_items2/stop_car.cpp b/lib/src/main/cpp/test_items2/stop_car.cpp
deleted file mode 100644
index a894626..0000000
--- a/lib/src/main/cpp/test_items2/stop_car.cpp
+++ /dev/null
@@ -1,142 +0,0 @@
-//
-// Created by YY on 2020/3/24.
-//
-
-#include "stop_car.h"
-#include "../driver_test.h"
-#include "../native-lib.h"
-#include "../jni_log.h"
-#include "road_exam.h"
-#include "../utils/xconvert.h"
-#include "../common/apptimer.h"
-#include "../test_common/car_sensor.h"
-#include "../defs.h"
-#include "../test_common/odo_graph.h"
-
-#define DEBUG(fmt, args...) LOGD("<road_exam stop_car> <%s>: " fmt, __func__, ##args)
-
-static bool BreakHandbreakReleaseSametime, OpenDoor;
-
-static const int ENGINE_MIN_ROTATE = 100;
-
-static double beginOdo;
-static int setup;
-
-static void TtsBack(int seq)
-{
- setup = 1;
-}
-
-void StartStopCarExam(void) {
- DEBUG("闈犺竟鍋滆溅");
- BreakHandbreakReleaseSametime = false;
- setup = 0;
- OpenDoor = false;
- PlayTTS(examParam.stop_car_begin_tts, TtsBack);
-}
-
-bool TestStopCar(road_exam_map &RoadMap, int roadIndex, const car_model *car, int moveDirect, const struct RtkTime *rtkTime) {
- static struct RtkTime time;
-
- if (setup == 0)
- return true;
-
- if (roadIndex < 0) {
- DEBUG("鍋滆溅璺濈瓒呮爣锛岄潬杈瑰仠杞︾粨鏉�");
- AddExamFault(40602, rtkTime);
- return false;
- }
-
- if (setup == 1) {
- beginOdo = ReadOdo();
- setup = 2;
- } else if (setup == 2) {
- if (moveDirect == 0) {
- time = *rtkTime;
- setup = 3;
- }
- } else if (setup == 3) {
- if (moveDirect != 0) {
- setup = 2;
- } else if (TimeGetDiff(rtkTime, &time) >= D_SEC(2)) {
- // 鍋滆溅瓒�2绉掞紝寮�濮嬪垽鏂�
- DEBUG("妫�娴嬪拰璺竟鐨勮窛绂�");
-
- 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] ]);
-
- if (DistanceOf(p1, car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ]) > examParam.stop_car_edge_red_distance ||
- DistanceOf(p2, car->carXY[ car->right_rear_tire[TIRE_OUTSIDE] ]) > examParam.stop_car_edge_red_distance) {
- DEBUG("鍋滆溅瓒呭嚭璺竟0.5绫�");
- // 鍋滆溅璺濈瓒呰繃50鍘樼背锛屼笉鍚堟牸
- AddExamFault(40606, rtkTime);
- return false;
- } else if (DistanceOf(p1, car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ]) > examParam.stop_car_edge_yellow_distance ||
- DistanceOf(p2, car->carXY[ car->right_rear_tire[TIRE_OUTSIDE] ]) > examParam.stop_car_edge_yellow_distance) {
- DEBUG("鍋滆溅瓒呭嚭璺竟0.3绫�");
- // 鍋滆溅璺濈瓒呰繃30鍘樼背锛屾墸10鍒�
- AddExamFault(40607, rtkTime);
- }
-
- time = *rtkTime;
- 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 (!BreakHandbreakReleaseSametime && brk.value == BREAK_INACTIVE && hbrk.value == BREAK_INACTIVE) {
- // 鎷夋墜鍒瑰墠锛屾澗鑴氬埞锛屾墸10鍒�
- DEBUG("鎷夋墜鍒瑰墠锛屾澗鑴氬埞");
- AddExamFault(40609, rtkTime);
- BreakHandbreakReleaseSametime = true;
- }
-
- if (door.value == DOOR_OPEN) {
- if (!OpenDoor) {
- if (rpm.value > ENGINE_MIN_ROTATE) {
- // 涓嬭溅鍓嶏紝涓嶇唲鐏紝鎵�5鍒�
- DEBUG("涓嬭溅鍓嶏紝涓嶇唲鐏�");
- AddExamFault(40610, rtkTime);
- }
- if (hbrk.value == BREAK_INACTIVE) {
- // 寮�闂ㄥ墠锛屾湭鎷夋墜鍒癸紝 鎵�10鍒�
- DEBUG("寮�闂ㄥ墠锛屾湭鎷夋墜鍒�");
- AddExamFault(40608, rtkTime);
- }
- time = *rtkTime;
- OpenDoor = true;
- }
-
- if (TimeGetDiff(rtkTime, &time) > examParam.stop_car_open_door_allow_time) {
- // 寮�闂ㄦ椂闂磋秴杩�15绉掞紝涓嶅悎鏍�
- DEBUG("寮�闂ㄦ椂闂磋秴杩�15绉�");
- AddExamFault(40605, rtkTime);
- return false;
- }
- } else {
- if (TimeGetDiff(rtkTime, &time) > D_SEC(30)) {
- DEBUG("瀹屾垚鍋滆溅瓒呮椂");
- PlayTTS(examParam.stop_car_end_tts, NULL);
- return false;
- }
- }
-
- if (OpenDoor && door.value == DOOR_CLOSE) {
- DEBUG("瀹屾垚鍋滆溅");
- PlayTTS(examParam.stop_car_end_tts, NULL);
- return false;
- }
- }
-
- if (ReadOdo() - beginOdo > examParam.stop_car_limit_distance) {
- // 150绫冲唴鏈仠杞︼紝涓嶅悎鏍�
- DEBUG("鍋滆溅璺濈瓒呮爣锛岄潬杈瑰仠杞︾粨鏉�");
- AddExamFault(40602, rtkTime);
- return false;
- }
-
- return true;
-}
diff --git a/lib/src/main/cpp/test_items2/stop_car.h b/lib/src/main/cpp/test_items2/stop_car.h
deleted file mode 100644
index 89619d7..0000000
--- a/lib/src/main/cpp/test_items2/stop_car.h
+++ /dev/null
@@ -1,13 +0,0 @@
-//
-// Created by YY on 2020/3/24.
-//
-
-#ifndef MYAPPLICATION2_STOP_CAR_H
-#define MYAPPLICATION2_STOP_CAR_H
-
-#include "../driver_test.h"
-
-void StartStopCarExam(void);
-bool TestStopCar(road_exam_map &RoadMap, int roadIndex, const car_model *car, int moveDirect, const struct RtkTime *rtkTime);
-
-#endif //MYAPPLICATION2_STOP_CAR_H
diff --git a/lib/src/main/cpp/test_items2/through_something.cpp b/lib/src/main/cpp/test_items2/through_something.cpp
deleted file mode 100644
index 3186830..0000000
--- a/lib/src/main/cpp/test_items2/through_something.cpp
+++ /dev/null
@@ -1,278 +0,0 @@
-//
-// Created by YY on 2020/3/20.
-//
-
-#include <map>
-#include <algorithm>
-
-#include "through_something.h"
-#include "../driver_test.h"
-#include "../test_common/Geometry.h"
-#include "../native-lib.h"
-#include "../jni_log.h"
-#include "../test_common/car_sensor.h"
-#include "road_exam.h"
-#include "../utils/xconvert.h"
-
-#define DEBUG(fmt, args...) LOGD("<road_exam through_something> <%s>: " fmt, __func__, ##args)
-
-using namespace std;
-
-static map <int, int> TargetReduceRec;
-static map <int, int> TargetReduceRec2;
-
-#define NOT_ENTER 1
-#define ENTER_Z 2
-#define REDUCE_SPEED 4
-#define STOP_CAR 8
-#define OVER_SPEED 16
-
-static void SetTargetReduceRec(map<int, int> &table, int key, int status)
-{
- auto it = table.find(key);
-
- if (it != table.end()) {
- table.erase(it);
- }
- table.insert(pair<int, int>(key, status));
-}
-
-static int GetTargetReduceRec(map<int, int> &table, int key)
-{
- auto it = table.find(key);
-
- if (it != table.end()) {
- return it->second;
- }
- return NOT_ENTER;
-}
-
-static void RemoveTargetReduceRec(map<int, int> &table, int key)
-{
- auto it = table.find(key);
-
- if (it != table.end()) {
- table.erase(it);
- }
-}
-
-void ResetTarget(road_exam_map &RoadMap)
-{
- TargetReduceRec.clear();
- TargetReduceRec2.clear();
-}
-
-double ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, bool dobreak, double speed, int moveDirect, const struct RtkTime *rtkTime)
-{
- vector<double> nearbyTarget; // 瀛︽牎銆佸叕浜ょ珯銆佷汉琛岄亾鍖哄煙鐨勮窛绂�
-
- if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
- return 100000;
- // 璺彛
- for (int i = 0; i < RoadMap.roads[roadIndex].stopLine.size(); ++i) {
- PointF point;
- double distance;
-
- point.X = RoadMap.roads[roadIndex].stopLine[i].line.X1;
- point.Y = RoadMap.roads[roadIndex].stopLine[i].line.Y1;
-
- distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, RoadMap.roads[roadIndex].rightEdge);
-
- int key = roadIndex * 100 + i;
- int rec = GetTargetReduceRec(TargetReduceRec, key);
-
- if (distance > 1e-3 && distance < examParam.crossing_break_valid_distance) {
- if (rec == NOT_ENTER) {
- SetTargetReduceRec(TargetReduceRec, key, ENTER_Z);
- }
- // 璁板綍鍒硅溅
- if (dobreak && !(rec & REDUCE_SPEED)) {
- DEBUG("妫�娴嬪埌璺彛鍒硅溅鍔ㄤ綔");
- SetTargetReduceRec(TargetReduceRec, key, rec | REDUCE_SPEED);
- }
- } else if (distance > 1e-3 && distance < examParam.crossing_stop_valid_distance) {
- // 璺彛鍋滆溅瑙傚療
- if (moveDirect == 0 && !(rec & STOP_CAR)) {
- DEBUG("妫�娴嬪埌璺彛鍋滆溅鍔ㄤ綔");
- SetTargetReduceRec(TargetReduceRec, key, rec | STOP_CAR);
- }
- } else if (distance > examParam.crossing_break_valid_distance + 5 && rec != NOT_ENTER) {
- RemoveTargetReduceRec(TargetReduceRec, key);
- }
- }
-
- if (RoadMap.calibrate == 1)
- goto GET_DISTANCE_OF_NEARBY_TARGET;
-
- // 浜鸿閬撱�佸叕浜ょ珯銆佸鏍�
- for (int i = 0; i < RoadMap.specialAreas.size(); i++) {
- if (RoadMap.specialAreas[i].type == GRID_AREA || RoadMap.specialAreas[i].road != RoadMap.roads[roadIndex].id)
- continue;
-
- if (RoadMap.specialAreas[i].type == ZEBRA_CROSSING || RoadMap.specialAreas[i].type == BUS_STATION_AREA) {
- double distance1 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[0], RoadMap.roads[roadIndex].rightEdge);
- double distance2 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[1], RoadMap.roads[roadIndex].rightEdge);
-
- int key = i;
- int rec = GetTargetReduceRec(TargetReduceRec2, key);
-
- if (distance1 < -1e-3 && distance2 > 1e-3) {
- nearbyTarget.push_back(0);
- } else if (distance1 > 1e-3 && distance2 > 1e-3) {
- nearbyTarget.push_back(distance1);
- }
-
- if (distance1 > 1e-3 && distance1 < examParam.crossing_break_valid_distance) {
- if (rec == NOT_ENTER) {
- SetTargetReduceRec(TargetReduceRec2, key, ENTER_Z);
- }
- // 璁板綍鍒硅溅锛屽仠杞�
- if (dobreak && !(rec & REDUCE_SPEED)) {
- DEBUG("妫�娴嬪埌浜鸿閬撶瓑鍒硅溅鍔ㄤ綔");
- SetTargetReduceRec(TargetReduceRec2, key, rec | REDUCE_SPEED);
- }
- } else if (distance1 > examParam.crossing_break_valid_distance + 5 && rec != NOT_ENTER) {
- RemoveTargetReduceRec(TargetReduceRec2, key);
- }
- } else if (RoadMap.specialAreas[i].type == SCHOOL_AREA) {
- double distance1 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[0], RoadMap.roads[roadIndex].rightEdge);
- double distance2 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[1], RoadMap.roads[roadIndex].rightEdge);
-
- int key = i;
- int rec = GetTargetReduceRec(TargetReduceRec2, key);
-
- if (distance1 < -1e-3 && distance2 > 1e-3) {
- nearbyTarget.push_back(0);
- if (rec == NOT_ENTER) {
- SetTargetReduceRec(TargetReduceRec2, key, ENTER_Z);
- }
- if (ConvertMs2KMh(speed) > examParam.cross_school_max_speed && !(rec & OVER_SPEED)) {
- SetTargetReduceRec(TargetReduceRec2, key, rec | OVER_SPEED);
-
- DEBUG("閫氳繃瀛︽牎鍖哄煙瓒呴�� %f kmh", ConvertMs2KMh(speed));
- AddExamFault(41101, rtkTime);
- }
- } else if (distance1 < -1e-3 && distance2 < -1e-3) {
- if (rec != NOT_ENTER) {
- RemoveTargetReduceRec(TargetReduceRec2, key);
- }
- } else {
- nearbyTarget.push_back(distance1);
- if (rec != NOT_ENTER) {
- RemoveTargetReduceRec(TargetReduceRec2, key);
- }
- }
- }
- }
-
- GET_DISTANCE_OF_NEARBY_TARGET:
- if (nearbyTarget.size() > 0) {
- sort(nearbyTarget.begin(), nearbyTarget.end());
- return nearbyTarget[0];
- }
- return 100000;
-}
-
-void ExitTarget(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, const struct RtkTime *rtkTime)
-{
- RECHECK:
- for (auto it = TargetReduceRec.begin(); it != TargetReduceRec.end(); ++it) {
- int road = it->first / 100;
- int x = it->first % 100;
-
- if (CrashTheLine(RoadMap.roads[road].stopLine[x].line, car, CarModelList)) {
- DEBUG("瓒婅繃璺彛 road %d 璺彛 %d", road, x);
-
- CrossRoadCallback(RoadMap, road, x, ExamSchemeCrossing(RoadMap, road, x), car);
-
- if (RoadMap.calibrate == 0) {
- if (!(it->second & REDUCE_SPEED)) {
- // 涓嶆寜瑙勫畾鍑忛�燂紝涓嶅悎鏍�
- DEBUG("璺彛 涓嶆寜瑙勫畾鍑忛��");
-
- switch (ExamSchemeCrossing(RoadMap, road, x) ) {
- case ROAD_ACTIVE_FORWARD:
- DEBUG("璺彛鐩磋鏈噺閫�");
- AddExamFault(40701, rtkTime);
- break;
- case ROAD_ACTIVE_TURN_LEFT:
- DEBUG("璺彛宸﹁浆鏈噺閫�");
- AddExamFault(40801, rtkTime);
- break;
- case ROAD_ACTIVE_TURN_RIGHT:
- DEBUG("璺彛鍙宠浆鏈噺閫�");
- AddExamFault(40901, rtkTime);
- break;
- default:
- DEBUG("璺彛 鏈厤缃� 鏈噺閫�");
- break;
- }
-
-
-// if (ExamSchemeCrossing(RoadMap, road, x) == ROAD_ACTIVE_FORWARD) {
-// AddExamFault(40701, rtkTime);
-// } else if (ExamSchemeCrossing(RoadMap, road, x) ==
-// ROAD_ACTIVE_TURN_LEFT) {
-// AddExamFault(40801, rtkTime);
-// } else if (ExamSchemeCrossing(RoadMap, road, x) ==
-// ROAD_ACTIVE_TURN_RIGHT) {
-// AddExamFault(40901, rtkTime);
-// }
- }
- if (!(it->second & STOP_CAR) && RoadMap.roads[road].stopLine[x].stopFlag) {
- // 涓嶅仠杞︾灜鏈涳紝涓嶅悎鏍�
- DEBUG("涓嶅仠杞︾灜鏈�");
- if (ExamSchemeCrossing(RoadMap, road, x) == ROAD_ACTIVE_FORWARD) {
- AddExamFault(40701, rtkTime);
- } else if (ExamSchemeCrossing(RoadMap, road, x) ==
- ROAD_ACTIVE_TURN_LEFT) {
- AddExamFault(40801, rtkTime);
- } else if (ExamSchemeCrossing(RoadMap, road, x) ==
- ROAD_ACTIVE_TURN_RIGHT) {
- AddExamFault(40901, rtkTime);
- }
- }
- }
- RemoveTargetReduceRec(TargetReduceRec, it->first);
- goto RECHECK;
- }
- }
-
- if (RoadMap.calibrate == 1)
- return;
-
- RECHECK2:
- for (auto it = TargetReduceRec2.begin(); it != TargetReduceRec2.end(); ++it) {
- int x = it->first;
- Line line;
- int roadIndex = 0;
-
- if (RoadMap.specialAreas[x].type == SCHOOL_AREA) { // 绂诲紑瀛︽牎鍖哄煙涓嶆寜璺ㄧ嚎鍒ゅ畾
- continue;
- }
-
- for (; roadIndex < RoadMap.roads.size(); ++roadIndex) {
- if (RoadMap.roads[roadIndex].id == RoadMap.specialAreas[x].road)
- break;
- }
-
- PointF point2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].leftEdge,
- RoadMap.specialAreas[x].area[0]);
- MakeLine(&line, &RoadMap.specialAreas[x].area[0], &point2);
-
- if (CrashTheLine(line, car, CarModelList)) {
- if (RoadMap.specialAreas[x].type == ZEBRA_CROSSING &&
- !(it->second & REDUCE_SPEED)) {
- DEBUG("浜鸿閬� 涓嶆寜瑙勫畾鍑忛��");
- AddExamFault(41001, rtkTime);
- }
- if (RoadMap.specialAreas[x].type == BUS_STATION_AREA &&
- !(it->second & REDUCE_SPEED)) {
- DEBUG("鍏氦绔� 涓嶆寜瑙勫畾鍑忛��");
- AddExamFault(41201, rtkTime);
- }
- RemoveTargetReduceRec(TargetReduceRec2, it->first);
- goto RECHECK2;
- }
- }
-}
diff --git a/lib/src/main/cpp/test_items2/through_something.h b/lib/src/main/cpp/test_items2/through_something.h
deleted file mode 100644
index eaa37ff..0000000
--- a/lib/src/main/cpp/test_items2/through_something.h
+++ /dev/null
@@ -1,22 +0,0 @@
-//
-// Created by YY on 2020/3/20.
-//
-
-#ifndef MYAPPLICATION2_THROUGH_SOMETHING_H
-#define MYAPPLICATION2_THROUGH_SOMETHING_H
-
-#include "../driver_test.h"
-
-void ResetTarget(road_exam_map &RoadMap);
-double ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, bool dobreak, double speed, int moveDirect, const struct RtkTime *rtkTime);
-void ExitTarget(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, const struct RtkTime *rtkTime);
-
-
-/*
-void StartThroughExam(int index, LIST_ROAD_MAP &RoadMapList);
-void InitThroughSomething(road_exam_map &RoadMap);
-void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList);
-int ExecuteThroughExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
- LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);
-*/
-#endif //MYAPPLICATION2_THROUGH_SOMETHING_H
diff --git a/lib/src/main/cpp/utils/xconvert.cpp b/lib/src/main/cpp/utils/xconvert.cpp
index b411ffb..87f3d51 100644
--- a/lib/src/main/cpp/utils/xconvert.cpp
+++ b/lib/src/main/cpp/utils/xconvert.cpp
@@ -22,6 +22,8 @@
#define DAYS_PER_WEEK 7
#define DAYS_PER_YEAR 365
+#define DAYS_UP_TO_1970 ((70*365LU) +17)
+
double ConvertKMh2Ms(int kmh)
{
return ((double)kmh) * 1000.0 / 3600.0;
@@ -86,9 +88,20 @@
}
}
+static int TimeMonthOffset( int leapStatus, uint8_t mon )
+{
+ const int monthOffsets[2][12] =
+ {
+/* jan feb mar apr may jun jul aug sep oct nov dec */
+ {0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334}, /* regular year */
+ {0, 31, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335} /* leap year */
+ };
+ return monthOffsets[leapStatus][mon] ;
+}
+
const int LibTimeDays[] = { 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 };
-static inline bool TimeYearIsLeap(int year)
+static bool TimeYearIsLeap(int year)
{
if ((year % 4) || year == 2100)
return false;
@@ -113,7 +126,7 @@
for (i = 0; i<(month - 1); i++)
daysThisYear += (int)LibTimeDays[i];
- for (i = 2000; i<year; i++) {
+ for (i = 1970; i<year; i++) {
daysThisYear += 365;
if (TimeYearIsLeap(i))
daysThisYear += 1;
@@ -127,22 +140,59 @@
return (hour*SECONDS_PER_HOUR + minute*SECONDS_PER_MINUTE + second) * 1000 + msecond;
}
-/*********************************************************
- * TIME1 - TIME2: msecond
- * @param hour1
- * @param minute1
- * @param second1
- * @param msecond1
- * @param hour2
- * @param minute2
- * @param second2
- * @param msecond2
- * @return
- */
-uint32_t TimeGetDiff(int hour1, int minute1, int second1, int msecond1, int hour2, int minute2, int second2, int msecond2)
-{
- return (TimeMakeComposite(hour1, minute1, second1, msecond1) + SECONDS_PER_DAY * 1000 -
- TimeMakeComposite(hour2, minute2, second2, msecond2)) % (SECONDS_PER_DAY * 1000);
+void TimeBreakdown(uint32_t SecondsSince1970 , struct TimeStructure *pTS) {
+ uint16_t yearSince1900;
+ uint32_t days;
+ uint32_t daysTillYearStarted;
+ uint32_t secs;
+ bool isLeapYear;
+ uint16_t leapDaysSince1900;
+
+ secs = SecondsSince1970;
+ days = SecondsSince1970 / SECONDS_PER_DAY;
+
+ // 1970.1.1 is Thursday
+ pTS->Wday = (days + 4) % DAYS_PER_WEEK; /* days since Sunday */
+
+ yearSince1900 = (days / DAYS_PER_YEAR) + 70;
+
+ leapDaysSince1900 = (yearSince1900 - 1) / 4;
+
+ if (yearSince1900 > 200)
+ leapDaysSince1900 -= 1;
+
+ daysTillYearStarted = leapDaysSince1900 + (365L * yearSince1900);
+
+ if (days + DAYS_UP_TO_1970 < daysTillYearStarted) {
+ yearSince1900--;
+
+ leapDaysSince1900 = (yearSince1900 - 1) / 4;
+
+ if (yearSince1900 > 200)
+ leapDaysSince1900 -= 1;
+ daysTillYearStarted = leapDaysSince1900 + (365L * yearSince1900);
+ }
+ days = days - (daysTillYearStarted - DAYS_UP_TO_1970);
+
+ pTS->Year = yearSince1900 + 1900;
+ pTS->Yday = days;
+
+ isLeapYear = TimeYearIsLeap(pTS->Year);
+
+ pTS->Month = 12;
+ do {
+ pTS->Month--;
+ } while (days < TimeMonthOffset(isLeapYear? 1 : 0, pTS->Month));
+
+ pTS->Day = days - TimeMonthOffset(isLeapYear? 1: 0, pTS->Month) + 1;
+
+ pTS->Month++;
+
+ secs %= SECONDS_PER_DAY;
+ pTS->Hour = secs / SECONDS_PER_HOUR;
+ secs %= SECONDS_PER_HOUR;
+ pTS->Minute = secs / 60;
+ pTS->Second = secs % 60;
}
vector<string> split(string str, string pattern)
diff --git a/lib/src/main/cpp/utils/xconvert.h b/lib/src/main/cpp/utils/xconvert.h
index 27e0bdd..1656e79 100644
--- a/lib/src/main/cpp/utils/xconvert.h
+++ b/lib/src/main/cpp/utils/xconvert.h
@@ -9,6 +9,18 @@
#include <string>
#include <vector>
+struct TimeStructure
+{
+ uint8_t Second; /* seconds after the minute [0, 59] */
+ uint8_t Minute; /* minutes after the hour [0, 59 ] */
+ uint8_t Hour; /* hours since midnight [0, 23] */
+ uint8_t Day; /* day of the month [1, 31] */
+ uint8_t Month; /* month of the year [1, 12] */
+ uint16_t Year; /* years since Christ born [0, lots] */
+ uint8_t Wday; /* days since Sunday [0, 6] */
+ uint16_t Yday; /* days since January 1 [0, 365] */
+};
+
double ConvertKMh2Ms(int kmh);
double ConvertMs2KMh(double ms);
@@ -16,6 +28,7 @@
void ConvertHex2String(char *str, const uint8_t *hex, int length);
void ConvertString2Hex(uint8_t *hex, int length, const char *str);
uint32_t TimeMakeComposite(int year, int month, int day, int hour, int minute, int second);
+void TimeBreakdown(uint32_t SecondsSince1970 , struct TimeStructure *pTS);
uint32_t TimeMakeComposite(int hour, int minute, int second, int msecond);
uint32_t TimeGetDiff(int hour1, int minute1, int second1, int msecond1, int hour2, int minute2, int second2, int msecond2);
--
Gitblit v1.8.0