车辆信号完善，路边停车完善。

yy1717

2020-03-24 25777013517d1bd398a98504826a417236706af2

车辆信号完善，路边停车完善。

 lib/src/main/cpp/CMakeLists.txt

@@ -41,6 +41,7 @@
        test_items2/road_exam.cpp
        test_items2/through_something.cpp
        test_items2/drive_straight.cpp
        test_items2/stop_car.cpp

        rtk_module/rtk.cpp
        rtk_module/virtual_rtk.cpp

 lib/src/main/cpp/Geometry.cpp

@@ -16,10 +16,16 @@
using namespace std;

const double EPSILON = 1e-6;
const double EPSILON2 = 1e-3;

inline bool isEqual(double a, double b)
{
    return (fabs(a - b) <= EPSILON);
}

inline bool isEqual2(double a, double b)
{
    return (fabs(a - b) <= EPSILON2);
}

inline double toRadians(double degree)
@@ -434,3 +440,37 @@
        return -1;
    }
}

/***************************************************************
 * 得到p3于p1,p2组成的直线上的垂点
 * @param p1
 * @param p2
 * @param p3
 * @return
 */
PointF GetVerticalPoint(PointF p1, PointF p2, PointF p3)
{
    PointF p4;

    if (isEqual2(p1.X, p2.X)) {
        p4.Y = p3.Y;
        p4.X = p1.X;
        return p4;
    }
    if (isEqual2(p1.Y, p2.Y)) {
        p4.X = p3.X;
        p4.Y = p1.Y;
        return p4;
    }

    double k = (p2.Y - p1.Y) / (p2.X - p1.X);
    double b = p1.Y - k * p1.X;

    double k2 = (p2.X - p1.X) / (p1.Y - p2.Y);
    double b2 = p3.Y - p3.X * k2;

    p4.X = (b2 - b) / (k - k2);
    p4.Y = k2 * p4.X + b2;

    return p4;
}

 lib/src/main/cpp/Geometry.h

@@ -36,6 +36,8 @@
inline double toRadians(double degree);
inline double toDegree(double radians);
inline bool isEqual(double a, double b);
inline bool isEqual2(double a, double b);

void MakeLine(Line *line, const PointF *p1, const PointF *p2);
void MakePolygon(Polygon *polygon, std::initializer_list<PointF> point_set);
void CleanPolygon(Polygon *polygon);
@@ -55,5 +57,6 @@
bool PartInsidePolygon(const Polygon *t1, const Polygon *t2);
bool OutsidePolygon(const Polygon *t1, const Polygon *t2);
int IntersectionOfLine(PointF p1, PointF p2, PointF p3);
PointF GetVerticalPoint(PointF p1, PointF p2, PointF p3);

#endif //GUI_GEOMETRY_H

 lib/src/main/cpp/driver_test.cpp

@@ -551,10 +551,9 @@

    if (start == 0) {
        DEBUG("结束考试");
//        CurrExamMapIndex = -1;
//        ExamStart = false;
//        CommTestStart(false);
//        MA_SendExamStatus(0, 0);
        CurrExamMapIndex = -1;
        ExamStart = false;
        MA_SendExamStatus(0, 0);
        return;
    }

@@ -728,6 +727,75 @@

