上坡地图修正

fctom1215

2020-02-19 dc2a01d4764efd33a23afcaf4f1d7543dc35c4fa

上坡地图修正

 lib/src/main/cpp/driver_test.cpp

@@ -26,6 +26,7 @@
#include "test_items/stop_and_start.h"
#include "master/comm_if.h"
#include "utils/xconvert.h"
#include "test_items/comm_test.h"

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

@@ -87,9 +88,14 @@
} SensorConfig[32];
static int SensorNum = 0;

static int SensorValidLevel;

#define MOV_AVG_SIZE                1
#define RTK_BUFFER_SIZE            100
#define CAR_MODEL_CACHE_SIZE      10

static pthread_mutex_t rtk_clock_mutex = PTHREAD_MUTEX_INITIALIZER;
struct RtkTime rtkClock;

static rtk_info *RtkBuffer = NULL;
static int RtkBufferNum = 0, RtkBufferIn = 0;
@@ -106,14 +112,20 @@

void DriverTestInit(void)
{
    pthread_mutex_init(&rtk_clock_mutex, NULL);
    memset(&rtkClock, 0, sizeof(rtkClock));

    TestStart = false;
    memset(&MapList, 0, sizeof(MapList));
    MapNum = 0;
    CarModel = NULL;
    CarModelPrev = NULL;

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

    SensorValidLevel = 0;

    RtkBuffer = (rtk_info *) malloc(RTK_BUFFER_SIZE * sizeof(rtk_info));
    RtkBufferNum = RtkBufferIn = 0;
@@ -299,22 +311,43 @@

void SetSensorCfg(int (*sensor)[3], int sensorNum)
{
    DEBUG("SetSensorCfg sensorNum %d", sensorNum);
    DEBUG("加入传感器配置 sensorNum %d", sensorNum);
    SensorValidLevel = 0;

    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];

        if (sensor[i][2] != 0) {
            SensorValidLevel |= BV(i);
        }
    }
}

void GetFuncGpio(int func, int &gpio, int &lvl)
int GetSensorValidLevel(void)
{
    return SensorValidLevel;
}

void GetSensorCfg(int gpio, int &func, bool &lvl)
{
    for (int i = 0; i < SensorNum; ++i) {
        if (SensorConfig[i].gpioId == gpio) {
            func = SensorConfig[i].funId;
            lvl = SensorConfig[i].validLvl == 0 ? false : true;
        }
    }
}

void FindSensorCfg(int func, int &gpio, bool &lvl)
{
    gpio = -1;
    for (int i = 0; i < SensorNum; ++i) {
        if (SensorConfig[i].funId == func) {
            gpio = SensorConfig[i].gpioId;
            lvl = SensorConfig[i].validLvl;
            lvl = SensorConfig[i].validLvl == 0 ? false : true;
        }
    }
}
@@ -326,6 +359,7 @@
    DEBUG("StartDriverExam %d", start);
    if (start == 0) {
        TestStart = false;
        CommTestStart(false);
        MA_SendExamStatus(0, 0);
        return;
    }
@@ -344,13 +378,31 @@
            examFaultIndex = 0;

            TestStart = true;
            CommTestStart(true);
        }
        MA_SendExamStatus(1, 0);
    }
}

void GetRtkClock(struct RtkTime *s)
{
    pthread_mutex_lock(&rtk_clock_mutex);
    *s = rtkClock;
    pthread_mutex_unlock(&rtk_clock_mutex);
}

