增加单片机信息传递。

fctom1215

2020-02-14 7c5b25c8617807635e9f272ebe9e98304d07cf2f

增加单片机信息传递。

 lib/src/main/cpp/CMakeLists.txt

@@ -33,6 +33,7 @@
        test_items/stop_and_start.cpp
        test_items/driving_curve.cpp
        test_items/turn_a90.cpp
        test_items/comm_test.cpp
        rtk_module/rtk.cpp
        rtk_module/virtual_rtk.cpp
        master/comm_if.cpp

 lib/src/main/cpp/driver_test.cpp

@@ -80,6 +80,13 @@
static car_model *CarModel = NULL;
static car_model *CarModelPrev = NULL;

static struct sensor_cfg {
    int gpioId;
    int funId;
    int validLvl;
} SensorConfig[32];
static int SensorNum = 0;

#define MOV_AVG_SIZE                1
#define RTK_BUFFER_SIZE            100
#define CAR_MODEL_CACHE_SIZE      10
@@ -104,6 +111,9 @@
    MapNum = 0;
    CarModel = NULL;
    CarModelPrev = NULL;

    SensorNum = 0;
    memset(SensorConfig, 0, sizeof(SensorConfig));

    RtkBuffer = (rtk_info *) malloc(RTK_BUFFER_SIZE * sizeof(rtk_info));
    RtkBufferNum = RtkBufferIn = 0;
@@ -287,9 +297,26 @@
    DEBUG("SetCarMeasurePoint Calc Over");
}

void SetSensorCfg(int (*sensor)[2], int sensorNum)
void SetSensorCfg(int (*sensor)[3], int sensorNum)
{
    DEBUG("SetSensorCfg sensorNum %d", sensorNum);

    SensorNum = sensorNum;
    for (int i = 0; i < sensorNum; ++i) {
        SensorConfig[i].gpioId = sensor[i][0];
        SensorConfig[i].funId = sensor[i][1];
        SensorConfig[i].validLvl = sensor[i][2];
    }
}

void GetFuncGpio(int func, int &gpio, int &lvl)
{
    for (int i = 0; i < SensorNum; ++i) {
        if (SensorConfig[i].funId == func) {
            gpio = SensorConfig[i].gpioId;
            lvl = SensorConfig[i].validLvl;
        }
    }
}

void StartDriverExam(int start)
@@ -345,7 +372,7 @@
                RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);

        brief.qf = RtkBuffer[index].qf;
        brief.map_id = 865;//GetMapId(CurrExamMapIndex, MapList, MapNum);
        brief.map_id = 866;//GetMapId(CurrExamMapIndex, MapList, MapNum);
        brief.move = move;
        brief.speed = speed * 3.6;
        brief.heading = RtkBuffer[index].heading;
@@ -469,15 +496,14 @@
}

/*************************************************
 * 2次采样相差的时间
 * 2次采样相差的时间, a 最近的，b 先前的
 * @param a
 * @param b
 * @return ms
 */
static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b)
{
    return ABS((long)(TimeMakeComposite(2000 + a->YY, a->MM, a->DD, a->hh, a->mm, a->ss) * 1000 + a->dss*10) -
               (long)(TimeMakeComposite(2000 + b->YY, b->MM, b->DD, b->hh, b->mm, b->ss) * 1000 + b->dss*10));
    return TimeGetDiff(a->hh, a->mm, a->ss, a->dss*10, b->hh, b->mm, b->ss, b->dss*10);
}

static bool UpdateCarCoord(double &spd, int &mov, int &idx)
@@ -686,7 +712,7 @@
{
    // 车的最前点是否进入地图
    for (int i = 0; i < mapNum && car != NULL; ++i) {
        if (mapList[i].type == MAP_TYPE_STOP_START) {
        /*if (mapList[i].type == MAP_TYPE_STOP_START) {
            // 构造虚拟的左上角点
            double x9, y9, xo, yo;

@@ -721,7 +747,7 @@
            free(map.point);

            if (enter) return i;
        } /*else if (mapList[i].type == MAP_TYPE_CURVE) {
        } else if (mapList[i].type == MAP_TYPE_CURVE) {

        } else if (mapList[i].type == MAP_TYPE_PARK_BUTTOM || mapList[i].type == MAP_TYPE_PART_EDGE) {
            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
@@ -734,7 +760,7 @@
                    DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
                    return i;
            }
        } else if (mapList[i].type == MAP_TYPE_TURN_90) {
        } else */if (mapList[i].type == MAP_TYPE_TURN_90) {
            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
                Line enterLine1;

@@ -743,7 +769,7 @@
                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
                    return i;
            }
        }*/
        }
    }
    return -1;
}

 lib/src/main/cpp/driver_test.h