static void ExecuteExam(const struct RtkTime* rtkTime)
{
    {
        static const char *NAME[] = {"OBD_SPEED",
                                     "ENGINE_RPM",
                                     "GEAR",
                                     "TURN_SIGNAL_LAMP",
                                     "DIPPED_BEAM_LAMP",
                                     "FOG_LAMP",
                                     "CLEARANCE_LAMP",
                                     "FLASH_BEAM_LAMP",
                                     "MAIN_BEAM_LAMP",
                                     "SEATBELT",
                                     "ENGINE_START",
                                     "BREAK",
                                     "HAND_BREAK",
                                     "SECOND_BREAK",
                                     "DOOR",
                                     "SURROUND_CAR_1",
                                     "SURROUND_CAR_2",
                                     "SURROUND_CAR_3",
                                     "SURROUND_CAR_4",
                                     "CAR_STATUS_END"};

        static const char *VALUE[] = {
                "0",
                "HAZARD_LIGHTS",
                "LEFT_TURN_LIGHT",
                "RIGHT_TURN_LIGHT",
                "CLEARANCE_LIGHT",
                "DIPPED_BEAM_LIGHT",
                "MAIN_BEAM_LIGHT",
                "FLASH_BEAM_LIGHT",
                "FOG_LIGHT",
                "INSERT_SEATBELT",
                "ENGINE_START_ACTIVE",
                "GEAR_N",
                "GEAR_1",
                "GEAR_2",
                "GEAR_3",
                "GEAR_4",
                "GEAR_5",
                "GEAR_R",
                "BREAK_ACTIVE",
                "DOOR_CLOSE",
                "SURROUND_CAR_ACTIVE"
        };



        static int cs[CAR_STATUS_END] = {0};

        int cs_temp[CAR_STATUS_END];

        for (int i = 0; i < CAR_STATUS_END; ++i) {
            cs_temp[i] = ReadCarStatus(i);

//            DEBUG("读取 %d  <----  %d", i, cs_temp[i]);
        }

        for (int i = 0; i < CAR_STATUS_END; ++i) {
            if (cs_temp[i] != cs[i]) {
                DEBUG("车辆状态 %s = %s", NAME[i], VALUE[ cs_temp[i] ]);

                cs[i] = cs_temp[i];
            }
        }

    }


    if (ReadCarStatus(ENGINE_RPM) < ENGINE_MIN_ROTATE) {
        if (engineRuning) {
            engineRuning = false;

 lib/src/main/cpp/native-lib.cpp

@@ -31,7 +31,7 @@

static pthread_mutex_t tts_mutex = PTHREAD_MUTEX_INITIALIZER;

static int ttsSeq = 0;
static int ttsSeq = 1;

static void SendBootIndicate(union sigval sig);


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

@@ -22,6 +22,7 @@
#include "../utils/num.h"
#include "../driver_test.h"
#include "../test_common/car_sensor.h"
#include "../test_items2/stop_car.h"

#define PARSE_BUFF_SIZE         4096

@@ -432,6 +433,7 @@
    }
    if (events & PLAY_TTS_DONE_EVT) {
        DummyLightTTSDone(*((int *)data));
        StopCarTTSDone(*((int *)data));
    }
}


 lib/src/main/cpp/test_common/car_sensor.cpp

@@ -23,7 +23,6 @@
    SENSOR_RIGHT_TURN_SIGNAL,
    SENSOR_HANDBREAK,
    SENSOR_BREAK,
    SENSOR_CLEARANCE_LIGHT,
    SENSOR_DIPPED_BEAM_LIGHT,
    SENSOR_MAIN_BEAM_LIGHT,
    SENSOR_DOOR,
@@ -35,6 +34,13 @@
    SENSOR_SHIFT_5,
    SENSOR_SHIFT_R,
    SENSOR_ENGINE_START,
    SENSOR_BREAK2,
    SENSOR_SURROUND_CAR_1,
    SENSOR_SURROUND_CAR_2,
    SENSOR_SURROUND_CAR_3,
    SENSOR_SURROUND_CAR_4,
    SENSOR_FOG_LIGHT,
    SENSOR_CLEARANCE_LIGHT,
    SENSOR_ENGINE_RPM,
    SENSOR_SPEED
};
@@ -137,8 +143,6 @@
            }
        }
    }


}

int ReadCarStatus(uint16_t id)
@@ -194,6 +198,8 @@

static void SensorChanged(int id, int value)
{
    DEBUG("状态改变 %d = %d", id, value);

    switch (id) {
        case SENSOR_LEFT_TURN_SIGNAL: {
            left_turn_signal = value;
@@ -220,6 +226,14 @@
                } else {
                    AppTimer_add(ConfirmTurnSigalLater, 200, RIGHT_TURN_LIGHT);
                }
            }
            break;
        }
        case SENSOR_FOG_LIGHT: {
            if (value == 0) {
                WriteCarStatus(FOG_LAMP, OFF_LIGHT);
            } else {
                WriteCarStatus(FOG_LAMP, FOG_LIGHT);
            }
            break;
        }