void UpdateRTKInfo(const rtk_info *s)
{
    pthread_mutex_lock(&rtk_clock_mutex);
    rtkClock.YY = s->YY;
    rtkClock.MM = s->MM;
    rtkClock.DD = s->DD;
    rtkClock.hh = s->hh;
    rtkClock.mm = s->mm;
    rtkClock.ss = s->ss;
    rtkClock.mss = s->dss;
    pthread_mutex_unlock(&rtk_clock_mutex);

    if (s->qf == 3) {
        RtkBuffer[RtkBufferIn] = *s;
        RtkBufferIn = (RtkBufferIn + 1) % RTK_BUFFER_SIZE;
@@ -372,7 +424,7 @@
                RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);

        brief.qf = RtkBuffer[index].qf;
        brief.map_id = 866;//GetMapId(CurrExamMapIndex, MapList, MapNum);
        brief.map_id = 863;//GetMapId(CurrExamMapIndex, MapList, MapNum);
        brief.move = move;
        brief.speed = speed * 3.6;
        brief.heading = RtkBuffer[index].heading;
@@ -431,7 +483,7 @@
                        DEBUG("进入倒车入库场地");
                        MA_SendDebugInfo("进入倒车入库场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));

                        StartParkBottom();
                        StartParkBottom(move, &rtkTime);
                        CurrExamStatus = 0;
                        break;
                    case MAP_TYPE_STOP_START:
@@ -465,7 +517,7 @@
                int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);
                switch (mtype) {
                    case MAP_TYPE_PARK_BUTTOM:
                        CurrExamStatus = TestParkBottom(ErrorList, &MapList[CurrExamMapIndex].map,
                        CurrExamStatus = TestParkBottom(&MapList[CurrExamMapIndex].map,
                                                        CarModel, CarModelPrev, speed, move, &rtkTime);
                        break;
                    case MAP_TYPE_STOP_START:
@@ -712,44 +764,46 @@
{
    // 车的最前点是否进入地图
    for (int i = 0; i < mapNum && car != NULL; ++i) {
        /*if (mapList[i].type == MAP_TYPE_STOP_START) {
            // 构造虚拟的左上角点
            double x9, y9, xo, yo;

            bool enter = false;

            xo = (mapList[i].map.point[0].X + mapList[i].map.point[7].X) / 2;
            yo = (mapList[i].map.point[0].Y + mapList[i].map.point[7].Y) / 2;

            x9 = 2*xo - mapList[i].map.point[8].X;
            y9 = 2*yo - mapList[i].map.point[8].Y;

            Polygon map;

            map.num = 4;
            map.point = (PointF *) malloc(map.num * sizeof(PointF));

            map.point[0] = mapList[i].map.point[0];
            map.point[1] = mapList[i].map.point[8];
            map.point[2] = mapList[i].map.point[7];
            map.point[3].X = x9;
            map.point[3].Y = y9;

            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &map) == GM_Containment) {
                Line enterLine1;

                MakeLine(&enterLine1, &(map.point[0]), &(map.point[3]));

                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
                    enter = true;
            }

            free(map.point);

            if (enter) return i;
        } 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 (mapList[i].type == MAP_TYPE_STOP_START) {
//            // 构造虚拟的左上角点
//            double x9, y9, xo, yo;
//
//            bool enter = false;
//
//            xo = (mapList[i].map.point[0].X + mapList[i].map.point[7].X) / 2;
//            yo = (mapList[i].map.point[0].Y + mapList[i].map.point[7].Y) / 2;
//
//            x9 = 2*xo - mapList[i].map.point[8].X;
//            y9 = 2*yo - mapList[i].map.point[8].Y;
//
//            Polygon map;
//
//            map.num = 4;
//            map.point = (PointF *) malloc(map.num * sizeof(PointF));
//
//            map.point[0] = mapList[i].map.point[0];
//            map.point[1] = mapList[i].map.point[8];
//            map.point[2] = mapList[i].map.point[7];
//            map.point[3].X = x9;
//            map.point[3].Y = y9;
//
//            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &map) == GM_Containment) {
//                Line enterLine1;
//
//                MakeLine(&enterLine1, &(map.point[0]), &(map.point[3]));
//
//                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
//                    enter = true;
//            }
//
//            free(map.point);
//
//            if (enter) return i;
//        }
//        if (mapList[i].type == MAP_TYPE_CURVE) {
//
//        }
        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) {
                Line enterLine1, enterLine2;

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

                MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));

                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
                    return i;
            }
        }
//        if (mapList[i].type == MAP_TYPE_TURN_90) {
//            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
//                Line enterLine1;
//
//                MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
//
//                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
//                    return i;
//            }
//        }
    }
    return -1;
}

 lib/src/main/cpp/driver_test.h

@@ -82,8 +82,12 @@
                        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)[3], int sensorNum);
void GetFuncGpio(int func, int &gpio, int &lvl);
int GetSensorValidLevel(void);
void GetSensorCfg(int gpio, int &func, bool &lvl);
void FindSensorCfg(int func, int &gpio, bool &lvl);

void StartDriverExam(int start);
void GetRtkClock(struct RtkTime *s);
void UpdateRTKInfo(const rtk_info *s);
void AddExamFault(int wrong, const struct RtkTime *rtkTime);
car_model_cache_t *GetCarModelCache(int node);

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

@@ -49,6 +49,8 @@
#define ID_SM_RTCM_IND           0x000F
#define ID_SM_DEBUG_INFO        0x0010
#define ID_MS_FILE              0x8100
#define ID_MS_READ_CARD         0x800F
#define ID_SM_PUT_CARD          0x000F

#define MA_OUT_GPS_BRIEF        0x0001
#define MA_OUT_RTK_BRIEF        0x0002
@@ -135,7 +137,7 @@

void MA_ReadSensor(void)
{

    ReadCard();
}

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

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

void MA_SendCardBrief(const struct cardBrief *brief)
{
    StringBuffer sb;
    Writer<StringBuffer> writer(sb);

    writer.StartObject();
    writer.Key("result");
    writer.Int(brief->result);
    writer.Key("serialno");
    writer.String(brief->card);
    writer.EndObject();

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

void MA_SendMcuBrief(const struct mcuBrief *brief)
@@ -688,6 +705,10 @@
            }
            break;
        }
        case ID_MS_READ_CARD : {
            ReadCard();
            break;
        }
        default:break;
    }
}

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

@@ -17,6 +17,11 @@
    char sn[20];
};

struct cardBrief {
    int result;
    char card[32];
};

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

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

@@ -72,6 +72,7 @@
static void SendDfuFile(int fileLen, int sentLen, int blockLen, const uint8_t *data);
static void GoNextDfuLater(union sigval sig);
static void GoNextDfu(void);
static void ReadCardTimeout(union sigval sig);

void ParseMcuInit(void)
{
@@ -369,6 +370,10 @@
            break;
        case ID_MC_RFCARD_RSP:
            DEBUG("ID_MC_RFCARD_RSP");
            AppTimer_delete(ReadCardTimeout);

            if (lenth > 0)
                PlatformStatusChanged(CARD_UPDATE_EVT, data, lenth);
            break;
        default:
            break;
@@ -484,3 +489,28 @@
        GoNextDfu();
    }
}

static int readCartCnt = 0;

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

    readCartCnt++;

    if (readCartCnt < 2) {
        AppTimer_add(ReadCardTimeout, D_SEC(3));
        SendMcuCommand(ID_CM_READ_RFCARD, NULL, 0);
    } else {
        uint8_t data[8] = {0};
        PlatformStatusChanged(CARD_UPDATE_EVT, data, sizeof(data));
    }
}

void ReadCard(void)
{
    readCartCnt = 0;

    AppTimer_delete(ReadCardTimeout);
    AppTimer_add(ReadCardTimeout, D_SEC(3));
    SendMcuCommand(ID_CM_READ_RFCARD, NULL, 0);
}

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

@@ -15,5 +15,6 @@

void UploadDfuFileEnd(void);
void UploadDfuFile(const uint8_t *file, int length);
void ReadCard(void);

#endif //RTKDRIVERTEST_MCU_IF_H

 lib/src/main/cpp/rtk_module/rtk.cpp

@@ -202,17 +202,17 @@
        }*/

        if (RxBufLen > 0) {
//            const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
//            if(ptr != RxBuf) {
//                memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf));
//                RxBufLen -= ptr - RxBuf;
//            } else if(RxBufLen == PARSE_BUFF_SIZE) {        //填满了，且没有一个\r，都抛弃
//                DEBUG("Parse GPS error");
//                RxBufLen = 0;
//            }
            const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
            if(ptr != RxBuf) {
                memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf));
                RxBufLen -= ptr - RxBuf;
            } else if(RxBufLen == PARSE_BUFF_SIZE) {        //填满了，且没有一个\r，都抛弃
                DEBUG("Parse GPS error");
                RxBufLen = 0;
            }


            RxBufLen = 0;           /////////////////////////////
//            RxBufLen = 0;           //PC模拟用时
        }
    }
    if (res == 0) {

 lib/src/main/cpp/rtk_platform/parse_net.cpp

@@ -21,6 +21,8 @@

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

//#define ENABLE_DEBUG_PROTOCOL

using namespace std;

#define PKT_HEAD_CHAR           0x7E
@@ -718,7 +720,7 @@
        }

#ifdef ENABLE_DEBUG_PROTOCOL
        /*{
        {
            static char buff[16384];

            buff[0] = 0;
@@ -731,15 +733,15 @@
                sprintf(buff + strlen(buff), "%02X ", item->data[i]);

                if (strlen(buff) > 800) {
                    DEBUG("%s -> %s...", item->access == DATA_ACCESS_MCU ? "UART" : "TCP" , buff);
                    DEBUG("DATAOUT: %s -> %s...", item->access == DATA_ACCESS_MCU ? "UART" : "TCP" , buff);
                    buff[0] = 0;
                }
            }

            if (strlen(buff) > 0) {
                DEBUG("%s -> %s", item->access == DATA_ACCESS_MCU ? "UART" : "TCP", buff);
                DEBUG("DATAOUT: %s -> %s", item->access == DATA_ACCESS_MCU ? "UART" : "TCP", buff);
            }
        }*/
        }
#endif
        // 系统层确认发送成功
        item->curr_cnt += 1;

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

@@ -21,6 +21,7 @@
#include "../utils/xconvert.h"
#include "../utils/num.h"
#include "../driver_test.h"
#include "../test_items/comm_test.h"

#define PARSE_BUFF_SIZE         4096

@@ -184,26 +185,6 @@
    pthread_attr_init(&attr);
    pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);//detached
    pthread_create(&pid, &attr, StatusListenThread, NULL);


    AddEvnet(100, (uint8_t *)"1234567890", 11);
    AddEvnet(101, (uint8_t *)"abcd", 5);
    AddEvnet(102, (uint8_t *)"xyz", 4);
    AddEvnet(104, NULL, 0);
    AddEvnet(105, (uint8_t *)"uio", 4);

    while (true) {
        struct event_t *evp = FetchEvent();

        if (evp != NULL) {
            DEBUG("得到event %d %d", evp->id, evp->length);

            if (evp->data != NULL)
                DEBUG("数据 %s", evp->data);
            RemoveEvent();
        } else
         break;
    }
}