@@ -81,7 +81,8 @@
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
                        int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
                        int *body, int bodyNum, double (*point)[2], int pointNum);
void SetSensorCfg(int (*sensor)[2], int sensorNum);
void SetSensorCfg(int (*sensor)[3], int sensorNum);
void GetFuncGpio(int func, int &gpio, int &lvl);
void StartDriverExam(int start);
void UpdateRTKInfo(const rtk_info *s);
void AddExamFault(int wrong, const struct RtkTime *rtkTime);

 lib/src/main/cpp/master/comm_if.cpp

@@ -36,7 +36,7 @@
#define ID_MS_MAP               0x8006
#define ID_SM_READ_CAR          0x0007
#define ID_MS_CAR               0x8007
#define ID_SM_READ_SENSOR_CFG   0x0008
#define ID_SM_MCU_BRIEF   0x0008
#define ID_MS_SENSOR_CFG        0x8008
#define ID_MS_START_EXAM        0x8009
#define ID_SM_EXAM_STATUS       0x0009
@@ -135,7 +135,7 @@

void MA_ReadSensor(void)
{
    SendMsgToMainProc(ID_SM_READ_SENSOR_CFG, NULL);

}

void MA_SendExamStatus(int start, int errorCode)
@@ -153,6 +153,31 @@
    writer.EndObject();

    SendMsgToMainProc(ID_SM_EXAM_STATUS, sb.GetString());
}

void MA_SendMcuBrief(const struct mcuBrief *brief)
{
    StringBuffer sb;
    Writer<StringBuffer> writer(sb);

    writer.StartObject();

    writer.Key("version");
    writer.String(brief->version);
    writer.Key("selftest");
    writer.Int(brief->selftest);
    writer.Key("gpio");
    writer.Int(brief->gpio);
    writer.Key("speed");
    writer.Int(brief->speed);
    writer.Key("engine");
    writer.Int(brief->engine);
    writer.Key("sn");
    writer.String(brief->sn);

    writer.EndObject();

    SendMsgToMainProc(ID_SM_MCU_BRIEF, sb.GetString());
}

void MA_SendGpsBrief(const struct gpsBrief *brief)
@@ -615,7 +640,7 @@
                if (a.IsArray() && a.Size() > 0) {
                    int n = a.Size();
                    int i = 0;
                    int (*sensor)[2] = (int (*)[2]) new int[n][2];
                    int (*sensor)[3] = (int (*)[3]) new int[n][3];

                    for (Value::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr) {
                        // a gpio mapping
@@ -627,6 +652,10 @@
                            const Value &s = (*itr)["func_id"];
                            sensor[i][1] = s.GetInt();
                        }
                        if (itr->HasMember("level")) {
                            const Value &s = (*itr)["level"];
                            sensor[i][2] = s.GetInt();
                        }
                        ++i;
                    }
                    SetSensorCfg(sensor, n);

 lib/src/main/cpp/master/comm_if.h

@@ -8,6 +8,15 @@
#include <cstdint>
#include "../driver_test.h"

struct mcuBrief {
    char version[70];
    int selftest;
    int gpio;
    int speed;
    int engine;
    char sn[20];
};

struct gpsBrief {
    char utc[32];
    int sat_num;
@@ -61,6 +70,7 @@
void MA_ReadCar(void);
void MA_ReadSensor(void);
void MA_SendExamStatus(int start, int errorCode);
void MA_SendMcuBrief(const struct mcuBrief *brief);
void MA_SendGpsBrief(const struct gpsBrief *brief);
void MA_SendRtkBrief(const struct rtkBrief *brief);
void MA_SendCarPosition(const struct carBrief *brief);