@@ -250,7 +264,7 @@
                WriteCarStatus(FLASH_BEAM_LAMP, FLASH_BEAM_LIGHT);

                AppTimer_delete(flashBeamLightClose);
                AppTimer_add(flashBeamLightClose, D_SEC(5));
                AppTimer_add(flashBeamLightClose, D_SEC(2));
            }

            AppTimer_delete(confirmFlashBeamLightLater);
@@ -266,6 +280,87 @@
            break;
        }
        case SENSOR_ENGINE_START: {
            if (value == 0) {
                WriteCarStatus(ENGINE_START, ENGINE_START_INACTIVE);
            } else {
                WriteCarStatus(ENGINE_START, ENGINE_START_ACTIVE);
            }
            break;
        }
        case SENSOR_BREAK: {
            if (value == 0) {
                WriteCarStatus(BREAK, BREAK_INACTIVE);
            } else {
                WriteCarStatus(BREAK, BREAK_ACTIVE);
            }
            break;
        }
        case SENSOR_BREAK2: {
            if (value == 0) {
                WriteCarStatus(SECOND_BREAK, BREAK_INACTIVE);
            } else {
                WriteCarStatus(SECOND_BREAK, BREAK_ACTIVE);
            }
            break;
        }
        case SENSOR_HANDBREAK: {
            if (value == 0) {
                WriteCarStatus(HAND_BREAK, BREAK_INACTIVE);
            } else {
                WriteCarStatus(HAND_BREAK, BREAK_ACTIVE);
            }
            break;
        }
        case SENSOR_DOOR: {
            if (value == 0) {
                WriteCarStatus(DOOR, DOOR_OPEN);
            } else {
                WriteCarStatus(DOOR, DOOR_CLOSE);
            }
            break;
        }
        case SENSOR_SURROUND_CAR_1: {
            if (value == 0) {
                WriteCarStatus(SURROUND_CAR_1, SURROUND_CAR_INACTIVE);
            } else {
                WriteCarStatus(SURROUND_CAR_1, SURROUND_CAR_ACTIVE);
            }
            break;
        }
        case SENSOR_SURROUND_CAR_2: {
            if (value == 0) {
                WriteCarStatus(SURROUND_CAR_2, SURROUND_CAR_INACTIVE);
            } else {
                WriteCarStatus(SURROUND_CAR_2, SURROUND_CAR_ACTIVE);
            }
            break;
        }
        case SENSOR_SURROUND_CAR_3: {
            if (value == 0) {
                WriteCarStatus(SURROUND_CAR_3, SURROUND_CAR_INACTIVE);
            } else {
                WriteCarStatus(SURROUND_CAR_3, SURROUND_CAR_ACTIVE);
            }
            break;
        }
        case SENSOR_SURROUND_CAR_4: {
            if (value == 0) {
                WriteCarStatus(SURROUND_CAR_4, SURROUND_CAR_INACTIVE);
            } else {
                WriteCarStatus(SURROUND_CAR_4, SURROUND_CAR_ACTIVE);
            }
            break;
        }
        case SENSOR_SHIFT_N:
        case SENSOR_SHIFT_1:
        case SENSOR_SHIFT_2:
        case SENSOR_SHIFT_3:
        case SENSOR_SHIFT_4:
        case SENSOR_SHIFT_5:
        case SENSOR_SHIFT_R: {
            if (value != 0) {
                WriteCarStatus(GEAR, id - SENSOR_SHIFT_N + GEAR_N);
            }
            break;
        }
        default:

 lib/src/main/cpp/test_common/car_sensor.h

@@ -7,6 +7,7 @@

#include <cstdint>