static void ReqRtkPlatformConfigTimeout(union sigval sig)
@@ -413,6 +394,28 @@
        memcpy(brief.sn, data+40, 16);

        MA_SendMcuBrief(&brief);

        UpdateSensor(brief.gpio, brief.speed, brief.engine);
    }
    if (events & CARD_UPDATE_EVT) {
        DEBUG("CARD_UPDATE_EVT length %d", length);

        int ret = -1;

        for (int i = 0; i < length; ++i) {
            if (data[i] != 0) {
                ret = 0;
                break;
            }
        }

        struct cardBrief brief;
        
        brief.result = ret;

        ConvertHex2String(brief.card, data, length);

        MA_SendCardBrief(&brief);
    }
}


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

@@ -14,6 +14,7 @@
#define RTK_UPDATE_EVT                               0x0010
#define GPS_UPDATE_EVT                              0x0020
#define MCU_UPDATE_EVT                              0x0040
#define CARD_UPDATE_EVT                             0x0080

typedef struct {
    char domain_name[32];

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

@@ -5,20 +5,37 @@
#include "comm_test.h"
#include "../driver_test.h"
#include "../defs.h"
#include "../common/apptimer.h"

static bool seatbeltInsert;
static bool engineStart;

const int ENGINE_MIN_ROTATE = 200;

static bool commTest = false;
static uint16_t gpioStore = 0;
static uint16_t engineStore = 0;

static void SensorChange(int index, bool value);
static void SensorChange(int gpio, bool value);
static void EngineStartHold(union sigval sig);

void CommTestStart(void)
void CommTestInit(void)
{
    gpioStore = engineStore = 0;
    gpioStore = 0;
    engineStart = true;
    commTest = false;
}

void CommTestStart(bool start)
{
    commTest = start;
    if (start) {
        // 检查安全带
        if (CheckSensorX(SEATBELT) == 0) {
            struct RtkTime rt;
            GetRtkClock(&rt);
            AddExamFault(1, &rt);
        }
    }
}

void UpdateSensor(uint16_t gpio, uint16_t speed, uint16_t engine)
@@ -27,37 +44,128 @@

    uint16_t chg = gpioStore^gpio;

    if (chg == 0)
        return;
    gpioStore = gpio;

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

    gpioStore = gpio;



    // 安全带

    // 挡位

    // 启动指示

    // 熄火监控
    if (engine < ENGINE_MIN_ROTATE) {
        if (engineStart) {
            if (commTest) {
                // 熄火1次，扣10分
                struct RtkTime rt;
                GetRtkClock(&rt);
                AddExamFault(5, &rt);
            }
            engineStart = false;
        }
    } else {
        engineStart = true;
    }
}

