坐标

fctom1215

2020-08-28 bf8e1f4ef4961b32dfa73a0b898f82c20721bf09

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
//
// Created by YY on 2020/8/21.
//
 
#include "overtake.h"
#include "../native-lib.h"
#include "../jni_log.h"
#include "../test_common/odo_graph.h"
#include "../driver_test.h"
 
#define DEBUG(fmt, args...)     LOGD("<road_exam overtake> <%s>: " fmt, __func__, ##args)
 
static double maxMoveDistance;
static int setup;
static int originalLane;
 
static void TtsBack(int seq)
{
    maxMoveDistance = ReadOdo();
    setup = 1;
}
 
void StartOvertakeExam(int ori_lane)
{
    DEBUG("超越前方车辆");
 
    PlayTTS("请超越前方车辆", TtsBack);
 
    setup = 0;
    originalLane = ori_lane;
}
 
bool TestOvertake(int currLane, const struct RtkTime *rtkTime)
{
    if (setup == 0) {
        return true;
    }
    if (setup == 1) {
        if (originalLane == currLane) {
 
        } else if (currLane + 1 == originalLane) {
            setup = 2;
            originalLane = currLane;
        } else {
            DEBUG("右侧超车");
            AddExamFault(3, rtkTime);
            return false;
        }
    } else if (setup == 2) {
        if (originalLane == currLane) {
 
        } else if (currLane == originalLane + 1) {
            PlayTTS("完成超车", NULL);
            return false;
        } else {
            DEBUG("超车违规变道");
            AddExamFault(3, rtkTime);
            return false;
        }
    }
 
    if (ReadOdo() - maxMoveDistance > 150) {
        // 超车未完成
        DEBUG("超车固定距离内未完成");
        AddExamFault(3, rtkTime);
        return false;
    }
 
    return true;
}