// Name
enum {
    OBD_SPEED,
    ENGINE_RPM,
@@ -23,15 +24,22 @@
    HAND_BREAK,
    SECOND_BREAK,
    DOOR,
    SURROUND_CAR_1,
    SURROUND_CAR_2,
    SURROUND_CAR_3,
    SURROUND_CAR_4,
    CAR_STATUS_END              //////////////
};

// Value
enum {
    OFF_LIGHT = 0,
    EJECT_SEATBELT = 0,
    ENGINE_START_INACTIVE = 0,
    BREAK_INACTIVE = 0,
    DOOR_OPEN = 0,
    SURROUND_CAR_INACTIVE = 0,

    HAZARD_LIGHTS,
    LEFT_TURN_LIGHT,
    RIGHT_TURN_LIGHT,
@@ -43,14 +51,15 @@
    INSERT_SEATBELT,
    ENGINE_START_ACTIVE,
    GEAR_N,
    SHIFT_R,
    GEAR_1,
    GEAR_2,
    GEAR_3,
    GEAR_4,
    GEAR_5,
    GEAR_R,
    BREAK_ACTIVE,
    DOOR_CLOSE
    DOOR_CLOSE,
    SURROUND_CAR_ACTIVE
};

void CarSensorInit(void);

 lib/src/main/cpp/test_items2/drive_straight.cpp

@@ -11,6 +11,9 @@