static void SensorChange(int index, bool value)
int CheckSensorX(int func)
{
    int gpio;
    bool level;
    int validLevel = GetSensorValidLevel();

    // 传感器无效，认为通过
    switch (func) {
        case SEATBELT:
            FindSensorCfg(SENSOR_SEATBELT, gpio, level);
            if (gpio >= 0 && (gpioStore & BV(gpio)) != (validLevel & BV(gpio)) ) {
                return 0;
            }
            return 1;
        case LEFT_TURN_SIGNAL:
            FindSensorCfg(SENSOR_LEFT_TURN_SIGNAL, gpio, level);
            if (gpio == -1 || (gpioStore & BV(gpio)) == (validLevel & BV(gpio)) ) {
                return 1;
            }
            return 0;
        case RIGHT_TURN_SIGNAL:
            FindSensorCfg(SENSOR_RIGHT_TURN_SIGNAL, gpio, level);
            if (gpio == -1 || (gpioStore & BV(gpio)) == (validLevel & BV(gpio)) ) {
                return 1;
            }
            return 0;
        case HANDBREAK:
            FindSensorCfg(SENSOR_HANDBREAK, gpio, level);
            if (gpio == -1 || (gpioStore & BV(gpio)) == (validLevel & BV(gpio)) ) {
                return 1;
            }
            return 0;
        case SHIFT:
            FindSensorCfg(SENSOR_SHIFT_N, gpio, level);
            if (gpio == -1 || (gpioStore & BV(gpio)) == (validLevel & BV(gpio))) {
                return 'N';
            }
            return 0;
        default:
            break;
    }

    return -1;
}

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

    // 不及时松开启动开关，扣10分
    struct RtkTime rt;
    GetRtkClock(&rt);
    AddExamFault(4, &rt);
}

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

    GetSensorCfg(gpio, func, level);

    switch (func) {
        case SENSOR_SEATBELT:
            break;
        case SENSOR_TURNRIGHT:
            break;
            if (level != value) {
                // 不使用安全带，不合格
                seatbeltInsert = false;

                struct RtkTime rt;
                GetRtkClock(&rt);
                AddExamFault(1, &rt);
            } else {
                seatbeltInsert = true;
            }
            break;
        case SENSOR_RIGHT_TURN_SIGNAL:
            break;
        case SENSOR_LEFT_TURN_SIGNAL:
            break;
        case SENSOR_HANDBREAK:
            break;
        case SENSOR_ENGINE_START:
            AppTimer_delete(EngineStartHold);
            if (level == value) {
                AppTimer_add(EngineStartHold, D_SEC(2));

                // 检查是否在空挡
                if (CheckSensorX(SHIFT) != 'N') {
                    // 不是空挡点火，不合格
                    struct RtkTime rt;
                    GetRtkClock(&rt);
                    AddExamFault(3, &rt);
                }
            } else {

            }
            break;
        default:
            break;
    }
}

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

@@ -5,10 +5,42 @@
#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
#include <cstdint>

enum {
    SENSOR_SEATBELT,
    SENSOR_LEFT_TURN_SIGNAL,
    SENSOR_RIGHT_TURN_SIGNAL,
    SENSOR_HANDBREAK,
    SENSOR_BREAK,
    SENSOR_LIGHT,
    SENSOR_FAR_LIGHT,
    SENSOR_DOOR,
    SENSOR_SHIFT_N,
    SENSOR_SHIFT_1,
    SENSOR_SHIFT_2,
    SENSOR_SHIFT_3,
    SENSOR_SHIFT_4,
    SENSOR_SHIFT_5,
    SENSOR_SHIFT_R,
    SENSOR_ENGINE_START
};

enum {
    SEATBELT,
    LEFT_TURN_SIGNAL,
    RIGHT_TURN_SIGNAL,
    HANDBREAK,
    SHIFT,
    LIGHT,
    FAR_LIGHT,
    DOOR,
    ENGINE_START
};

void CommTestInit(void);
void CommTestStart(bool start);
void UpdateSensor(uint16_t gpio, uint16_t speed, uint16_t engine);
int CheckSensorX(int func);

#endif //MYAPPLICATION2_COMM_TEST_H

 lib/src/main/cpp/test_items/park_bottom.cpp

@@ -8,6 +8,7 @@
#include "../native-lib.h"
#include "../jni_log.h"
#include "../driver_test.h"
#include "../utils/xconvert.h"
#include <vector>
#include <cstdlib>

@@ -24,317 +25,211 @@
    THIRD_TOUCH_CTRL_LINE
};

const int PARK_TIMEOUT = 210;
const int PARK_TIMEOUT = D_SEC(210);

static bool PBTesting = false;
static bool trigLeaveTestAreaDetect = false;
static bool leaveTestArea = false;
static bool stopCar2S = false;
static bool testing = false;

static int currTarget;
static bool leftTireCrossLeftLine, leftTireCrossRightLine, rightTireCrossLeftLine, rightTireCrossRightLine;
static char first_ctrl_line_id;
static bool carStopEvent;               // 中途停车标记
static bool carParkSuccess;             // 是否停在库位
static bool parkTimeout;
const uint32_t STOP_CAR_TIME = D_SEC(2);
static uint32_t stopTimepoint;
static int prevMoveDirect, storeMoveDirectBeforeStop;
static bool occurCrashRedLine;
static bool checkPartStatus;
static uint32_t firstReverseTimepoint;
static bool reportExamTimeout, reportParkFail;
static bool crossCtrlLineSw;
static int parkCount;
static char carray[3];
static int darray[3];
static int parkStatus[3];