 lib/src/main/cpp/mcu/mcu_if.cpp

@@ -11,6 +11,7 @@
#include "../defs.h"
#include "../jni_log.h"
#include "../common/serial_port.h"
#include "../rtk_platform/platform.h"

#define DEBUG(fmt, args...)     LOGD("<mcu_if> <%s>: " fmt, __func__, ##args)

@@ -62,6 +63,7 @@
static int dfuTryCnt = 0;
const int DFU_MAX_TRY = 3;
const int DFU_FILE_BLOCK_SIZE = 896;


static void *UartThread1(void *p);
static void ParseMcuTimeout(union sigval sig);
@@ -357,12 +359,9 @@
            break;
        case ID_MC_CAR_INFO: {
            DEBUG("ID_MC_CAR_INFO %d", lenth);
            char ver[64] = {0};

            memcpy(ver, data, 32);

            DEBUG("ID_MC_CAR_INFO %s", ver);

            if (lenth > 0)
                PlatformStatusChanged(MCU_UPDATE_EVT, data, lenth);
            break;
        }
        case ID_MC_RTK_DATA:

 lib/src/main/cpp/rtk_platform/platform.cpp

@@ -390,6 +390,30 @@
        MA_SendRtkBrief(&brief);
        UpdateRTKInfo(rtk);
    }
    if (events & MCU_UPDATE_EVT) {
        DEBUG("MCU_UPDATE_EVT length %d", length);
        // 0-31 version
        // 32-33 selftest
        // 34-35 gpio
        // 36-37 speed
        // 38-39 engine
        // 40-55 sn
        struct mcuBrief brief;

        memset(&brief, 0, sizeof(brief));

        int x = 0;
        while(data[x] != 0 && x < 32) x++;

        ConvertHex2String(brief.version, data, x);
        brief.selftest = BUILD_UINT16(data[33], data[32]);
        brief.gpio = BUILD_UINT16(data[35], data[34]);
        brief.speed = BUILD_UINT16(data[37], data[36]);
        brief.engine = BUILD_UINT16(data[39], data[38]);
        memcpy(brief.sn, data+40, 16);

        MA_SendMcuBrief(&brief);
    }
}