#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;
@@ -49,20 +52,25 @@
        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);
        }
    } else {
        double runDistance;
        runDistance = DistanceOf(car->basePoint, startPoint);
        double distanceToStart = DistanceOf(car->basePoint, startPoint);

        if (runDistance < 100.0) {
            if (!reportOffsetOver && fabs(DistanceOf(car->basePoint, road_edge) - edgeDistance) > 0.3) {
        if (distanceToStart > CHECK_STAGE_DISTANCE) {
            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;
            }
        } else {
            startPoint = car->basePoint;

 lib/src/main/cpp/test_items2/dummy_light.cpp

@@ -25,6 +25,7 @@

static int checkCnt;
static bool turn_left_active, flash_beam_active;

static int examTtsSeq = 0;
static bool testing;

@@ -43,6 +44,8 @@
        content[i].itemStatus = TTS_NOT_START;
    }
    testing = true;

    AppTimer_delete(DummyLightCheckActive);
    AppTimer_delete(ExamDummyLight);
    AppTimer_add(ExamDummyLight, D_SEC(2));
}
@@ -71,9 +74,11 @@

static void DummyLightCheckActive(union sigval sig)
{
    DEBUG("DummyLightCheckActive item = %d", sig.sival_int);
    int active = sig.sival_int;
    AppTimer_delete(DummyLightCheckActive);
    DEBUG("DummyLightCheckActive item = %d", active);

    switch (sig.sival_int) {
    switch (active) {
        case DRIVE_AT_NIGHT:
        case TURN_ON_MAIN_BEAM_LAMP:
            if (ReadCarStatus(MAIN_BEAM_LAMP) != MAIN_BEAM_LIGHT) {
@@ -95,8 +100,16 @@
        case THROUGE_CROSSWALK:
        case THROUGE_CURVE:
        case THROUGE_CROSSROADS:
            if (ReadCarStatus(FLASH_BEAM_LAMP) != FLASH_BEAM_LIGHT) {
            if (++checkCnt < 5) {
                if (ReadCarStatus(FLASH_BEAM_LAMP) == FLASH_BEAM_LIGHT) {
                    flash_beam_active = true;
                }
                AppTimer_add(DummyLightCheckActive, D_SEC(1), active);
                return;
            } else {
                if (!flash_beam_active) {
                AddExamFault(58, &currRtkTime);
                }
            }
            break;
        case OVERTAKE:
@@ -130,7 +143,7 @@
    }

    for (int i = 0; i < contentNum; ++i) {
        if (content[i].item == sig.sival_int) {
        if (content[i].item == active) {
            content[i].itemStatus = CHECK_OPERATE;
            break;
        }
@@ -161,8 +174,11 @@
                    checkCnt = 0;
                    turn_left_active = flash_beam_active = false;
                    AppTimer_add(DummyLightCheckActive, D_SEC(1), content[i].item);
                }
                else if (content[i].item >= 100)
                } else if (content[i].item == THROUGE_CROSSWALK || content[i].item == THROUGE_CURVE || content[i].item == THROUGE_CROSSROADS) {
                    checkCnt = 0;
                    flash_beam_active = false;
                    AppTimer_add(DummyLightCheckActive, D_SEC(1), content[i].item);
                } else if (content[i].item >= 100)
                    AppTimer_add(DummyLightCheckActive, D_SEC(3), content[i].item);
                else
                    AppTimer_add(DummyLightCheckActive, D_SEC(5), content[i].item);

 lib/src/main/cpp/test_items2/road_exam.cpp

@@ -13,6 +13,7 @@
#include "through_something.h"
#include "../master/comm_if.h"
#include "drive_straight.h"
#include "stop_car.h"

#include <vector>
#include <list>
@@ -55,13 +56,7 @@
static bool slideNormalDistance;
static bool occurSlide;

static struct drive_timer {
    int hour;
    int min;
    int sec;
    int msec;
} crashGreenRunTime, crashGreenCmpTime, crashGreenStartTime, turnSignalChangeTime;

static struct drive_timer crashGreenRunTime, crashGreenCmpTime, crashGreenStartTime, turnSignalChangeTime;
static struct drive_timer gearErrorTimePoint;
static struct drive_timer gearNSlideTimePoint;

@@ -85,7 +80,7 @@
static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};

static void TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime);
static void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime);

static char isTurn(int currYaw, int prevYaw, int &ang);
static char CheckCarTurn(LIST_CAR_MODEL &CarModelList);
static bool CrashRedLine(LIST_ROAD_MAP &RoadMapList, const car_model *car);
@@ -494,22 +489,29 @@
                StartThroughExam(currExamMapIndex, RoadMapList);
            } else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) {
                StartDriveStraightExam(currExamMapIndex, RoadMapList);
            } else if (RoadMapList[currExamMapIndex].type == STOP_CAR_MAP) {
                StartStopCarExam(currExamMapIndex, RoadMapList);
            }
        }
    } else if (startCar == START_CAR_DONE) {
        int prevIdx = currExamMapIndex;

        if (currExamMapIndex >= THROUGH_INTERSECTION_MAP && currExamMapIndex <= TURN_AROUND_MAP) {
        if (RoadMapList[currExamMapIndex].type >= THROUGH_INTERSECTION_MAP && currExamMapIndex <= TURN_AROUND_MAP) {
            currExamMapIndex = ExecuteThroughExam(currExamMapIndex, RoadMapList, car,
                                                  CarModelList, speed, moveDirect, rtkTime);
        }
        else if (currExamMapIndex == DRIVE_STRAIGHT_MAP) {
        else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) {
            currExamMapIndex = ExecuteDriveStraightExam(currExamMapIndex, RoadMapList, car,
                                     CarModelList, speed, moveDirect, rtkTime);
        } else if (RoadMapList[currExamMapIndex].type == STOP_CAR_MAP) {
            currExamMapIndex = ExecuteStopCarExam(currExamMapIndex, RoadMapList, car,
                                     CarModelList, speed, moveDirect, rtkTime);
        }

        if (currExamMapIndex == -1) {
            MA_EnterMap(RoadMapList[prevIdx].id, RoadMapList[prevIdx].type, 1);
            DEBUG("离开区域 index %d id %d type %d", prevIdx, RoadMapList[prevIdx].id, RoadMapList[prevIdx].type);

            MA_EnterMap(RoadMapList[prevIdx].id, RoadMapList[prevIdx].type, 0);
        }
    }
}
@@ -596,7 +598,7 @@
    return ret;
}

static void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
{
    tm.hour = rtkTime->hh;
    tm.min = rtkTime->mm;

 lib/src/main/cpp/test_items2/road_exam.h

@@ -19,6 +19,14 @@
#define THROUGH_BUS_STATION_MAP         109
#define TURN_AROUND_MAP                 110

struct drive_timer {
    int hour;
    int min;
    int sec;
    int msec;
};

void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime);
void InitRoadExam(void);
void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);
bool ExitSonArea(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car);

 lib/src/main/cpp/test_items2/stop_car.cpp