static void StopCarTimeout(union sigval sig);
static void LeaveTestAreaLongtime(union sigval sig);
static void CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car);
static char CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car);
static bool EnterParking(const Polygon *map, const car_model *car);
static void ParkBottomTimeout(union sigval sig);
static bool CrashRedLine(const Polygon *map, const car_model *car);
static bool ExitParkArea(const Polygon *map, const car_model *car);

void StartParkBottom(void)
void StartParkBottom(int moveDirect, const struct RtkTime *rtkTime)
{
    stopCar2S = false;
    trigLeaveTestAreaDetect = false;
    leaveTestArea = false;
    PBTesting = true;
    parkTimeout = false;
    first_ctrl_line_id = 0;
    currTarget = FIRST_TOUCH_CTRL_LINE;
    leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;

    DEBUG("StartParkBottom");
    testing = true;
    memset(carray, 0, sizeof(carray));
    memset(darray, 0, sizeof(darray));
    memset(parkStatus, 0, sizeof(parkStatus));
    prevMoveDirect = moveDirect;

    checkPartStatus = false;
    firstReverseTimepoint = 0;
    reportExamTimeout = false;
    parkCount = 0;
    crossCtrlLineSw = false;
    reportParkFail = false;
    occurCrashRedLine = false;
}

void StopParkBottom(void)
int TestParkBottom(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    PBTesting = false;
    AppTimer_delete(StopCarTimeout);
    AppTimer_delete(ParkBottomTimeout);
    AppTimer_delete(LeaveTestAreaLongtime);
    currTarget = NONE;
    char crossCtrlLine = 0;
    uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

    DEBUG("ParkBottom End");
}

