坐标

yy1717

2020-08-19 712eb83d17ffc6e259af6c53ba63bbd58cd1242b

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
//
// Created by YY on 2020/8/11.
//
 
#include "odo_graph.h"
#include "../test_items2/road_exam.h"
#include "../common/apptimer.h"
#include "../utils/xconvert.h"
 
static double odoGraph;
static struct drive_timer odoTimer;
static double odoPrevSpeed;
static int odoCnt;
 
void ResetOdo(void)
{
    odoCnt = 0;
    odoGraph = 0;
}
 
double ReadOdo(void)
{
    return odoGraph;
}
 
void UpdataOdo(double speed, int moveDirect, const struct RtkTime *rtkTime) {
    // 行驶距离，不包含倒车
    if (odoCnt == 0 && moveDirect == 1) {
        odoPrevSpeed = speed;
        odoCnt = 1;
        Rtk2DriveTimer(odoTimer, rtkTime);
    } else if (odoCnt == 1) {
        if (moveDirect == 1) {
            uint32_t tm = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss * 10,
                                      odoTimer.hour, odoTimer.min, odoTimer.sec,
                                      odoTimer.msec * 10);
            if (tm >= D_SEC(1)) {
                odoGraph += ((double) tm) * (odoPrevSpeed + speed) / 2.0 / 1000.0;
                Rtk2DriveTimer(odoTimer, rtkTime);
                odoPrevSpeed = speed;
            }
        } else {
            odoCnt = 0;
        }
    }
}