From 27fc91fbe8f88b6885356e68828cfe1ce1db7601 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期三, 28 二月 2024 09:53:13 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/driver_test.cpp | 567 ++++++++++++++++++++++++++++++--------------------------
1 files changed, 303 insertions(+), 264 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 1563f56..a85c306 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -15,6 +15,7 @@
#include <numeric>
#include <algorithm>
#include <string>
+#include <tuple>
#include "driver_test.h"
#include "defs.h"
@@ -26,9 +27,9 @@
#include "test_items/park_bottom.h"
#include "test_items/park_edge.h"
#include "test_items/error_list.h"
-#include "test_items/turn_a90.h"
+#include "test_items/right_corner.h"
#include "test_items/driving_curve.h"
-#include "test_items/stop_and_start.h"
+#include "test_items/uphill.h"
#include "master/comm_if.h"
#include "utils/xconvert.h"
#include "utils/num.h"
@@ -127,19 +128,13 @@
static const char DEFAULT_CROSSING_TURN_BACK_TTS[] = "鍓嶆柟閫夋嫨鍚堥�傚湴鐐规帀澶�";
static const char DEFAULT_CROSSING_TURN_UNKNOWN_TTS[] = "鍓嶆柟璺彛鍚粠鏁欑粌鎸囩ず";
-static bool ExamStart = false;
static int ExamType;
static bool reportSeatbeltEject;
vector<ExamFault> ExamFaultList;
static int examFaultIndex = 0;
-//static LIST_AREA_MAP AreaMapList;
-static area_map_t AreaMap;
-
ilovers::semaphore sem(0);
-
-static road_exam_map RoadMap;
static int exam_dummy_light;
@@ -152,8 +147,6 @@
static bool engineRuning = false;
const int ENGINE_MIN_ROTATE = 200;
static bool engineStart = false;
-
-exam_param_t examParam;
#define MOV_AVG_SIZE 1
#define RTK_BUFFER_SIZE 100
@@ -177,19 +170,23 @@
static void work_thread(void);
static void UploadModeling(motion_t &motion, modeling_t &modeling);
+prime_t& GetPrime(void)
+{
+ return prime;
+}
+
void DriverTestInit(void)
{
- ExamStart = false;
+ prime.examing = false;
+ prime.pMotion = &realtimeMotionStatus;
+ prime.pModeling = realtimeBodyModeling;
+ prime.pModel = &CarModel;
+
SetExamParamDefault();
// CarModelList.clear();
// AreaMapList.clear();
-
- RoadMap.roads.clear();
- RoadMap.specialAreas.clear();
- RoadMap.forbidLines.clear();
- RoadMap.examScheme.clear();
CarSensorInit();
@@ -201,97 +198,97 @@
static void SetExamParamDefault(void)
{
- examParam.hold_start_key_limit_time = DEFAULT_START_KEY_HOLD_TIME;
- examParam.curve_pause_criteria = DEFAULT_CURVE_PAUSE_TIME;
- examParam.park_bottom_pause_criteria = DEFAULT_PARK_BOTTOM_PAUSE_TIME;
- examParam.park_bottom_limit_time = DEFAULT_PARK_BOTTOM_FINISH_TIME;
- examParam.park_edge_pause_criteria = DEFAULT_PART_EDGE_PAUSE_TIME;
- examParam.park_edge_limit_time = DEFAULT_PARK_EDGE_FINISH_TIME;
- examParam.turn_a90_pause_criteria = DEFAULT_TURN_A90_PAUSE_TIME;
+ prime.examParam.hold_start_key_limit_time = DEFAULT_START_KEY_HOLD_TIME;
+ prime.examParam.curve_pause_criteria = DEFAULT_CURVE_PAUSE_TIME;
+ prime.examParam.park_bottom_pause_criteria = DEFAULT_PARK_BOTTOM_PAUSE_TIME;
+ prime.examParam.park_bottom_limit_time = DEFAULT_PARK_BOTTOM_FINISH_TIME;
+ prime.examParam.park_edge_pause_criteria = DEFAULT_PART_EDGE_PAUSE_TIME;
+ prime.examParam.park_edge_limit_time = DEFAULT_PARK_EDGE_FINISH_TIME;
+ prime.examParam.turn_a90_pause_criteria = DEFAULT_TURN_A90_PAUSE_TIME;
- examParam.ramp_stoppoint_red_distance = DEFAULT_RAMP_STOPPOINT_RED_DISTANCE;
- examParam.ramp_edge_yellow_distance = DEFAULT_RAMP_EDGE_YELLOW_DISTANCE;
- examParam.ramp_edge_red_distance = DEFAULT_RAMP_EDGE_RED_DISTANCE;
- examParam.ramp_slide_yellow_distance = DEFAULT_RAMP_SLIDE_YELLOW_DISTANCE;
- examParam.ramp_slide_red_distance = DEFAULT_RAMP_SLIDE_RED_DISTANCE;
- examParam.ramp_start_car_limit_time = DEFAULT_RAMP_FINISH_TIME;
+ prime.examParam.ramp_stoppoint_red_distance = DEFAULT_RAMP_STOPPOINT_RED_DISTANCE;
+ prime.examParam.ramp_edge_yellow_distance = DEFAULT_RAMP_EDGE_YELLOW_DISTANCE;
+ prime.examParam.ramp_edge_red_distance = DEFAULT_RAMP_EDGE_RED_DISTANCE;
+ prime.examParam.ramp_slide_yellow_distance = DEFAULT_RAMP_SLIDE_YELLOW_DISTANCE;
+ prime.examParam.ramp_slide_red_distance = DEFAULT_RAMP_SLIDE_RED_DISTANCE;
+ prime.examParam.ramp_start_car_limit_time = DEFAULT_RAMP_FINISH_TIME;
- examParam.road_slide_yellow_distance = DEFAULT_ROAD_SLIDE_YELLOW_DISTANCE;
- examParam.road_slide_red_distance = DEFAULT_ROAD_SLIDE_RED_DISTANCE;
- examParam.road_total_distance = DEFAULT_ROAD_MAX_DISTANCE;
- examParam.road_max_speed = DEFAULT_ROAD_MAX_SPEED;
+ prime.examParam.road_slide_yellow_distance = DEFAULT_ROAD_SLIDE_YELLOW_DISTANCE;
+ prime.examParam.road_slide_red_distance = DEFAULT_ROAD_SLIDE_RED_DISTANCE;
+ prime.examParam.road_total_distance = DEFAULT_ROAD_MAX_DISTANCE;
+ prime.examParam.road_max_speed = DEFAULT_ROAD_MAX_SPEED;
- examParam.gear_speed_table.resize(6);
- for (int i = 0; i < examParam.gear_speed_table.size(); ++i) {
- examParam.gear_speed_table[i].resize(2);
+ prime.examParam.gear_speed_table.resize(6);
+ for (int i = 0; i < prime.examParam.gear_speed_table.size(); ++i) {
+ prime.examParam.gear_speed_table[i].resize(2);
}
- examParam.gear_speed_table[0][0] = 0;
- examParam.gear_speed_table[0][1] = 20;
- examParam.gear_speed_table[1][0] = 5;
- examParam.gear_speed_table[1][1] = 30;
- examParam.gear_speed_table[2][0] = 15;
- examParam.gear_speed_table[2][1] = 40;
- examParam.gear_speed_table[3][0] = 25;
- examParam.gear_speed_table[3][1] = 10000;
- examParam.gear_speed_table[4][0] = 35;
- examParam.gear_speed_table[4][1] = 10000;
- examParam.gear_speed_table[5][0] = 45;
- examParam.gear_speed_table[5][1] = 10000;
+ prime.examParam.gear_speed_table[0][0] = 0;
+ prime.examParam.gear_speed_table[0][1] = 20;
+ prime.examParam.gear_speed_table[1][0] = 5;
+ prime.examParam.gear_speed_table[1][1] = 30;
+ prime.examParam.gear_speed_table[2][0] = 15;
+ prime.examParam.gear_speed_table[2][1] = 40;
+ prime.examParam.gear_speed_table[3][0] = 25;
+ prime.examParam.gear_speed_table[3][1] = 10000;
+ prime.examParam.gear_speed_table[4][0] = 35;
+ prime.examParam.gear_speed_table[4][1] = 10000;
+ prime.examParam.gear_speed_table[5][0] = 45;
+ prime.examParam.gear_speed_table[5][1] = 10000;
- examParam.gear_n_allow_time = DEFAULT_GEAR_N_TIME;
- examParam.same_gear_min_time = DEFAULT_SAME_GEAR_HOLD_TIME; // x绉掑唴锛屼笉鍏佽N->X->N->X缃悓涓�鎸′綅
- examParam.gear_speed_error_cumulative_time = DEFAULT_GEAR_SPEED_ERROR_MAX_TIME;
- examParam.road_pause_criteria = DEFAULT_ROAD_PAUSE_TIME;
- examParam.continuous_change_lane_min_time = DEFAULT_CHANGE_LANE_MIN_TIME;
- examParam.crash_dotted_line_cumulative_time = DEFAULT_CRASH_DOTTED_LINE_MAX_TIME;
- examParam.turn_signal_min_advance = DEFAULT_TURN_SIGNAL_MIN_ADVANCE;
- examParam.start_car_max_rpm = DEFAULT_START_CAR_MAX_RMP;
- examParam.start_car_limit_distance = DEFAULT_START_CAR_DISTANCE;
- examParam.open_door_drive_allow_distance = DEFAULT_START_CAR_OPEN_DOOR_DISTANCE;
+ prime.examParam.gear_n_allow_time = DEFAULT_GEAR_N_TIME;
+ prime.examParam.same_gear_min_time = DEFAULT_SAME_GEAR_HOLD_TIME; // x绉掑唴锛屼笉鍏佽N->X->N->X缃悓涓�鎸′綅
+ prime.examParam.gear_speed_error_cumulative_time = DEFAULT_GEAR_SPEED_ERROR_MAX_TIME;
+ prime.examParam.road_pause_criteria = DEFAULT_ROAD_PAUSE_TIME;
+ prime.examParam.continuous_change_lane_min_time = DEFAULT_CHANGE_LANE_MIN_TIME;
+ prime.examParam.crash_dotted_line_cumulative_time = DEFAULT_CRASH_DOTTED_LINE_MAX_TIME;
+ prime.examParam.turn_signal_min_advance = DEFAULT_TURN_SIGNAL_MIN_ADVANCE;
+ prime.examParam.start_car_max_rpm = DEFAULT_START_CAR_MAX_RMP;
+ prime.examParam.start_car_limit_distance = DEFAULT_START_CAR_DISTANCE;
+ prime.examParam.open_door_drive_allow_distance = DEFAULT_START_CAR_OPEN_DOOR_DISTANCE;
- examParam.prepare_tts = DEFAULT_PREPARE_TTS;
- examParam.touch_leftfront_tts = DEFAULT_TOUCH_LEFT_FRONT;
- examParam.touch_leftrear_tts = DEFAULT_TOUCH_LEFT_REAR;
- examParam.touch_rightfront_tts = DEFAULT_TOUCH_RIGHT_FRONT;
- examParam.touch_rightrear_tts = DEFAULT_TOUCH_RIGHT_REAR;
- examParam.start_engine_tts = DEFAULT_START_ENGINE;
+ prime.examParam.prepare_tts = DEFAULT_PREPARE_TTS;
+ prime.examParam.touch_leftfront_tts = DEFAULT_TOUCH_LEFT_FRONT;
+ prime.examParam.touch_leftrear_tts = DEFAULT_TOUCH_LEFT_REAR;
+ prime.examParam.touch_rightfront_tts = DEFAULT_TOUCH_RIGHT_FRONT;
+ prime.examParam.touch_rightrear_tts = DEFAULT_TOUCH_RIGHT_REAR;
+ prime.examParam.start_engine_tts = DEFAULT_START_ENGINE;
- examParam.start_car_begin_tts = DEFAULT_START_CAR_BEGIN_TTS;
- examParam.start_car_end_tts = DEFAULT_START_CAR_END_TTS;
- examParam.change_lane_limit_distance = CHANGE_LANE_MAX_DISTANCE;
- examParam.change_lane_begin_tts = DEFAULT_CHANGE_LANE_BEGIN_TTS;
- examParam.change_lane_end_tts = DEFAULT_CHANGE_LANE_END_TTS;
- examParam.shift_limit_distance = DEFAULT_SHIFT_MAX_DISTANCE;
- examParam.shift_hold_time = DEFAULT_SHIFT_HOLD_TIME;
- examParam.shift_begin_tts = DEFAULT_SHIFT_BEGIN_TTS;
- examParam.shift_end_tts = DEFAULT_SHIFT_END_TTS;
- examParam.shift_up_tts = "";
- examParam.shift_down_tts = "";
+ prime.examParam.start_car_begin_tts = DEFAULT_START_CAR_BEGIN_TTS;
+ prime.examParam.start_car_end_tts = DEFAULT_START_CAR_END_TTS;
+ prime.examParam.change_lane_limit_distance = CHANGE_LANE_MAX_DISTANCE;
+ prime.examParam.change_lane_begin_tts = DEFAULT_CHANGE_LANE_BEGIN_TTS;
+ prime.examParam.change_lane_end_tts = DEFAULT_CHANGE_LANE_END_TTS;
+ prime.examParam.shift_limit_distance = DEFAULT_SHIFT_MAX_DISTANCE;
+ prime.examParam.shift_hold_time = DEFAULT_SHIFT_HOLD_TIME;
+ prime.examParam.shift_begin_tts = DEFAULT_SHIFT_BEGIN_TTS;
+ prime.examParam.shift_end_tts = DEFAULT_SHIFT_END_TTS;
+ prime.examParam.shift_up_tts = "";
+ prime.examParam.shift_down_tts = "";
- examParam.straight_begin_tts = DEFAULT_STRAIGHT_BEGIN_TTS;
- examParam.straight_end_tts = DEFAULT_STRAIGHT_END_TTS;
- examParam.straight_limit_distance = DEFAULT_STRAIGHT_MAX_DISTANCE;
- examParam.straight_max_offset = DEFAULT_STRAIGHT_MAX_OFFSET;
+ prime.examParam.straight_begin_tts = DEFAULT_STRAIGHT_BEGIN_TTS;
+ prime.examParam.straight_end_tts = DEFAULT_STRAIGHT_END_TTS;
+ prime.examParam.straight_limit_distance = DEFAULT_STRAIGHT_MAX_DISTANCE;
+ prime.examParam.straight_max_offset = DEFAULT_STRAIGHT_MAX_OFFSET;
- examParam.overtake_limit_distance = DEFAULT_OVERTAKE_MAX_DISTANCE;
- examParam.overtake_begin_tts = DEFAULT_OVERTAKE_BEGIN_TTS;
- examParam.overtake_end_tts = DEFAULT_OVERTAKE_END_TTS;
+ prime.examParam.overtake_limit_distance = DEFAULT_OVERTAKE_MAX_DISTANCE;
+ prime.examParam.overtake_begin_tts = DEFAULT_OVERTAKE_BEGIN_TTS;
+ prime.examParam.overtake_end_tts = DEFAULT_OVERTAKE_END_TTS;
- examParam.stop_car_limit_distance = DEFAULT_STOP_CAR_MAX_DISTANCE;
- examParam.stop_car_open_door_allow_time = DEFAULT_STOP_CAR_OPEN_DOOR_MAX_TIME;
- examParam.stop_car_edge_red_distance = DEFAULT_STOP_CAR_EDGE_RED_DISTANCE;
- examParam.stop_car_edge_yellow_distance = DEFAULT_STOP_CAR_EDGE_YELLOW_DISTANCE;
- examParam.stop_car_begin_tts = DEFAULT_STOP_CAR_BEGIN_TTS;
- examParam.stop_car_end_tts = DEFAULT_STOP_CAR_END_TTS;
- examParam.crossing_stop_valid_distance = DEFAULT_CROSSING_STOP_VALID_DISTANCE;
- examParam.cross_school_max_speed = DEFAULT_CROSS_SCHOOL_MAX_SPEED;
- examParam.crossing_break_valid_distance = DEFAULT_CROSS_BREAK_VALID_DISTANCE;
+ prime.examParam.stop_car_limit_distance = DEFAULT_STOP_CAR_MAX_DISTANCE;
+ prime.examParam.stop_car_open_door_allow_time = DEFAULT_STOP_CAR_OPEN_DOOR_MAX_TIME;
+ prime.examParam.stop_car_edge_red_distance = DEFAULT_STOP_CAR_EDGE_RED_DISTANCE;
+ prime.examParam.stop_car_edge_yellow_distance = DEFAULT_STOP_CAR_EDGE_YELLOW_DISTANCE;
+ prime.examParam.stop_car_begin_tts = DEFAULT_STOP_CAR_BEGIN_TTS;
+ prime.examParam.stop_car_end_tts = DEFAULT_STOP_CAR_END_TTS;
+ prime.examParam.crossing_stop_valid_distance = DEFAULT_CROSSING_STOP_VALID_DISTANCE;
+ prime.examParam.cross_school_max_speed = DEFAULT_CROSS_SCHOOL_MAX_SPEED;
+ prime.examParam.crossing_break_valid_distance = DEFAULT_CROSS_BREAK_VALID_DISTANCE;
- examParam.crossing_go_straight_tts = DEFAULT_CROSSING_GO_STRAIGHT_TTS;
- examParam.crossing_turn_left_tts = DEFAULT_CROSSING_TURN_LEFT_TTS;
- examParam.crossing_turn_right_tts = DEFAULT_CROSSING_TURN_RIGHT_TTS;
- examParam.crossing_turn_back_tts = DEFAULT_CROSSING_TURN_BACK_TTS;
- examParam.crossing_turn_unknown_tts = DEFAULT_CROSSING_TURN_UNKNOWN_TTS;
+ prime.examParam.crossing_go_straight_tts = DEFAULT_CROSSING_GO_STRAIGHT_TTS;
+ prime.examParam.crossing_turn_left_tts = DEFAULT_CROSSING_TURN_LEFT_TTS;
+ prime.examParam.crossing_turn_right_tts = DEFAULT_CROSSING_TURN_RIGHT_TTS;
+ prime.examParam.crossing_turn_back_tts = DEFAULT_CROSSING_TURN_BACK_TTS;
+ prime.examParam.crossing_turn_unknown_tts = DEFAULT_CROSSING_TURN_UNKNOWN_TTS;
}
static void ReadDriverExamPrimerTimeout(apptimer_var_t val)
@@ -308,134 +305,113 @@
void ClearAreaMap(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);
-// }
-//
-// 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)
-{
- if (map == NULL || pointNum == 0 || ExamStart)
- return;
- DEBUG("鍔犲叆鍦板浘淇℃伅 id %d type %d pointNum %d point2Num %d", id, type, pointNum, pointNum2);
-
-// 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);
+ if (!prime.examing) {
+ DEBUG("娓呴櫎鎵�鏈夊湴鍥�");
+ vector<curve_map_t>().swap(std::get<MAP_TYPE_CURVE>(prime.maps));
+ vector<park_button_map_t>().swap(std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps));
+ vector<park_edge_map_t>().swap(std::get<MAP_TYPE_PARK_EDGE>(prime.maps));
+ vector<turn_a90_map_t>().swap(std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps));
+ vector<uphill_map_t>().swap(std::get<MAP_TYPE_UPHILL>(prime.maps));
+ }
}
void AddCurveMap(curve_map_t &map)
{
- AreaMap.curve_map.push_back(map);
+ DEBUG("娣诲姞鏇茬嚎琛岄┒鍦板浘");
+ if (prime.examing) {
+ return;
+ }
+ for (auto m: std::get<MAP_TYPE_CURVE>(prime.maps)) {
+ if (m.id == map.id) {
+ return;
+ }
+ }
+
+ std::get<MAP_TYPE_CURVE>(prime.maps).push_back(map);
}
+// 濡傛灉鏈噰闆嗚溅浣嶅彛铏氱嚎鍐呬晶锛屽垯闇�瑕佽绠楀嚭鏉�
void AddParkButtonMap(park_button_map_t &map)
{
- AreaMap.park_button_map.push_back(map);
+ DEBUG("娣诲姞鍊掕溅鍏ュ簱鍦板浘");
+ if (prime.examing) {
+ return;
+ }
+
+ for (auto m: std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)) {
+ if (m.id == map.id) {
+ return;
+ }
+ }
+ if (map.points.size() == 8) {
+ PointF p8 = PointExtend(map.points[2], map.line_width, YawOf(map.points[2], map.points[3]));
+ PointF p9 = PointExtend(map.points[5], map.line_width, YawOf(map.points[5], map.points[4]));
+ map.points.push_back(p8);
+ map.points.push_back(p9);
+ }
+ std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps).push_back(map);
+ DEBUG("鍊掑簱鍦板浘鐐规暟锛�%d", map.points.size());
+ DEBUG("鍊掑簱鍦板浘鐐规暟 %d",std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[0].points.size());
}
+// 濡傛灉鏈噰闆嗚溅浣嶅彛铏氱嚎鍐呬晶锛屽垯闇�瑕佽绠楀嚭鏉�
void AddParkEdgeMap(park_edge_map_t &map)
{
- AreaMap.park_edge_map.push_back(map);
-}
-
-void CleanRoadMap(void)
-{
- if (ExamStart) return;
-
- DEBUG("娓呴櫎鏃х殑璺�冨湴鍥�");
-
- vector<road_t>().swap(RoadMap.roads);
- vector<special_area_t>().swap(RoadMap.specialAreas);
- vector<forbid_line_t>().swap(RoadMap.forbidLines);
-// vector<scheme_t>().swap(RoadMap.examScheme);
-}
-
-void SetRoadMap(road_exam_map &map, vector<scheme_t> &scheme)
-{
- if (ExamStart) return;
-
- RoadMap.roads.assign(map.roads.begin(), map.roads.end());
- RoadMap.specialAreas.assign(map.specialAreas.begin(), map.specialAreas.end());
-// RoadMap.triggerLines.assign(map.triggerLines.begin(), map.triggerLines.end());
- RoadMap.forbidLines.assign(map.forbidLines.begin(), map.forbidLines.end());
-
-// RoadMap.examScheme.assign(scheme.begin(), scheme.end());
-
- DEBUG("寰楀埌鏂扮殑璺�冨湴鍥� 璺暟閲� %d 鐗规畩鍖哄煙鏁伴噺 %d 鍏朵粬绂佹绾挎暟閲� %d 椤圭洰鏁伴噺 %d", RoadMap.roads.size(), RoadMap.specialAreas.size(), RoadMap.forbidLines.size(), RoadMap.examScheme.size());
-
- /*for (int i = 0; i < RoadMap.roads.size(); ++i) {
- DEBUG("璺� id = %d", RoadMap.roads[i].id);
- DEBUG("宸﹁竟绾挎鏁� %d", RoadMap.roads[i].leftEdge.size());
- for (int j = 0; j < RoadMap.roads[i].leftEdge.size(); ++j) {
- int n = RoadMap.roads[i].leftEdge[j].points.size();
-
- DEBUG("\t褰撳墠宸﹁竟绾挎 绫诲瀷 %d 鐐规暟 %d", RoadMap.roads[i].leftEdge[j].character, n);
- for (int k = 0; k < n; ++k) {
- DEBUG("\t\t鐐瑰潗鏍� %d: %f, %f", k, RoadMap.roads[i].leftEdge[j].points[k].X, RoadMap.roads[i].leftEdge[j].points[k].Y);
- }
+ DEBUG("娣诲姞渚ф柟鍋滆溅鍦板浘");
+ if (prime.examing) {
+ return;
+ }
+ for (auto m: std::get<MAP_TYPE_PARK_EDGE>(prime.maps)) {
+ if (m.id == map.id) {
+ return;
}
-
- DEBUG("鍙宠竟绾挎鏁� %d", RoadMap.roads[i].rightEdge.size());
- for (int j = 0; j < RoadMap.roads[i].rightEdge.size(); ++j) {
- int n = RoadMap.roads[i].rightEdge[j].points.size();
-
- DEBUG("\t褰撳墠鍙宠竟绾挎 绫诲瀷 %d 鐐规暟 %d", RoadMap.roads[i].rightEdge[j].character, n);
- for (int k = 0; k < n; ++k) {
- DEBUG("\t\t鐐瑰潗鏍� %d: %f, %f", k, RoadMap.roads[i].rightEdge[j].points[k].X, RoadMap.roads[i].rightEdge[j].points[k].Y);
- }
- }
- }*/
+ }
+ if (map.points.size() == 8) {
+ PointF p8 = PointExtend(map.points[2], map.line_width, YawOf(map.points[2], map.points[3]));
+ PointF p9 = PointExtend(map.points[5], map.line_width, YawOf(map.points[5], map.points[4]));
+ map.points.push_back(p8);
+ map.points.push_back(p9);
+ }
+ std::get<MAP_TYPE_PARK_EDGE>(prime.maps).push_back(map);
}
-void SetRoadExamScheme(vector<scheme_t> &scheme)
+// 鍧￠亾璧锋鍦板浘瑕佹妸宸︿笅瑙掑拰宸︿笂瑙掕绠楀嚭鏉ワ紝鏂逛究鍦哄湴鐨勮繘鍏ュ拰閫�鍑哄垽鏂�
+void AddUphillMap(uphill_map_t &map)
{
- if (ExamStart) return;
+ DEBUG("娣诲姞涓婂潯璧锋鍦板浘");
+ if (prime.examing) {
+ return;
+ }
+ for (auto m: std::get<MAP_TYPE_UPHILL>(prime.maps)) {
+ if (m.id == map.id) {
+ return;
+ }
+ }
- vector<scheme_t>().swap(RoadMap.examScheme);
+ if (map.points.size() != 10)
+ return;
- RoadMap.examScheme.assign(scheme.begin(), scheme.end());
+ PointF bottom_left = Calc3Point(map.points[1], map.points[0], DistanceOf(map.points[1], map.points[2]), 'R');
+ PointF top_left = Calc3Point(map.points[8], map.points[9], DistanceOf(map.points[8], map.points[7]), 'L');
- DEBUG("寰楀埌璺�冮」鐩柟妗� 椤圭洰鏁伴噺 %d", RoadMap.examScheme.size());
+ map.points.push_back(bottom_left); // 浣滀负10鍙风偣
+ map.points.push_back(top_left); // 浣滀负11鍙风偣
+
+ std::get<MAP_TYPE_UPHILL>(prime.maps).push_back(map);
+}
+
+void AddTurnA90Map(turn_a90_map_t &map)
+{
+ DEBUG("娣诲姞鐩磋杞集鍦板浘");
+ if (prime.examing) {
+ return;
+ }
+ for (auto m: std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)) {
+ if (m.id == map.id) {
+ return;
+ }
+ }
+ std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps).push_back(map);
}
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
@@ -444,7 +420,7 @@
{
DEBUG("鍔犲叆杞﹁締淇℃伅 pointNum %d", pointNum);
- if (point == NULL || pointNum == 0 || ExamStart) return;
+ if (point == NULL || pointNum == 0 || prime.examing) return;
vector<int>().swap(CarModel.body);
vector<car_desc_t>().swap(CarModel.carDesc);
@@ -533,19 +509,19 @@
TerminateAreaExam();
- ExamStart = false;
+ prime.examing = false;
MA_SendExamStatus(0, 0);
return;
}
// type = TEST_TYPE_ROAD_CALIBRATE;
- /*if (AreaMapList.size() == 0 && type == TEST_TYPE_AREA) {
+ if (std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps).size() == 0 && type == TEST_TYPE_AREA) {
DEBUG("娌℃湁鍦鸿�冨湴鍥�");
err = true;
MA_SendExamStatus(0, -1);
- }*/
- if (CarModel.carDesc.size() == 0) {
+ }
+ if (prime.pModel->carDesc.size() == 0) {
DEBUG("娌℃湁杞︽ā");
err = true;
MA_SendExamStatus(0, -2);
@@ -555,40 +531,25 @@
// err = true;
// MA_SendExamStatus(0, -3);
}
- if (type != TEST_TYPE_AREA && RoadMap.roads.size() == 0) {
- DEBUG("娌℃湁璺�冨湴鍥�");
- err = true;
- MA_SendExamStatus(0, -1);
- }
-
- if ((type == TEST_TYPE_ROAD_DUMMY_LIGHT || type == TEST_TYPE_ROAD_TRUE_LIGHT) && RoadMap.examScheme.size() == 0) {
- DEBUG("娌℃湁璺�冪嚎璺柟妗�");
- err = true;
- MA_SendExamStatus(0, -3);
- }
if (!err) {
- if (!ExamStart) {
+ if (!prime.examing) {
DEBUG("鍚姩鑰冭瘯");
ExamFaultList.clear();
examFaultIndex = 0;
- ExamStart = true;
+ prime.examing = true;
ExamType = type;
reportSeatbeltEject = false;
- if (type == TEST_TYPE_ROAD_DUMMY_LIGHT) {
- exam_dummy_light = 0; //0
- RoadMap.calibrate = 0;
- }
- if (type == TEST_TYPE_ROAD_TRUE_LIGHT) {
- RoadMap.calibrate = 0;
- }
if (type == TEST_TYPE_AREA) {
+ DEBUG("鍊掑簱鍦板浘鏁扮洰锛�%d", std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps).size());
+ DEBUG("渚ф柟鍦板浘鏁扮洰锛�%d", std::get<MAP_TYPE_PARK_EDGE>(prime.maps).size());
+ DEBUG("鏇茬嚎鍦板浘鏁扮洰锛�%d", std::get<MAP_TYPE_CURVE>(prime.maps).size());
+ DEBUG("鍧￠亾鍦板浘鏁扮洰锛�%d", std::get<MAP_TYPE_UPHILL>(prime.maps).size());
+ DEBUG("鐩磋鍦板浘鏁扮洰锛�%d", std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps).size());
+
InitAreaExam();
- }
- if (type == TEST_TYPE_ROAD_CALIBRATE) {
- RoadMap.calibrate = 1;
}
}
MA_SendExamStatus(1, 0);
@@ -607,16 +568,23 @@
realtimeMotionStatus = CalcMotionState(RtkInfoList);
// 绱姞閲岀▼
UpdataOdo(realtimeMotionStatus);
+ prime.odo = ReadOdo();
// 璁＄畻褰撳墠杞﹁締鐐瑰潗鏍囧��
- prime.prev_modeling_index = prime.curr_modeling_index;
- prime.curr_modeling_index = (prime.curr_modeling_index+1) % (sizeof (realtimeBodyModeling) / sizeof (realtimeBodyModeling[0]));
- CalcBodyModeling(realtimeBodyModeling[prime.curr_modeling_index], CarModel, RtkInfoList.front());
- // 鍚慤I涓婃姤杞﹁締鐐瑰潗鏍�
- UploadModeling(realtimeMotionStatus, realtimeBodyModeling[prime.curr_modeling_index]);
- // 瑙﹀彂鑰冭瘯椤圭洰
- if (ExamStart) {
- ExecuteExam(prime);
+ if (CarModel.carDesc.size() > 0) {
+ prime.prev_modeling_index = prime.curr_modeling_index;
+ DEBUG("缂撳瓨鍧愭爣 %d", prime.prev_modeling_index);
+ prime.curr_modeling_index = (prime.curr_modeling_index + 1) %
+ (sizeof(realtimeBodyModeling) /
+ sizeof(realtimeBodyModeling[0]));
+ DEBUG("褰撳墠鍧愭爣 %d", prime.curr_modeling_index);
+ CalcBodyModeling(realtimeBodyModeling[prime.curr_modeling_index], CarModel,
+ RtkInfoList.front());
+ DEBUG("褰撳墠鍧愭爣 鐐规暟 %d", realtimeBodyModeling[prime.curr_modeling_index].points.size());
+ // 鍚慤I涓婃姤杞﹁締鐐瑰潗鏍�
+ UploadModeling(realtimeMotionStatus, realtimeBodyModeling[prime.curr_modeling_index]);
}
+ // 瑙﹀彂鑰冭瘯椤圭洰
+ ExecuteExam(prime);
}
}
@@ -637,13 +605,14 @@
// 鏌ユ壘1绉掑墠鐨勭偣,濡傛灉鎵句笉鍒板垯瑙嗕负鍋滆溅
auto prev = s.begin();
+
std::advance(prev, 1);
for (; prev != s.end(); ++prev) {
- if (prev->utc_time - curr->utc_time > 3000) {
+ if (curr->utc_time - prev->utc_time > 3000) {
return motion;
}
- if (prev->utc_time - curr->utc_time >= 1000) {
+ if (curr->utc_time - prev->utc_time >= 1000) {
break;
}
}
@@ -713,6 +682,70 @@
return motion;
}
+void UpdateSensor(int id, int value)
+{
+ switch (id) {
+ case OBD_SPEED:
+ prime.sensor.speed = value;
+ break;
+ case ENGINE_RPM:
+ prime.sensor.rpm = value;
+ break;
+ case GEAR:
+ prime.sensor.gear = value;
+ break;
+ case TURN_SIGNAL_LAMP:
+ prime.sensor.turn_signal_lamp = value;
+ break;
+ case DIPPED_BEAM_LAMP:
+ prime.sensor.dipped_beam_lamp = value;
+ break;
+ case FOG_LAMP:
+ prime.sensor.fog_lamp = value;
+ break;
+ case CLEARANCE_LAMP:
+ prime.sensor.clearance_lamp = value;
+ break;
+ case FLASH_BEAM_LAMP:
+ prime.sensor.flash_beam_lamp = value;
+ break;
+ case MAIN_BEAM_LAMP:
+ prime.sensor.main_beam_lamp = value;
+ break;
+ case SEATBELT:
+ prime.sensor.seatbelt = value;
+ break;
+ case ENGINE_START:
+ prime.sensor.engine_start = value;
+ break;
+ case BRAKE:
+ prime.sensor.brake = value;
+ break;
+ case HAND_BRAKE:
+ prime.sensor.hand_brake = value;
+ break;
+ case SECOND_BRAKE:
+ prime.sensor.second_brake = value;
+ break;
+ case DOOR:
+ prime.sensor.door = value;
+ break;
+ case SURROUND_CAR_1:
+ prime.sensor.surround_car_1 = value;
+ break;
+ case SURROUND_CAR_2:
+ prime.sensor.surround_car_2 = value;
+ break;
+ case SURROUND_CAR_3:
+ prime.sensor.surround_car_3 = value;
+ break;
+ case SURROUND_CAR_4:
+ prime.sensor.surround_car_4 = value;
+ break;
+ default:break;
+ }
+}
+
void UpdateRTKInfo(const rtk_info_t *s)
{
RtkInfoList.push_front(*s);
@@ -769,18 +802,37 @@
static double startCarMoveDistance;
static move_status_t prevMove = STOP;
+ // 鏇存柊杞﹁締浼犳劅鍣ㄦ暟鎹�
+ DEBUG("GEAR = %d", ReadCarStatus(GEAR));
+ DEBUG("RPM = %d", ReadCarStatus(ENGINE_RPM));
+ DEBUG("TURN_SIGNAL_LAMP = %d", ReadCarStatus(TURN_SIGNAL_LAMP));
+ DEBUG("DIPPED_BEAM_LAMP = %d", ReadCarStatus(DIPPED_BEAM_LAMP));
+ DEBUG("FOG_LAMP = %d", ReadCarStatus(FOG_LAMP));
+ DEBUG("CLEARANCE_LAMP = %d", ReadCarStatus(CLEARANCE_LAMP));
+ DEBUG("FLASH_BEAM_LAMP = %d", ReadCarStatus(FLASH_BEAM_LAMP));
+ DEBUG("MAIN_BEAM_LAMP = %d", ReadCarStatus(MAIN_BEAM_LAMP));
+ DEBUG("SEATBELT = %d", ReadCarStatus(SEATBELT));
+ DEBUG("BRAKE = %d", ReadCarStatus(BRAKE));
+ DEBUG("HAND_BRAKE = %d", ReadCarStatus(HAND_BRAKE));
+ DEBUG("SECOND_BRAKE = %d", ReadCarStatus(SECOND_BRAKE));
+ DEBUG("DOOR = %d", ReadCarStatus(DOOR));
+
+ if (!prime.examing) {
+ return;
+ }
+
if (prime.pMotion->move != STOP) {
- if (ReadCarStatus(SEATBELT) == EJECT_SEATBELT && !reportSeatbeltEject) {
+ if (prime.sensor.seatbelt == EJECT && !reportSeatbeltEject) {
DEBUG("涓嶇郴瀹夊叏甯�");
reportSeatbeltEject = true;
AddExamFault(ExamType == TEST_TYPE_AREA? 10101: 30101);
}
if (rec) {
- if (!handBreakActive2 && ReadOdo() - startCarMoveDistance >= examParam.start_car_limit_distance) {
+ if (!handBreakActive2 && prime.odo - startCarMoveDistance >= prime.examParam.start_car_limit_distance) {
handBreakActive2 = true;
if (ExamType == TEST_TYPE_ROAD_DUMMY_LIGHT || ExamType == TEST_TYPE_ROAD_TRUE_LIGHT) {
- if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
+ if (prime.sensor.hand_brake == ACTIVE) {
DEBUG("Handbreak active move over 10m");
// 鎵嬪埞鎷夎捣鐘舵�佷笅锛岃椹朵簡10绫充互涓婏紝涓嶅悎鏍�
AddExamFault(40205);
@@ -790,7 +842,7 @@
AddExamFault(40206);
}
}
- } else if (!handBreakActive && ReadOdo() - startCarMoveDistance >= examParam.open_door_drive_allow_distance && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
+ } else if (!handBreakActive && prime.odo - startCarMoveDistance >= prime.examParam.open_door_drive_allow_distance && prime.sensor.hand_brake == ACTIVE) {
handBreakActive = true;
if (ExamType == TEST_TYPE_AREA) {
@@ -802,7 +854,7 @@
} else if (!rec || prevMove != STOP) { // 璁板綍鍋滆溅鐐�
rec = true;
handBreakActive = handBreakActive2 = false;
- startCarMoveDistance = ReadOdo();
+ startCarMoveDistance = prime.odo;
}
prevMove = prime.pMotion->move;
@@ -812,7 +864,7 @@
static void EngineStartHold(apptimer_var_t val) {
DEBUG("鐐圭伀瓒呮椂");
- if (ReadCarStatus(ENGINE_START) == ENGINE_START_ACTIVE) {
+ if (ReadCarStatus(ENGINE_START) == ACTIVE) {
// 涓嶅強鏃舵澗寮�鍚姩寮�鍏筹紝鎵�10鍒�
if (ExamType == TEST_TYPE_AREA) {
AddExamFault(10201);
@@ -826,16 +878,10 @@
{
struct ExamFault fault;
- if (!ExamStart)
- return;
-
fault.sn = examFaultIndex++;
- strcpy(fault.utc, StringUtil::FormatUTCTime(AppTimer_GetGmtTickCount()).c_str());
-
-// if (ExamType != TEST_TYPE_AREA) {
-// wrong += 1000;
-// }
+ //strcpy(fault.utc, StringUtil::FormatUTCTime(AppTimer_GetGmtTickCount()).c_str());
+ strcpy(fault.utc, "20230413172712.20");
fault.wrong_id = wrong;
@@ -921,10 +967,3 @@
}
}
-
-void SensorXChanged(uint16_t id, int value)
-{
-// handlePrepare(id, value);
-// handleLigthExam(id, value);
-}
-
--
Gitblit v1.8.0