int TestParkBottom(vector<int>&err, const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int run_status, const struct RtkTime *rtkTime)
{
    int status = 0;

    if (!PBTesting)
        return -2;

    DEBUG("TestParkBottom speed %f dir %d", speed, run_status);

    if (currTarget > FIRST_TOUCH_CTRL_LINE) {
        // 是否超时
        if (parkTimeout) {
            // 不合格：动作超时
            DEBUG("不合格：动作超时");
            err.push_back(10);
            AddExamFault(10, rtkTime);
            status = -1;
    if (ExitParkArea(map, car)) {
        DEBUG("离开场地");
        // 离开场地
        testing = false;
        if ((parkStatus[0] != 1 || parkStatus[1] != 1) && !reportParkFail) {
            // 倒库不入，不合格
            reportParkFail = true;
            AddExamFault(8, rtkTime);
            DEBUG("倒库不入");
        }
        // 是否压线
        if (CrashRedLine(map, car)) {
            // 不合格：车身出线
            DEBUG("不合格：车身出线");
            err.push_back(7);
        goto TEST_END;
    }

    if (CrashRedLine(map, car)) {
        if (!occurCrashRedLine) {
            occurCrashRedLine = true;
            // 车身出线，不合格
            AddExamFault(7, rtkTime);
            status = -1;
            DEBUG("车轮压线");
        }

        if (trigLeaveTestAreaDetect) {
            if (IntersectionOf(car->carXY[ car->left_front_tire[TIRE_OUTSIDE] ], map) == GM_Containment &&
                IntersectionOf(car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ], map) == GM_Containment) {
                trigLeaveTestAreaDetect = false;
                AppTimer_delete(LeaveTestAreaLongtime);
            }
        }
    } else {
        occurCrashRedLine = false;
    }

    if (currTarget == FIRST_TOUCH_CTRL_LINE ||
        currTarget == SECOND_TOUCH_CTRL_LINE ||
        currTarget == THIRD_TOUCH_CTRL_LINE) {
        if (run_status > 0) {
            if (stopCar2S && currTarget != FIRST_TOUCH_CTRL_LINE) {
                // 扣5分：中途停车超过2秒
                DEBUG("扣5分：中途停车超过2秒");
                err.push_back(11);
                AddExamFault(11, rtkTime);
            }

            if (!((leftTireCrossLeftLine && rightTireCrossLeftLine) ||
                    (leftTireCrossRightLine && rightTireCrossRightLine))) {
                DEBUG("CrossCtrlLine");
                CrossCtrlLine(map, car, carPrev);
            } else if (currTarget == FIRST_TOUCH_CTRL_LINE || currTarget == SECOND_TOUCH_CTRL_LINE) {
                // 跨过控制线后，车辆持续向前行驶，处理这些乱搞情况
                // 整个车都离开测试区后，如果持续15秒，还没回到测试区，就忽略该测试或者淘汰
                if (leaveTestArea) {
                    if (currTarget == FIRST_TOUCH_CTRL_LINE) {
                        status = -2;
                        DEBUG("来道场玩的");
                    } else {
                        // 不合格：未按规定线路行驶（直接跑出测试区了）
                        DEBUG("不合格：未按规定线路行驶（直接跑出测试区了）");
                        err.push_back(6);
                        AddExamFault(6, rtkTime);
                        status = -1;
                        DEBUG("直接跑出测试区了");
                    }
                } else if (!trigLeaveTestAreaDetect) {
                    Polygon car_body;

                    car_body.num = car->bodyNum;
                    car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
                    for (int i = 0; i < car_body.num; ++i) {
                        car_body.point[i] = car->carXY[car->body[i]];
                    }

                    if (IntersectionOf(map, &car_body) == GM_None) {
                        trigLeaveTestAreaDetect = true;
                        AppTimer_delete(LeaveTestAreaLongtime);
                        AppTimer_add(LeaveTestAreaLongtime, D_SEC(15));
                        DEBUG("开始离场计时");
                    }

                    free(car_body.point);
                }
            }

            if (currTarget == THIRD_TOUCH_CTRL_LINE) {
                char the_ctrl_line_crossed = 0;

                if (leftTireCrossLeftLine && rightTireCrossLeftLine) {
                    the_ctrl_line_crossed = 'L';
                } else if (leftTireCrossRightLine && rightTireCrossRightLine) {
                    the_ctrl_line_crossed = 'R';
                }

                if (the_ctrl_line_crossed != 0 && the_ctrl_line_crossed == first_ctrl_line_id) {
                    // 项目完成
                    status = 1;
                } else if (the_ctrl_line_crossed != 0) {
                    // 不合格：未按规定线路行驶（未回到起始点）
                    DEBUG("不合格：未按规定线路行驶（未回到起始点）");
                    err.push_back(6);
                    AddExamFault(6, rtkTime);
                    status = -1;
                }
            }
            if (carStopEvent)
                AppTimer_delete(StopCarTimeout);
            carStopEvent = false;
            stopCar2S = false;
        } else if (run_status < 0) {
            // 左右倒库大纲并未要求谁先完成，故以先越过的控制线为准，下次得越过另外一条
            char the_ctrl_line_crossed = 0;

            if (leftTireCrossLeftLine && rightTireCrossLeftLine) {
                the_ctrl_line_crossed = 'L';
            } else if (leftTireCrossRightLine && rightTireCrossRightLine) {
                the_ctrl_line_crossed = 'R';
            }

            if (first_ctrl_line_id > 0 && first_ctrl_line_id == the_ctrl_line_crossed) {
                // 不合格：未按规定线路行驶（试图做2次同方向的倒库）
                DEBUG("不合格：未按规定线路行驶（试图做2次同方向的倒库）");
                err.push_back(6);
    crossCtrlLine = CrossCtrlLine(map, car, carPrev);
    if (crossCtrlLine > 0 && !crossCtrlLineSw) {
        crossCtrlLineSw = true;
        if (parkCount == 0) {
            carray[0] = crossCtrlLine;
        } else if (parkCount == 1) {
            if (carray[0] == crossCtrlLine) {
                // 不按规定线路，顺序形式，不合格
                AddExamFault(6, rtkTime);
                status = -1;
            } else if (the_ctrl_line_crossed > 0 && first_ctrl_line_id == 0) {
                first_ctrl_line_id = the_ctrl_line_crossed;
                // 项目正式开始，210秒内完成
                AppTimer_delete(ParkBottomTimeout);
                AppTimer_add(ParkBottomTimeout, D_SEC(PARK_TIMEOUT));
                currTarget = FIRST_PARK;
                carParkSuccess = false;
                parkTimeout = false;
                leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;

                DEBUG("第一次倒库");
            } else if (the_ctrl_line_crossed > 0) {
                currTarget = SECOND_PARK;
                carParkSuccess = false;
                leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;
                DEBUG("第二次倒库");
            } else if (currTarget != THIRD_TOUCH_CTRL_LINE) {
                // 不合格：倒车前，2前轮没驶过控制线
                DEBUG("不合格：倒车前，2前轮没驶过控制线");
                err.push_back(9);
                AddExamFault(9, rtkTime);
                status = -1;
                DEBUG("不按规定线路，顺序形式");
            }
            if (carStopEvent)
                AppTimer_delete(StopCarTimeout);
            carStopEvent = false;
            stopCar2S = false;
        } else {
            if (!carStopEvent) {
                AppTimer_delete(StopCarTimeout);
                AppTimer_add(StopCarTimeout, D_SEC(2));
            carray[1] = crossCtrlLine;
        } else if (parkCount == 2) {
            if (carray[0] != crossCtrlLine) {
                // 不按规定线路，顺序形式，不合格
                AddExamFault(6, rtkTime);
                DEBUG("不按规定线路，顺序形式");
            } else {
                // 离开测试区，停止计时
                DEBUG("离开测试区，停止计时");
                testing = false;
                goto TEST_END;
            }
            carStopEvent = true;
            carray[2] = crossCtrlLine;
        }
    } else if (currTarget == FIRST_PARK || currTarget == SECOND_PARK) {
        if (run_status < 0) {
            if (stopCar2S) {
                // 扣5分：中途停车
                DEBUG("扣5分：中途停车");
                err.push_back(11);
                AddExamFault(11, rtkTime);
    }

    if (testing && darray[0] > 0 && tp - firstReverseTimepoint >= PARK_TIMEOUT) {
        // 完成超时，不合格
        if (!reportExamTimeout) {
            reportExamTimeout = true;
            AddExamFault(10, rtkTime);
            DEBUG("项目超时");
        }
    }

    if (moveDirect != prevMoveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = tp;
            storeMoveDirectBeforeStop = prevMoveDirect;
            if (prevMoveDirect == -1) {
                checkPartStatus = true;
            }

            carStopEvent = false;
            stopCar2S = false;
        } else if (run_status == 0) {
            // 立即检查是否停车到位，也许是中途停车，先不管，待发生前进事件后，再断定是否停车到位
            if (!carStopEvent) {
                carStopEvent = true;
                carParkSuccess = EnterParking(map, car);
                AppTimer_delete(StopCarTimeout);
                AppTimer_add(StopCarTimeout, D_SEC(2));
            }
            DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
        } else {
            if (carStopEvent) {
                if (!carParkSuccess) {
                    // 不合格：倒库不入
                    DEBUG("不合格：倒库不入");
                    err.push_back(8);
                    AddExamFault(8, rtkTime);
                    status = -1;
                } else if (currTarget == FIRST_PARK) {
                    currTarget = SECOND_TOUCH_CTRL_LINE;
                    DEBUG("过另一根控制线");
                } else {
                    currTarget = THIRD_TOUCH_CTRL_LINE;
                    DEBUG("再过第一根控制线");
            DEBUG("继续行驶 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);

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

            if (moveDirect == storeMoveDirectBeforeStop) {
                // 同方向再启动，继续判断是否停车超时
                if (tp - stopTimepoint >= STOP_CAR_TIME) {
                    // 停车超2秒，每次扣5分
                    AddExamFault(11, rtkTime);
                    DEBUG("中途停车");
                }
            } else if (moveDirect == -1) {
                // 切换为倒车
                if (darray[parkCount] == 0) {
                    if (!crossCtrlLineSw) {
                        // 倒车前，前轮未驶过控制线
                        AddExamFault(9, rtkTime);
                        DEBUG("倒车前，前轮未驶过控制线");
                    }
                    darray[parkCount] = 1;
                    firstReverseTimepoint = tp;
                }
            } else {
                // 切换为前进
                if (tp - stopTimepoint >= STOP_CAR_TIME) {
                    if (crossCtrlLineSw) {
                        if (checkPartStatus) {
                            if (EnterParking(map, car)) {
                                parkStatus[parkCount] = 1;
                            }
                            checkPartStatus = false;
                        }
                        if (parkStatus[parkCount] != 1) {
                            // 倒库不入，不合格
                            reportParkFail = true;
                            AddExamFault(8, rtkTime);
                            DEBUG("倒库不入");
                        }
                    }

                    crossCtrlLineSw = false;

                    if (parkCount < 2)
                        parkCount++;
                }
            }
            carStopEvent = false;
            stopCar2S = false;
        }
        prevMoveDirect = moveDirect;
    } else if (moveDirect == -1) {
        if (darray[parkCount] == 0) {
            // 切换为倒车
            if (!crossCtrlLineSw) {
                // 倒车前，前轮未驶过控制线
                AddExamFault(9, rtkTime);
                DEBUG("倒车前，前轮未驶过控制线");
            }
            darray[parkCount] = 1;
            firstReverseTimepoint = tp;
        }
    } else if (moveDirect == 0 && crossCtrlLineSw) {
        if (tp - stopTimepoint >= STOP_CAR_TIME && checkPartStatus) {
            if (EnterParking(map, car)) {
                parkStatus[parkCount] = 1;
            }
            checkPartStatus = false;
        }
    }

    if (status != 0) {
        StopParkBottom();
    }

    return status;
}

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

    stopCar2S = true;
}

static void LeaveTestAreaLongtime(union sigval sig) {
    AppTimer_delete(LeaveTestAreaLongtime);
    leaveTestArea = true;
    trigLeaveTestAreaDetect = false;
}

static void ParkBottomTimeout(union sigval sig) {
    AppTimer_delete(ParkBottomTimeout);
    parkTimeout = true;
TEST_END:
    return testing ? 0 : 1;
}

// 检测2前轮是否正向越过左右控制线
static void CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car)
static char CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car)
{
    Line leftCtrlLine, rightCtrlLine;
    Line track1;
    PointF p1, p2;
    // 过右控制线
    if ((IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1) &&
        (IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->right_front_tire[TIRE_OUTSIDE]]) == -1) &&
        (IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->axial[AXIAL_REAR]]) == 1)) {
        return 'R';
    }

    MakeLine(&leftCtrlLine, &(map->point[0]), &(map->point[1]));
    MakeLine(&rightCtrlLine, &(map->point[6]), &map->point[7]);
    // 左前轮，取轮宽的中点
    p1.X = (car->carXY[car->left_front_tire[TIRE_OUTSIDE]].X + car->carXY[car->left_front_tire[TIRE_INSIDE]].X) / 2;
    p1.Y = (car->carXY[car->left_front_tire[TIRE_OUTSIDE]].Y + car->carXY[car->left_front_tire[TIRE_INSIDE]].Y) / 2;
    p2.X = (prev_car->carXY[car->left_front_tire[TIRE_OUTSIDE]].X + prev_car->carXY[car->left_front_tire[TIRE_INSIDE]].X) / 2;
    p2.Y = (prev_car->carXY[car->left_front_tire[TIRE_OUTSIDE]].Y + prev_car->carXY[car->left_front_tire[TIRE_INSIDE]].Y) / 2;
    MakeLine(&track1, &p1, &p2);
    if (IntersectionOf(track1, leftCtrlLine) == GM_Intersection &&
            IntersectionOfLine(map->point[0], map->point[1], p1) == -1) {
        leftTireCrossLeftLine = true;
        DEBUG("左轮跨越左控制线");
    // 过左控制线
    if ((IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == 1) &&
        (IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->right_front_tire[TIRE_OUTSIDE]]) == 1) &&
        (IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->axial[AXIAL_REAR]]) == -1)) {
        return 'L';
    }
    if (IntersectionOf(track1, rightCtrlLine) == GM_Intersection &&
            IntersectionOfLine(map->point[6], map->point[7], p1) == -1) {
        leftTireCrossRightLine = true;
        DEBUG("左轮跨越右控制线");
    }
    // 右前轮
    p1.X = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].X + car->carXY[car->right_front_tire[TIRE_INSIDE]].X) / 2;
    p1.Y = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].Y + car->carXY[car->right_front_tire[TIRE_INSIDE]].Y) / 2;
    p2.X = (prev_car->carXY[car->right_front_tire[TIRE_OUTSIDE]].X + prev_car->carXY[car->right_front_tire[TIRE_INSIDE]].X) / 2;
    p2.Y = (prev_car->carXY[car->right_front_tire[TIRE_OUTSIDE]].Y + prev_car->carXY[car->right_front_tire[TIRE_INSIDE]].Y) / 2;
    MakeLine(&track1, &p1, &p2);
    if (IntersectionOf(track1, leftCtrlLine) == GM_Intersection &&
            IntersectionOfLine(map->point[0], map->point[1], p1) == -1) {
        rightTireCrossLeftLine = true;
        DEBUG("右轮跨越左控制线");
    }
    if (IntersectionOf(track1, rightCtrlLine) == GM_Intersection &&
            IntersectionOfLine(map->point[6], map->point[7], p1) == -1) {
        rightTireCrossRightLine = true;
        DEBUG("右轮跨越右控制线");
    }

    return 0;
}

