科目二修改

yy1717

2023-03-31 4bd08f0355b6b2cf3c027202d5ad301b4e182953

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
//
// Created by YY on 2020/8/11.
//
 
#include "odo_graph.h"
#include "../common/apptimer.h"
#include "../utils/xconvert.h"
#include "../driver_test.h"
 
static double odoGraph;
static int64_t prevTimestamp;
static double prevSpeed;
static bool isstop;
 
void ResetOdo(void)
{
    isstop = true;
    odoGraph = 0;
}
 
double ReadOdo(void)
{
    return odoGraph;
}
 
void UpdataOdo(motion_t &motion) {
    // 行驶距离，含倒车
    if (isstop && motion.move != STOP) {
        prevSpeed = motion.speed;
        isstop = false;
        prevTimestamp = motion.timestamp;
    } else if (!isstop) {
        if (motion.move != STOP) {
            uint32_t elapsed = motion.timestamp - prevTimestamp;
            if (elapsed >= D_SEC(5)) {      // 中途长时间未定位，重新开始测量
                prevSpeed = motion.speed;
                prevTimestamp = motion.timestamp;
                return;
            }
            if (elapsed >= D_SEC(1)) {
                odoGraph += ((double) elapsed) * (prevSpeed + motion.speed) / 2.0 / 1000.0;
                prevTimestamp = motion.timestamp;
                prevSpeed = motion.speed;
            }
        } else {
            isstop = true;
        }
    }
}