yy1717
2024-02-28 27fc91fbe8f88b6885356e68828cfe1ce1db7601
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
//
// Created by YY on 2019/10/21.
//
 
#ifndef RTKDRIVERTEST_DRIVER_TEST_H
#define RTKDRIVERTEST_DRIVER_TEST_H
 
#include <vector>
#include <string>
#include <list>
#include <map>
#include <tuple>
#include <atomic>
 
#include "test_common/Geometry.h"
#include "rtk_module/rtk.h"
#include "map.h"
 
#define EXAM_AREA_NONE          0
#define EXAM_AREA_START         1
#define EXAM_AREA_RUN           2
#define EXAM_AREA_END           3
 
#define TIRE_OUTSIDE    0
#define TIRE_INSIDE     1
#define AXIAL_FRONT     0
#define AXIAL_REAR      1
 
typedef enum {
    MAP_TYPE_NONE,
    MAP_TYPE_PARK_BUTTOM,
    MAP_TYPE_UPHILL,
    MAP_TYPE_PARK_EDGE,
    MAP_TYPE_CURVE,
    MAP_TYPE_RIGHT_CORNER
} map_type_t;
 
typedef struct {
    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;
    std::vector<int> body;
//    int pointNum;
    double antPitch;                // 前后天线因安装位置之固有pitch角
    double antHeight;               // 车顶天线高程
    double groundHeight;            // 大地高程
    double yaw;
    double pitch;
    std::vector<car_desc_t> carDesc;
//    struct car_desc_ *carDesc;      // 经现场测量点计算后的值
//    PointF *carXY;                  // 计算后的车身各点坐标
} car_model_t;
 
typedef struct {
    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 idx;        // 对应科目地图集合中的序号
    int stage;      // 对于倒车入库等项目会有多个操作阶段
} map_t;
 
typedef struct {
    std::atomic_int speed;
    std::atomic_int rpm;
    std::atomic_int gear;
    std::atomic_int turn_signal_lamp;
    std::atomic_int dipped_beam_lamp;
    std::atomic_int fog_lamp;
    std::atomic_int clearance_lamp;
    std::atomic_int flash_beam_lamp;
    std::atomic_int main_beam_lamp;
    std::atomic_int seatbelt;
    std::atomic_int engine_start;
    std::atomic_int brake;
    std::atomic_int hand_brake;
    std::atomic_int second_brake;
    std::atomic_int door;
    std::atomic_int surround_car_1;
    std::atomic_int surround_car_2;
    std::atomic_int surround_car_3;
    std::atomic_int surround_car_4;
} sensor_t;
 
typedef struct {
    int hold_start_key_limit_time;      // 点火保持,毫秒
 
    int curve_pause_criteria;           // 停车断定,豪秒
 
    int park_bottom_pause_criteria;     // 豪秒
    int park_bottom_limit_time;         // 秒
 
    int park_edge_pause_criteria;       // MSec
    int park_edge_limit_time;           // Sec
 
    int turn_a90_pause_criteria;        // MSec
 
    double ramp_stoppoint_red_distance; // 米
    double ramp_edge_yellow_distance;
    double ramp_edge_red_distance;
    double ramp_slide_yellow_distance;
    double ramp_slide_red_distance;
    int ramp_start_car_limit_time;      // Sec
 
    double road_slide_yellow_distance;
    double road_slide_red_distance;
    int road_total_distance;
    int road_max_speed;                     // Km per hour
    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 road_pause_criteria;                // MSec
    int continuous_change_lane_min_time;    // Sec
    int crash_dotted_line_cumulative_time;  // Sec
    int turn_signal_min_advance;            // Sec
    int start_car_max_rpm;
    int start_car_limit_distance;
    double open_door_drive_allow_distance;
 
    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;
 
    std::string start_car_begin_tts;
    std::string start_car_end_tts;
 
    int change_lane_limit_distance;
    std::string change_lane_begin_tts;
    std::string change_lane_end_tts;
 
    int shift_limit_distance;
    int shift_hold_time;                    // Sec
    std::string shift_begin_tts;
    std::string shift_end_tts;
    std::string shift_up_tts;
    std::string shift_down_tts;
 
    std::string straight_begin_tts;
    std::string straight_end_tts;
    int straight_limit_distance;
    double straight_max_offset;
 
    int overtake_limit_distance;
    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;
    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;
 
    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;
 
typedef struct {
    bool examing = false;
    int curr_modeling_index = -1;
    int prev_modeling_index = -1;
    int arriving_map = -1;
    std::tuple<int, std::vector<park_button_map_t>, std::vector<uphill_map_t>, std::vector<park_edge_map_t>, std::vector<curve_map_t>, std::vector<turn_a90_map_t>> maps;
    car_model_t *pModel;
    modeling_t *pModeling;
    motion_t *pMotion;
    map_t examing_area;
    sensor_t sensor;
    exam_param_t examParam;
    double odo;             // 用RTK定位计算的行驶里程
} prime_t;
 
struct ExamFault {
    int sn;
    char utc[32];
    int wrong_id;
};
 
prime_t& GetPrime(void);
void DriverTestInit(void);
void ReadDriverExamPrimer(void);
 
void ClearAreaMap(void);
 
void AddCurveMap(curve_map_t &map);
void AddParkButtonMap(park_button_map_t &map);
void AddParkEdgeMap(park_edge_map_t &map);
void AddUphillMap(uphill_map_t &map);
void AddTurnA90Map(turn_a90_map_t &map);
 
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
                        int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
                        int *body, int bodyNum, double (*point)[2], int pointNum, double antPitch, double antHeight, double groundHeight);
 
void StartDriverExam(int start, int type);
 
void UpdateSensor(int id, int value);
void UpdateRTKInfo(const rtk_info_t *s);
void AddExamFault(int wrong);
 
 
void SystemShutdown(int event, int timeout);
 
int CorrectPauseCriteria(int src);
 
#endif //RTKDRIVERTEST_DRIVER_TEST_H