static bool EnterParking(const Polygon *map, const car_model *car) {
@@ -357,6 +252,8 @@

    CleanPolygon(&parking);
    free(car_body.point);

    DEBUG("检查倒库状态 %s", succ ? "成功" : "失败");

    return succ;
}
@@ -387,3 +284,25 @@
    free(car_body.point);
    return ret;
}

static bool ExitParkArea(const Polygon *map, const car_model *car)
{
// 全车都需不在地图中
    bool ret = false;

    Polygon carBody;

    carBody.num = car->bodyNum;
    carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
    for (int i = 0; i < carBody.num; ++i) {
        carBody.point[i] = car->carXY[car->body[i]];
    }

    if (IntersectionOf(&carBody, map) == GM_None) {
        ret = true;
    }

    free(carBody.point);

    return ret;
}

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

@@ -11,8 +11,7 @@

using namespace std;

void StartParkBottom(void);
void StopParkBottom(void);
int TestParkBottom(vector<int>&err, const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int run_status, const struct RtkTime *rtkTime);
void StartParkBottom(int moveStatus, const struct RtkTime *rtkTime);
int TestParkBottom(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime);

#endif //RTKDRIVERTEST_PARK_BOTTOM_H

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

@@ -27,6 +27,7 @@
static int parkStatus;
static bool occurMoveBack, parkSuccess;
static uint32_t moveBackTimePoint;
static bool testing = false;

