进出科目二区域调整

yy1717

2020-04-02 24428a1b69afcf113883e9cede1159443201a1b8

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
//
// Created by YY on 2020/3/23.
//
 
#include "drive_straight.h"
#include "../driver_test.h"
#include "../native-lib.h"
#include "../jni_log.h"
#include "road_exam.h"
#include "../master/comm_if.h"
#include <cmath>
 
#define DEBUG(fmt, args...)     LOGD("<drive_straight> <%s>: " fmt, __func__, ##args)
 
static const double CHECK_STAGE_DISTANCE = 100.0;
static const double MAX_OFFSET_DISTANCE = 0.3;
 
static bool crossStartLine;
static bool reportOffsetOver;
static double edgeDistance;
 
void StartDriveStraightExam(int index, LIST_ROAD_MAP &RoadMapList) {
    if (index == -1)
        return;
 
    DEBUG("进入路考直线行驶地图 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type);
 
    if (!RoadMapList[index].tts.empty()) {
        DEBUG("播放TTS");
        PlayTTS(RoadMapList[index].tts.c_str());
    } else {
        DEBUG("没有TTS");
    }
 
    crossStartLine = false;
    reportOffsetOver = false;
}
 
int ExecuteDriveStraightExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
                             LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) {
    Line road_edge;
 
    static PointF startPoint;
 
    MakeLine(&road_edge, &RoadMapList[index].roadEdgeLine[0].point[0], &RoadMapList[index].roadEdgeLine[0].point[1]);
 
    {
        double l1 = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], road_edge);
 
        double l2 = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], road_edge);
 
        MA_SendDistance(-1, (l1+l2)/2.0);
    }
 
    if (!crossStartLine) {
        PointF p1, p2;
 
        p1.X = RoadMapList[index].stopLine.X1;
        p1.Y = RoadMapList[index].stopLine.Y1;
        p2.X = RoadMapList[index].stopLine.X2;
        p2.Y = RoadMapList[index].stopLine.Y2;
 
        if (IntersectionOfLine(p1, p2, car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1) {
            DEBUG("跨过标记线，开始直线测试");
            crossStartLine = true;
            startPoint = car->basePoint;
            edgeDistance = DistanceOf(car->basePoint, road_edge);
 
            DEBUG("当前基准路边间距 %f", edgeDistance);
        }
    } else {
        double distanceToStart = DistanceOf(car->basePoint, startPoint);
 
        DEBUG("路边间距 %f --- %f", DistanceOf(car->basePoint, road_edge), edgeDistance);
 
        if (!reportOffsetOver && fabs(DistanceOf(car->basePoint, road_edge) - edgeDistance) > MAX_OFFSET_DISTANCE) {
            DEBUG("直线偏移大于30厘米");
            // 偏移大于30厘米，不合格
            AddExamFault(30, rtkTime);
            reportOffsetOver = true;
 
            //////////////////////////////////////////////
            startPoint = car->basePoint;
            edgeDistance = DistanceOf(car->basePoint, road_edge);
            reportOffsetOver = false;
        }
 
        if (distanceToStart > CHECK_STAGE_DISTANCE) {
            DEBUG("复位边距偏移量");
            startPoint = car->basePoint;
            edgeDistance = DistanceOf(car->basePoint, road_edge);
            reportOffsetOver = false;
        }
    }
 
    if (ExitSonArea(index, RoadMapList, car)) {
        DEBUG("离开直线行驶区域");
        return -1;
    }
 
    return index;
}