24个文件已删除
31个文件已修改
3个文件已添加
| | |
| | | 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 |
| | |
| | | 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); |
| | |
| | | |
| | | uint32_t AppTimer_GetTickCount(void); |
| | | |
| | | uint32_t AppTimer_GetGmtTickCount(void); |
| | | uint64_t AppTimer_GetGmtTickCount(void); |
| | | |
| | | std::string FormatTime(const char *fmt); |
| | | |
New file |
| | |
| | | // |
| | | // 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 |
New file |
| | |
| | | // |
| | | // 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 |
| | |
| | | #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) |
| | | |
| | |
| | | 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; |
| | |
| | | #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(); |
| | |
| | | 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) |
| | |
| | | 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) |
| | |
| | | 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) |
| | |
| | | |
| | | 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; |
| | |
| | | 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) |
| | | { |
| | |
| | | if (start == 0) { |
| | | DEBUG("结束考试"); |
| | | |
| | | TerminateRoadExam(); |
| | | TerminateAreaExam(); |
| | | |
| | | |
| | | ExamStart = false; |
| | | MA_SendExamStatus(0, 0); |
| | |
| | | |
| | | // 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); |
| | |
| | | } |
| | | 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()); |
| | | // 向UI上报车辆点坐标 |
| | | 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) { |
| | |
| | | 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) { |
| | |
| | | |
| | | 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 最近的,b 先前的 |
| | | * @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; |
| | | |
| | |
| | | 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; |
| | |
| | | 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); |
| | | } |
| | | |
| | | /*************************************************************** |
| | |
| | | * @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); |
| | | } |
| | |
| | | |
| | | 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; |
| | | } |
| | |
| | | #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 |
| | |
| | | #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; |
| | |
| | | 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 |
| | |
| | | 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, |
| | |
| | | |
| | | 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); |
| | | |
New file |
| | |
| | | // |
| | | // 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 |
| | |
| | | #include "../jni_log.h" |
| | | #include "../mcu/mcu_if.h" |
| | | #include "../test_common/car_sensor.h" |
| | | #include "../map.h" |
| | | |
| | | #include <string> |
| | | #include <iostream> |
| | |
| | | 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; |
| | | } |
| | | } |
| | | } |
| | |
| | | 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); |
| | |
| | | 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"); |
| | |
| | | |
| | | 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(); |
| | |
| | | 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; |
| | | } |
| | | } |
| | | } |
| | |
| | | Document doc; |
| | | doc.Parse(value); |
| | | if (!doc.HasParseError()) { |
| | | DEBUG("解析灯光配置..."); |
| | | /*DEBUG("解析灯光配置..."); |
| | | if (doc.HasMember("exam")) { |
| | | const Value& s = doc["exam"]; |
| | | s.GetInt(); |
| | |
| | | |
| | | delete []content; |
| | | } |
| | | } |
| | | }*/ |
| | | } |
| | | break; |
| | | } |
| | | case ID_MS_INQ_ROAD_CROSSING: { |
| | | MasterInqRoadStatus(); |
| | | break; |
| | | } |
| | | case ID_MS_BLUETOOTH_NAME: { |
| | |
| | | #define MYAPPLICATION2_COMM_IF_H |
| | | |
| | | #include <cstdint> |
| | | #include <vector> |
| | | #include "../driver_test.h" |
| | | |
| | | struct mcuBrief { |
| | |
| | | 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 { |
| | |
| | | |
| | | 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"); |
| | |
| | | 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) { |
| | |
| | | } |
| | | |
| | | case ID_GNSS_DATA: { |
| | | // DEBUG("ID_GNSS_DATA %d", length); |
| | | DEBUG("ID_GNSS_DATA %d", length); |
| | | memcpy(RxBuf + RxBufLen, data, length); |
| | | RxBufLen += length; |
| | | |
| | |
| | | pCls = nullptr; |
| | | } |
| | | |
| | | static void AhpRun(void) |
| | | void InitAhp(void) |
| | | { |
| | | // TODO |
| | | static struct serial_config serialConfig; |
| | | |
| | | strcpy(serialConfig.name, "/dev/ttyCH341USB3"); |
| | |
| | | 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) { |
| | |
| | | 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); |
| | | |
| | |
| | | #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) |
| | | |
| | |
| | | static gpsStatus_t gpsStatus; |
| | | static char rtkModel[32] = {0}; |
| | | |
| | | static rtk_info CurrRTKInfo; |
| | | static bool needSetPjk = false; |
| | | static int lostCnt; |
| | | |
| | | // 组合PJK和TRA的消息内容 |
| | | 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); |
| | |
| | | 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) { |
| | |
| | | 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) |
| | |
| | | }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; |
| | |
| | | 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) |
| | |
| | | |
| | | return sst.str(); |
| | | } |
| | | }rtk_info; |
| | | }rtk_info_t; |
| | | |
| | | void RtkCommModeSel(int mode); |
| | | void ConfigRTKModule(bool ayDevice); |
| | |
| | | #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 |
| | |
| | | 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; |
| | |
| | | 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; |
| | |
| | | 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; |
| | |
| | | |
| | | ost1 = AppTimer_GetTickCount() - ost1; |
| | | |
| | | UpdateRTKInfo(rtk); |
| | | if (rtk->qf == 3) |
| | | UpdateRTKInfo(rtk); |
| | | |
| | | ost = AppTimer_GetTickCount() - ost; |
| | | |
| | |
| | | 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; |
| | | |
| | |
| | | } |
| | | |
| | | 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; |
| | |
| | | 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; |
| | |
| | | // 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); |
| | | |
| | |
| | | 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) |
| | | { |
| | |
| | | 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); |
| | |
| | | } |
| | | } |
| | | |
| | | 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))) |
| | |
| | | } |
| | | |
| | | /********************************************************* |
| | | * p2----------->p1 线端和Y轴的夹角 |
| | | * p1----------->p2 线端和Y轴的夹角 |
| | | * @param p1 |
| | | * @param p2 |
| | | * @return yaw |
| | |
| | | { |
| | | 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相对于base的逆时针夹角 |
| | | * @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) |
| | |
| | | * @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; |
| | | |
| | |
| | | } |
| | | |
| | | return deg; |
| | | } |
| | | } |
| | | |
| | | 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; |
| | | } |
| | |
| | | #include <initializer_list> |
| | | #include <vector> |
| | | |
| | | enum Relation |
| | | typedef enum |
| | | { |
| | | GM_None, |
| | | GM_Tangent, |
| | | GM_Intersection, |
| | | GM_Containment |
| | | }; |
| | | } relation_t; |
| | | |
| | | typedef struct PointF_ { |
| | | double X; |
| | |
| | | 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); |
| | |
| | | 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); |
| | |
| | | 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); |
| | |
| | | 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 |
| | |
| | | // |
| | | |
| | | #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; |
| | | } |
| | | |
| | |
| | | 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; |
| | | } |
| | | } |
| | | } |
| | |
| | | #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 |
| | |
| | | #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) |
| | | { |
| | |
| | | { |
| | | 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; |
| | | } |
| | |
| | | #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 |
| | |
| | | |
| | | 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) |
| | | { |
| | |
| | | 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; |
| | |
| | | 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; |
| | |
| | | } |
| | | |
| | | // 车轮是否压边线 |
| | | 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; |
| | | |
| | |
| | | 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 |
| | |
| | | // |
| | | // 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" |
| | |
| | | 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]; |
| | |
| | | |
| | | 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; |
| | |
| | | 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; |
| | |
| | | |
| | | 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); |
| | |
| | | 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 ? "成功" : "失败"); |
| | |
| | | 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; |
| | | } |
| | |
| | | |
| | | 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 |
| | |
| | | // |
| | | // Created by YY on 2019/10/23. |
| | | // |
| | | // |
| | | // 4 ___________________________ 3 |
| | | // | | |
| | | // | | |
| | | // | | |
| | | // ________________| |___________1 |
| | | // 6 5 2 |
| | | // |
| | | // |
| | | // 7______________________________________________________0 |
| | | // |
| | | |
| | | #include "park_edge.h" |
| | | #include "../jni_log.h" |
| | |
| | | |
| | | const uint32_t CHECK_PARK_DELAY = 400; |
| | | |
| | | static int mapIndex = 0; |
| | | static bool reportExamTimeout; |
| | | static bool reportParkFail; |
| | | |
| | |
| | | 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; |
| | |
| | | }*/ |
| | | } |
| | | |
| | | 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; |
| | |
| | | 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) { |
| | |
| | | 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); |
| | | }*/ |
| | | } |
| | | |
| | |
| | | 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; |
| | | |
| | |
| | | } |
| | | |
| | | // 车身是否压库位线 |
| | | static bool CrashRedLine2(const Polygon *map, const car_model *car) |
| | | static bool CrashRedLine2(const Polygon *map, const car_model_t *car) |
| | | { |
| | | bool ret = false; |
| | | |
| | |
| | | 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) { |
| | |
| | | |
| | | 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; |
| | | } |
| | |
| | | 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 |
| | |
| | | // Created by YY on 2019/10/31. |
| | | // |
| | | |
| | | // |9 |
| | | // | |
| | | // | |
| | | // | |
| | | // | |
| | | // 7-----------------|8 |
| | | // 6_________________|5 |
| | | // 3_________________|4 |
| | | // | |
| | | // 2-----------------|1 |
| | | // | |
| | | // | |
| | | // | |
| | | // |0 |
| | | |
| | | #include <cstdlib> |
| | | #include <vector> |
| | | #include <cmath> |
| | |
| | | using namespace std; |
| | | |
| | | |
| | | const uint32_t STOP_CAR_TIME = D_SEC(1); |
| | | |
| | | |
| | | const double EPSILON = 1e-3; |
| | | |
| | |
| | | |
| | | 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; |
| | |
| | | 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; |
| | | } |
| | |
| | | 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); |
| | |
| | | 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; |
| | | |
| | |
| | | } |
| | | |
| | | free(carBody.point); |
| | | free(map2.point); |
| | | free(map2.point);*/ |
| | | return ret; |
| | | } |
| | |
| | | |
| | | 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 |
| | |
| | | // 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" |
| | |
| | | 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; |
| | | } |
| | | } |
| | | |
| | | // 检查转向状态 |
| | |
| | | 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 { |
| | |
| | | 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; |
| | |
| | | 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; |
| | | } |
| | |
| | | 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 |
| | |
| | | #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; |
| | |
| | | } |
| | | } |
| | | |
| | | 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; |
| | |
| | | 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; |
| | |
| | | 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) |
| | |
| | | #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); |
| | | |
| | |
| | | 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); |
| | | |