static bool CrashRedLine1(const Polygon *map, const car_model *car);
static bool CrashRedLine2(const Polygon *map, const car_model *car);
@@ -36,6 +37,10 @@

void StartParkEdge(int moveStatus, const struct RtkTime *rtkTime)
{
    DEBUG("进入侧方停车场地");

    testing = true;

    occurCrashRedLine1 = occurCrashRedLine2 = false;        // 这个科目规定特殊点，发生一次扣10分，而不直接淘汰
    reportExamTimeout = false;

@@ -47,14 +52,10 @@
        occurMoveBack = true;
        moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
    }

    DEBUG("进入侧方停车场地");
}

int TestParkEdge(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime)
{
    int status = 0;

    if (CrashRedLine1(map, car)) {
        if (!occurCrashRedLine1) {
            // 车轮压边线，每次扣10分
@@ -83,7 +84,8 @@
            AddExamFault(21, rtkTime);
            DEBUG("直接驶离测试区,移库不入");
        }
        status = 1;
        testing = false;
        goto TEST_END;
    }

    if (occurMoveBack) {
@@ -121,7 +123,10 @@
                // 倒车切换到前进
                if (moveStatus == 1 && tp - stopTimepoint >= STOP_CAR_TIME) {
                    if (parkStatus == 0) {

                        if (EnterParking(map, car)) {
                            parkStatus = 1;
                            parkSuccess = true;
                        }
                    }

                    if (!parkSuccess) {
@@ -149,7 +154,6 @@
        prevMoveStatus = moveStatus;
    } else if (moveStatus == 0 && parkStatus == 0) {
        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) {
            if (EnterParking(map, car)) {
@@ -160,8 +164,8 @@
            }
        }
    }

    return status;
TEST_END:
    return testing ? 0 : 1;
}

// 车轮是否压道路边线

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

@@ -26,7 +26,7 @@

const double EPSILON = 1e-3;

static bool SASTesting = false;
static bool testing = false;

static double slideDistance;
static PointF stopPoint;
@@ -49,7 +49,7 @@
void StartSAS(int moveDirect, const struct RtkTime *rtkTime)
{
    DEBUG("进入坡起项目");
    SASTesting = true;
    testing = true;
    slideDistance = 0.0;

    prevMoveDirect = moveDirect;
@@ -68,8 +68,6 @@

int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    int status = 0;

    if (CrashRedLine(map, car)) {
        // 车轮压线，不合格
        if (!occurCrashRedLine) {
@@ -83,7 +81,7 @@

    if (ExitTestArea(map, car)) {
        // 驶离测试区
        status = 1;
        testing = false;
    }

    if (prevMoveDirect != moveDirect) {
@@ -173,7 +171,7 @@
        }
    }

    return status;
    return testing ? 0 : 1;
}

// 车轮是否压边线

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

@@ -19,7 +19,7 @@

const uint32_t STOP_CAR_TIME = D_SEC(2);

static bool TA90Testing;
static bool testing;

static int enterAreaHeading;
static bool turnLeftFinished;
@@ -35,6 +35,7 @@
void StartTurnA90(int moveDirect, double heading, const struct RtkTime *rtkTime)
{
    DEBUG("进入直角转弯场地");
    testing = true;
    enterAreaHeading = (int) heading;
    prevMoveDirect = moveDirect;
    if (moveDirect == 0) {
@@ -47,10 +48,11 @@

int TestTurnA90(const Polygon *map, const car_model *car, const car_model *carPrev, double heading, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    int status = 0;
    int az = (int) heading;

    if (ExitTestArea(map, car)) {
        status = 1;
        testing = false;
        goto TEST_END;
    }

    if (CrashRedLine(map, car)) {
@@ -86,15 +88,11 @@
    }

    // 检查转向状态
    int az = (int) heading;

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



    if (az >= 30) {
        if (!turnLeftFinished) {
@@ -113,8 +111,8 @@
    if (turnLeftFinished) {

    }

    return status;
TEST_END:
    return testing?0:1;
}

// 车轮是否压边线