实现rtk平台基础配置

yy1717

2020-01-09 74bb2be0e23e9f2290ff8ecfb6506acf8a070339

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
//
// Created by YY on 2019/11/4.
//
 
#include "driving_curve.h"
#include "../driver_test.h"
#include "../common/apptimer.h"
 
#include <vector>
 
using namespace std;
 
enum {
    DRIVING_ON_CURVE
};
 
const uint32_t STOP_CAR_TIME = D_SEC(2);
 
static bool carStopEvent = false;
static bool DCTesting = false;
static int currTarget;
static uint32_t stopTimepoint = 0;
 
static bool CrashRedLine(const Polygon *left, const Polygon *right, const car_model_cache_t *car);
static bool ExitTestArea(const Polygon *left, const Polygon *right, const car_model_cache_t *car);
 
void StartDrivingCurve(void)
{
    DCTesting = true;
    carStopEvent = false;
    currTarget = DRIVING_ON_CURVE;
}
 
void StopDrivingCurve(void)
{
    DCTesting = false;
}
 
int TestDrivingCurve(vector<int>&err, const Polygon *map, const Polygon *map2, const car_model_cache_t *car, double speed, int run_status)
{
    int status = 0;
 
    if (!DCTesting)
        return -2;
 
    if (currTarget == DRIVING_ON_CURVE) {
        if (CrashRedLine(map, map2, car)) {
            // 车轮压线
            err.push_back(27);
            status = -1;
        }
 
        if (ExitTestArea(map, map2, car)) {
            // 测试完成
            status = 1;
        }
 
        if (run_status != 0) {
            if (carStopEvent && AppTimer_GetTickCount() - stopTimepoint > D_SEC(2)) {
                // 中途停车
                err.push_back(28);
            }
            carStopEvent = false;
        } else {
            carStopEvent = true;
            stopTimepoint = AppTimer_GetTickCount();
        }
    }
 
    if (status != 0) {
        StopDrivingCurve();
    }
 
    return status;
}
 
// 车轮是否压边线
static bool CrashRedLine(const Polygon *left, const Polygon *right, const car_model_cache_t *car)
{
    bool ret = false;
 
    car_model_cache_t *prev_car = GetCarModelCache(1);
    if (prev_car == NULL)
        return false;
 
    // 按4个轮子的外侧计算
    Line front_left_tire_track, front_right_tire_track, rear_left_tire_track, rear_right_tire_track;
    MakeLine(&front_left_tire_track,  &car->points[car->desc->front_left_tire[TIRE_OUTSIDE]],  &prev_car->points[car->desc->front_left_tire[TIRE_OUTSIDE]]);
    MakeLine(&front_right_tire_track,  &car->points[car->desc->front_right_tire[TIRE_OUTSIDE]],  &prev_car->points[car->desc->front_right_tire[TIRE_OUTSIDE]]);
    MakeLine(&rear_left_tire_track,  &car->points[car->desc->rear_left_tire[TIRE_OUTSIDE]],  &prev_car->points[car->desc->rear_left_tire[TIRE_OUTSIDE]]);
    MakeLine(&rear_right_tire_track,  &car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]],  &prev_car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]]);
 
    Line line;
 
    line.X1 = left->point[0].X;
    line.Y1 = left->point[0].Y;
    for (int i = 1; i < left->num; ++i) {
        line.X2 = left->point[i].X;
        line.Y2 = left->point[i].Y;
 
        if (IntersectionOf(line, front_left_tire_track) == GM_Intersection ||
                IntersectionOf(line, front_right_tire_track) == GM_Intersection ||
                IntersectionOf(line, rear_left_tire_track) == GM_Intersection ||
                IntersectionOf(line, rear_right_tire_track) == GM_Intersection) {
            ret = true;
            break;
        }
        line.X1 = line.X2;
        line.Y1 = line.Y2;
    }
 
    line.X1 = right->point[0].X;
    line.Y1 = right->point[0].Y;
    for (int i = 1; !ret && i < right->num; ++i) {
        line.X2 = right->point[i].X;
        line.Y2 = right->point[i].Y;
 
        if (IntersectionOf(line, front_left_tire_track) == GM_Intersection ||
            IntersectionOf(line, front_right_tire_track) == GM_Intersection ||
            IntersectionOf(line, rear_left_tire_track) == GM_Intersection ||
            IntersectionOf(line, rear_right_tire_track) == GM_Intersection) {
            ret = true;
            break;
        }
        line.X1 = line.X2;
        line.Y1 = line.Y2;
    }
 
    return ret;
}
 
// 整个车辆都要驶离该测试区域
static bool ExitTestArea(const Polygon *left, const Polygon *right, const car_model_cache_t *car)
{
    for (int i = 0; i < car->point_num; ++i) {
        if (IntersectionOfLine(left->point[0], right->point[0], car->points[i]) != 1)
            return false;
    }
    return true;
}