static void *PlatformDataListenThread(void *p) {

 lib/src/main/cpp/rtk_platform/platform.h

@@ -13,6 +13,7 @@
#define PLATFORM_LOGIN_EVT                           0x0008
#define RTK_UPDATE_EVT                               0x0010
#define GPS_UPDATE_EVT                              0x0020
#define MCU_UPDATE_EVT                              0x0040

typedef struct {
    char domain_name[32];

 lib/src/main/cpp/test_items/comm_test.cpp

New file
@@ -0,0 +1,63 @@
//
// Created by fctom on 2020/2/13.
//

#include "comm_test.h"
#include "../driver_test.h"
#include "../defs.h"

static bool seatbeltInsert;
static bool engineStart;

const int ENGINE_MIN_ROTATE = 200;

static uint16_t gpioStore = 0;
static uint16_t engineStore = 0;

static void SensorChange(int index, bool value);

void CommTestStart(void)
{
    gpioStore = engineStore = 0;
}

void UpdateSensor(uint16_t gpio, uint16_t speed, uint16_t engine)
{
    int idx, lvl;

    uint16_t chg = gpioStore^gpio;

    if (chg == 0)
        return;

    for (int i = 0; i < 16; ++i) {
        if (chg & BV(i)) {
            SensorChange(i, (bool)(gpio & BV(i)));
        }
    }

    gpioStore = gpio;



    // 安全带

    // 挡位

    // 启动指示

    // 熄火监控
}

static void SensorChange(int index, bool value)
{
    int func;

    switch (func) {
        case SENSOR_SEATBELT:
            break;
        case SENSOR_TURNRIGHT:
            break;

    }
}

 lib/src/main/cpp/test_items/comm_test.h

New file
@@ -0,0 +1,14 @@
//
// Created by fctom on 2020/2/13.
//

#ifndef MYAPPLICATION2_COMM_TEST_H
#define MYAPPLICATION2_COMM_TEST_H

#define SENSOR_SEATBELT         0
#define SENSOR_TURNRIGHT         1
#define SENSOR_TURNLEFT         2
#define SENSOR_HANDBREAK         3


#endif //MYAPPLICATION2_COMM_TEST_H

 lib/src/main/cpp/test_items/park_edge.cpp

@@ -17,11 +17,11 @@

using namespace std;

const int PARK_TIMEOUT = 90;
const int PARK_TIMEOUT = D_SEC(90);
const uint32_t STOP_CAR_TIME = D_SEC(2);

static bool reportExamTimeout;
static uint64_t stopTimepoint = 0;
static uint32_t stopTimepoint = 0;
static bool occurCrashRedLine1, occurCrashRedLine2;
static int prevMoveStatus, storeMoveStatusBeforeStop;
static int parkStatus;
@@ -45,7 +45,7 @@
    occurMoveBack = false;
    if (moveStatus == -1) {
        occurMoveBack = true;
        moveBackTimePoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
        moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
    }

    DEBUG("进入侧方停车场地");
@@ -87,7 +87,7 @@
    }

    if (occurMoveBack) {
        uint32_t tp = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
        uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

        if (!reportExamTimeout && tp - moveBackTimePoint >= PARK_TIMEOUT) {
            // 超时90秒，不合格
@@ -100,15 +100,13 @@
    if (moveStatus != prevMoveStatus) {
        if (moveStatus == 0) {
            parkStatus = 0;
            stopTimepoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
            stopTimepoint = stopTimepoint * 1000 + rtkTime->mss*10;
            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
            storeMoveStatusBeforeStop = prevMoveStatus;

            DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
        } else {
            DEBUG("继续行驶 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
            uint64_t tp = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
            tp = tp * 1000 + rtkTime->mss * 10;
            uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

            DEBUG("停车时间 %ld", tp - stopTimepoint);

@@ -144,13 +142,13 @@
            if (moveStatus == -1 && !occurMoveBack) {
                DEBUG("开始倒车");
                occurMoveBack = true;
                moveBackTimePoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
                moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
            }
        }

        prevMoveStatus = moveStatus;
    } else if (moveStatus == 0 && parkStatus == 0) {
        uint64_t tp = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
        uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
        tp = tp * 1000 + rtkTime->mss * 10;

        if (tp - stopTimepoint >= STOP_CAR_TIME) {

 lib/src/main/cpp/test_items/stop_and_start.cpp

@@ -16,12 +16,6 @@

using namespace std;

enum
{
    STOP_CAR,
    START_CAR
};

const double STOP_DISTANCE_THRESHOLD_RED = 0.5;
const double EDGE_DISTANCE_THRESHOLD_RED = 0.5;
const double EDGE_DISTANCE_THRESHOLD_YELLOW = 0.3;
@@ -38,7 +32,7 @@
static PointF stopPoint;

static int prevMoveDirect;
static uint64_t stopTimepoint = 0;
static uint32_t stopTimepoint = 0;
static bool stopCar = false;
static uint32_t stopCarTime;
static bool occurCrashRedLine = false;
@@ -61,9 +55,7 @@
    prevMoveDirect = moveDirect;

    if (moveDirect == 0) {
        stopTimepoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh,
                                          rtkTime->mm, rtkTime->ss);
        stopTimepoint = stopTimepoint * 1000 + rtkTime->mss * 10;
        stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
    }

    occurCrashRedLine = false;
@@ -96,18 +88,16 @@

    if (prevMoveDirect != moveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
            stopTimepoint = stopTimepoint * 1000 + rtkTime->mss*10;
            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
        }
        prevMoveDirect = moveDirect;
    } else if (moveDirect == 0) {
        uint64_t tp = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
        tp = tp * 1000 + rtkTime->mss * 10;
        uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

        if (tp - stopTimepoint >= STOP_CAR_TIME && !stopCar) {
            // 这里判断停车状态
            stopCar = true;
            stopCarTime = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
            stopCarTime = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

            stopPoint = car->carXY[car->body[0]];

@@ -161,9 +151,10 @@
            }
        }


        if (!reportStartTimeout && (IntersectionOfLine(map->point[4], stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) != -1 ||
                DistanceOf(stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) < 0.1)) {
            if (TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss) - stopCarTime > CAR_START_TIMEOUT) {
            if (TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10) - stopCarTime > CAR_START_TIMEOUT) {
                // 起步时间超过30秒，不合格
                AddExamFault(15, rtkTime);
                DEBUG("起步时间超过30秒");
@@ -217,6 +208,9 @@
    int rel1 = IntersectionOfLine(map->point[4], map->point[3], car->carXY[car->body[0]]);
    int rel2 = IntersectionOfLine(map->point[5], map->point[6], car->carXY[car->body[0]]);

    DEBUG("%d %d %f, %f", car->body[0], car->axial[AXIAL_FRONT], car->carXY[car->body[0]].X, car->carXY[car->body[0]].Y);
    DEBUG("rel1 %d rel2 %d", rel1, rel2);

    if (rel1 == 1) {
        Line line1;

@@ -231,6 +225,8 @@
        dis = DistanceOf(car->carXY[car->body[0]], line2);
    }

    DEBUG("DistanceOfHead2Stopline dis %f", dis);

    return dis;
}


 lib/src/main/cpp/test_items/turn_a90.cpp

@@ -21,9 +21,9 @@

static bool TA90Testing;

static int azimuth;
static int enterAreaHeading;
static bool turnLeftFinished;
static uint64_t stopTimepoint = 0;
static uint32_t stopTimepoint = 0;

static bool reportStopCarTimeout;
static int prevMoveDirect;
@@ -34,11 +34,11 @@

void StartTurnA90(int moveDirect, double heading, const struct RtkTime *rtkTime)
{
    azimuth = (int) heading;
    DEBUG("进入直角转弯场地");
    enterAreaHeading = (int) heading;
    prevMoveDirect = moveDirect;
    if (moveDirect == 0) {
        stopTimepoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
        stopTimepoint = stopTimepoint * 1000 + rtkTime->mss*10;
        stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
    }
    reportStopCarTimeout = false;
    crashRedLine = false;
@@ -66,8 +66,7 @@

    if (moveDirect != prevMoveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
            stopTimepoint = stopTimepoint * 1000 + rtkTime->mss*10;
            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
            reportStopCarTimeout = false;

            DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
@@ -76,8 +75,7 @@
        }
        prevMoveDirect = moveDirect;
    } else if (moveDirect == 0) {
        uint64_t tp = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
        tp = tp * 1000 + rtkTime->mss * 10;
        uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

        if (tp - stopTimepoint >= STOP_CAR_TIME && !reportStopCarTimeout) {
            // 停车超2秒，每次扣5分
@@ -90,14 +88,21 @@
    // 检查转向状态
    int az = (int) heading;

    if (ABS(az - azimuth) > 180) {
        az = 360 - ABS(az-azimuth);
    if (ABS(az - enterAreaHeading) > 180) {
        az = 360 - ABS(az-enterAreaHeading);
    } else {
        az = ABS(az - azimuth);
        az = ABS(az - enterAreaHeading);
    }



    if (az >= 30) {
        if (!turnLeftFinished) {
            if((( ((int)heading) + 360 - enterAreaHeading) % 360) < 180) {
                DEBUG("右转");
            } else {
                DEBUG("左转");
            }
            // 转向灯未开启，扣10分
            AddExamFault(30, rtkTime);
            DEBUG("转向灯未开启");

 lib/src/main/cpp/utils/xconvert.cpp

@@ -107,3 +107,26 @@
    totalSeconds += daysThisYear * SECONDS_PER_DAY;
    return totalSeconds;
}

uint32_t TimeMakeComposite(int hour, int minute, int second, int msecond)
{
    return (hour*SECONDS_PER_HOUR + minute*SECONDS_PER_MINUTE + second) * 1000 + msecond;
}

/*********************************************************
 * TIME1 - TIME2: msecond
 * @param hour1
 * @param minute1
 * @param second1
 * @param msecond1
 * @param hour2
 * @param minute2
 * @param second2
 * @param msecond2
 * @return
 */
uint32_t TimeGetDiff(int hour1, int minute1, int second1, int msecond1, int hour2, int minute2, int second2, int msecond2)
{
    return (TimeMakeComposite(hour1, minute1, second1, msecond1) + SECONDS_PER_DAY * 1000 -
            TimeMakeComposite(hour2, minute2, second2, msecond2)) % (SECONDS_PER_DAY * 1000);
}

 lib/src/main/cpp/utils/xconvert.h

@@ -11,5 +11,7 @@
void ConvertHex2String(char *str, const uint8_t *hex, int length);
void ConvertString2Hex(uint8_t *hex, int length, const char *str);
uint32_t TimeMakeComposite(int year, int month, int day, int hour, int minute, int second);
uint32_t TimeMakeComposite(int hour, int minute, int second, int msecond);
uint32_t TimeGetDiff(int hour1, int minute1, int second1, int msecond1, int hour2, int minute2, int second2, int msecond2);

#endif //MYAPPLICATION2_XCONVERT_H