New file
@@ -0,0 +1,206 @@
//
// Created by YY on 2020/3/24.
//

#include "stop_car.h"
#include "../driver_test.h"
#include "../native-lib.h"
#include "../jni_log.h"
#include "road_exam.h"
#include "../utils/xconvert.h"
#include "../common/apptimer.h"
#include "../test_common/car_sensor.h"
#include "../defs.h"

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

static bool inactiveBreakHandbreakTogether, notCloseEngine, inactiveHandBreakAfterOpenDoor, occurOpenDoor, doorNotClose, checkRoadDistance;
static int examTtsSeq = 0;
static int ttsPlayEnd;
static double moveDistance;
static double prevSpeed;
static struct drive_timer prevPointTime;
static int prevMoveDirect;
static uint32_t stopTimepoint = 0;
static uint32_t openDoorTimepoint;

const int ENGINE_MIN_ROTATE = 200;

static const uint32_t STOP_CAR_TIME = D_SEC(2);
static const uint32_t OPEN_DOOR_TIMEOUT = D_SEC(15);
static const double DISTANCE_TO_ROAD_EDGE_1 = 0.5;
static const double DISTANCE_TO_ROAD_EDGE_2 = 0.3;

static void PlayTTSTimeout(union sigval sig);

void StartStopCarExam(int index, LIST_ROAD_MAP &RoadMapList) {
    if (index == -1)
        return;

    DEBUG("进入靠边停车地图 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type);

    ttsPlayEnd = 0;
    moveDistance = 0;
    prevMoveDirect = 0;
    inactiveBreakHandbreakTogether = false;
    notCloseEngine = false;
    inactiveHandBreakAfterOpenDoor = false;
    occurOpenDoor = false;
    doorNotClose = false;
    checkRoadDistance = false;

    if (!RoadMapList[index].tts.empty()) {
        examTtsSeq = PlayTTS(RoadMapList[index].tts.c_str());
    } else {
        examTtsSeq = PlayTTS("请靠边停车");
    }

    AppTimer_delete(PlayTTSTimeout);
    AppTimer_add(PlayTTSTimeout, D_SEC(8));
}

void StopCarTTSDone(int id)
{
    DEBUG("DummyLightTTSDone %d", id);
    // 等语音播报完毕后计时
    if (id == examTtsSeq) {
        ttsPlayEnd = 1;
    }
}

int ExecuteStopCarExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
                             LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) {
    if (ttsPlayEnd == 1) {
        ttsPlayEnd = 2;
        prevSpeed = speed;
        Rtk2DriveTimer(prevPointTime, rtkTime);
    }

    if (ttsPlayEnd != 2)
        return index;

    uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
                                rtkTime->mss * 10,
                                prevPointTime.hour, prevPointTime.min,
                                prevPointTime.sec, prevPointTime.msec * 10);

    moveDistance += (double)diff * (speed + prevSpeed) / 2;
    prevSpeed = speed;
    Rtk2DriveTimer(prevPointTime, rtkTime);

    if (moveDistance > 150) {
        // 150米内未停车，不合格
        DEBUG("停车距离超标");
        AddExamFault(33, rtkTime);
        return -1;
    }

    if (moveDirect != prevMoveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
            DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
        }
        prevMoveDirect = moveDirect;
    } else if (moveDirect == 0) {
        uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

        if (tp - stopTimepoint >= STOP_CAR_TIME && !checkRoadDistance) {
            // 停车超2秒，开始判断
            DEBUG("检测和路边的距离");
            PointF pc;

            checkRoadDistance = true;

            pc.X = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].X + car->carXY[car->right_rear_tire[TIRE_OUTSIDE]].X) / 2;
            pc.Y = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].Y + car->carXY[car->right_rear_tire[TIRE_OUTSIDE]].Y) / 2;

            // 检测道路边缘线
            for (int i = 0; i < RoadMapList[index].roadEdgeLineNum; ++i) {
                PointF p1, p2, pv;

                p1 = RoadMapList[index].roadEdgeLine[i].point[0];
                for (int j = 1; j < RoadMapList[index].roadEdgeLine[i].num; ++j) {
                    p2 = RoadMapList[index].roadEdgeLine[i].point[j];
                    pv = GetVerticalPoint(p1, p2, pc);

                    if (isEqual2(pv.X, MIN(p1.X, p2.X)) || isEqual2(pv.X, MAX(p1.X, p2.X)) ||
                            (pv.X > MIN(p1.X, p2.X) && pv.X < MAX(p1.X, p2.X))) {
                        double dis2roadEdge = DistanceOf(pc, pv);

                        DEBUG("停车距路边距离 %f", dis2roadEdge);

                        if (dis2roadEdge > DISTANCE_TO_ROAD_EDGE_1) {
                            DEBUG("停车超出路边0.5米");
                            // 停车距离超过50厘米，不合格
                            AddExamFault(36, rtkTime);
                        } else if (dis2roadEdge > DISTANCE_TO_ROAD_EDGE_2) {
                            DEBUG("停车超出路边0.3米");
                            // 停车距离超过30厘米，扣10分
                            AddExamFault(37, rtkTime);
                        }
                    }

                    p1 = p2;
                }
            }
        }
    }

    if (!inactiveBreakHandbreakTogether && ReadCarStatus(BREAK) == BREAK_INACTIVE && ReadCarStatus(HAND_BREAK) == BREAK_INACTIVE) {
        // 拉手刹前，松脚刹，扣10分
        DEBUG("拉手刹前，松脚刹");
        AddExamFault(39, rtkTime);
        inactiveBreakHandbreakTogether = true;
    }

    if (!notCloseEngine && ReadCarStatus(ENGINE_RPM) < ENGINE_MIN_ROTATE && ReadCarStatus(DOOR) == DOOR_OPEN) {
        // 下车前，不熄火，扣5分
        DEBUG("下车前，不熄火");
        AddExamFault(40, rtkTime);

        notCloseEngine = true;
    }

    if (!inactiveHandBreakAfterOpenDoor && ReadCarStatus(DOOR) == DOOR_OPEN && ReadCarStatus(HAND_BREAK) == BREAK_INACTIVE) {
        // 开门前，未拉手刹， 扣10分
        DEBUG("开门前，未拉手刹");
        AddExamFault(38, rtkTime);

        inactiveHandBreakAfterOpenDoor = true;
    }

    if (ReadCarStatus(DOOR) == DOOR_OPEN) {
        if (!occurOpenDoor) {
            occurOpenDoor = true;
            openDoorTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
        } else {
            uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

            if (!doorNotClose && tp - openDoorTimepoint >= OPEN_DOOR_TIMEOUT) {
                // 开门时间超过15秒，不合格
                DEBUG("开门时间超过15秒");
                AddExamFault(35, rtkTime);
                doorNotClose = true;
            }
        }
    } else if (occurOpenDoor) {
        return -1;
    }

    if (occurOpenDoor) {
        uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

        if (tp - openDoorTimepoint >= OPEN_DOOR_TIMEOUT + D_SEC(5)) {
            return -1;
        }
    }

    return index;
}

static void PlayTTSTimeout(union sigval sig)
{
    AppTimer_delete(PlayTTSTimeout);

    ttsPlayEnd = 1;
}

 lib/src/main/cpp/test_items2/stop_car.h

New file
@@ -0,0 +1,15 @@
//
// Created by YY on 2020/3/24.
//

#ifndef MYAPPLICATION2_STOP_CAR_H
#define MYAPPLICATION2_STOP_CAR_H

#include "../driver_test.h"

void StartStopCarExam(int index, LIST_ROAD_MAP &RoadMapList);
void StopCarTTSDone(int id);
int ExecuteStopCarExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
                       LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);

#endif //MYAPPLICATION2_STOP_CAR_H