进出科目二区域调整

yy1717

2020-04-02 24428a1b69afcf113883e9cede1159443201a1b8

进出科目二区域调整

 lib/src/main/cpp/driver_test.cpp

@@ -40,12 +40,6 @@

using namespace std;

#define MAP_TYPE_PARK_BUTTOM        1
#define MAP_TYPE_STOP_START         2
#define MAP_TYPE_PART_EDGE         3
#define MAP_TYPE_CURVE         4
#define MAP_TYPE_TURN_90         5

#define RTK_INVALID         0
#define RTK_SINGLE_POINT   1
#define RTK_DIFF            2
@@ -117,7 +111,8 @@
static void UpdateCarBodyCoord(struct RtkTime *rtkTime, double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel);
static bool UpdateCarCoord(double &spd, int &mov, int &idx);

static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum);
static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList);
static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, const struct map_list *mapList, int mapNum);
static bool ExitMap(const car_model *car, int mapId, const struct map_list *mapList, int mapNum);
static int GetMapId(int index, const struct map_list *mapList, int mapNum);
static int GetMapType(int index, const struct map_list *mapList, int mapNum);
@@ -650,7 +645,7 @@
                RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);

        brief.qf = RtkBuffer[index].qf;
        brief.map_id = GetMapId(CurrExamMapIndex, MapList, MapNum);
        brief.map_id = -1;//GetMapId(CurrExamMapIndex, MapList, MapNum);
        brief.move = move;
        brief.speed = speed * 3.6;
        brief.heading = RtkBuffer[index].heading;
@@ -712,18 +707,18 @@
    }
    if (CurrExamMapIndex < 0) {
        if (CurrEnterMapIndex < 0) {
            CurrEnterMapIndex = EnterMap(CarModel, MapList, MapNum);
            CurrEnterMapIndex = EnterMap(CarModel, CarModelList, MapList, MapNum);
            if (CurrEnterMapIndex >= 0) {
                DEBUG("发送进入场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
                MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 1);
//                DEBUG("发送进入场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
//                MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 1);

                CurrExamMapIndex = CurrEnterMapIndex;
                CurrExamStatus = EXAM_AREA_START;
            }
        } else {
            if (ExitMap(CarModel, CurrEnterMapIndex, MapList, MapNum)) {
                DEBUG("发送离开场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
                MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 0);
//                DEBUG("发送离开场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
//                MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 0);
                CurrEnterMapIndex = -1;
            }
        }
@@ -894,33 +889,33 @@
                    DEBUG("进入倒车入库场地");
                    MA_SendDebugInfo("进入倒车入库场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));

                    StartParkBottom(move, rtkTime);
                    StartParkBottom(CurrExamMapIndex, move, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_STOP_START:
                    DEBUG("进入上坡起步场地");
                    MA_SendDebugInfo("进入上坡起步场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    StartSAS(move, rtkTime);
                    StartSAS(CurrExamMapIndex, move, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_PART_EDGE:
                    DEBUG("进入侧方位停车场地");
                    MA_SendDebugInfo("进入侧方位停车场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    StartParkEdge(move, rtkTime);
                    StartParkEdge(CurrExamMapIndex, move, rtkTime);

                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_CURVE:
                    DEBUG("进入曲线行驶场地");
                    MA_SendDebugInfo("进入曲线行驶场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    StartDrivingCurve(move, rtkTime);
                    StartDrivingCurve(CurrExamMapIndex, move, rtkTime);

                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_TURN_90:
                    DEBUG("进入直角转弯场地");
                    MA_SendDebugInfo("进入直角转弯场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    StartTurnA90(move, azimuth, rtkTime);
                    StartTurnA90(CurrExamMapIndex, move, azimuth, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                default:break;
@@ -1145,15 +1140,79 @@
    return true;
}

static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum)
static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    // 车的最前点是否进入地图
    bool trigger = false;

    if (CarModelList.size() < 5)
        return trigger;

    Polygon trace, trace2;
    int pn = 0;

    trace2.num = trace.num = 5;
    trace.point = (PointF *) malloc(sizeof(PointF) * trace.num);
    trace2.point = (PointF *) malloc(sizeof(PointF) * trace2.num);

    list<car_model *>::iterator iter = CarModelList.begin();

    car_model *c1 = *iter;

    trace.point[pn] = c1->carXY[c1->left_front_tire[TIRE_OUTSIDE]];
    trace2.point[pn++] = c1->carXY[c1->left_rear_tire[TIRE_OUTSIDE]];

    ++iter;

    while (iter != CarModelList.end() && pn < trace.num) {
        car_model *c2 = *iter;

        uint32_t tdiff = TimeGetDiff(c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);

        if (tdiff >= D_SEC(1)) {
            trace.point[pn] = c2->carXY[c2->left_front_tire[TIRE_OUTSIDE]];
            trace2.point[pn++] = c2->carXY[c2->left_rear_tire[TIRE_OUTSIDE]];
            c1 = c2;
        }
        ++iter;
    }

    PointF p1, p2;

    p1.X = triggerLine.X1;
    p1.Y = triggerLine.Y1;
    p2.X = triggerLine.X2;
    p2.Y = triggerLine.Y2;

    int pp = 0;
    for (int p = 1; p < pn; ++p) {
        Line trace_line, trace2_line;

        MakeLine(&trace_line, &trace.point[pp], &trace.point[p]);
        MakeLine(&trace2_line, &trace2.point[pp], &trace2.point[p]);

        if ((IntersectionOf(trace_line, triggerLine) == GM_Intersection || IntersectionOf(trace2_line, triggerLine) == GM_Intersection) &&
            IntersectionOfLine(p1, p2, car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 &&
            DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], triggerLine) > 0.1) {
            // 碰到触发线
            DEBUG("碰撞触发线 引发地图");
            trigger = true;
            goto SEARCH_TRIGGER_LINE_END;
        }
    }

SEARCH_TRIGGER_LINE_END:
    free(trace.point);

    return trigger;
}

static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, const struct map_list *mapList, int mapNum)
{
    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;
@@ -1161,31 +1220,18 @@
            x9 = 2*xo - mapList[i].map.point[8].X;
            y9 = 2*yo - mapList[i].map.point[8].Y;

            Polygon map;
            Line triggerLine;

            map.num = 4;
            map.point = (PointF *) malloc(map.num * sizeof(PointF));
            triggerLine.X1 = mapList[i].map.point[0].X;
            triggerLine.Y1 = mapList[i].map.point[0].Y;
            triggerLine.X2 = x9;
            triggerLine.Y2 = y9;

            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 (CrashTriggerLine(triggerLine, car, CarModelList))
                return i;
        }
        if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
            // 车头顶点在场地内
            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
                Line enterLine1, enterLine2;

@@ -1198,6 +1244,7 @@
            }
        }
        if (mapList[i].type == MAP_TYPE_PART_EDGE) {
            // 车头顶点在场地内
            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
                Line enterLine;

@@ -1208,26 +1255,20 @@
            }
        }
        if (mapList[i].type == MAP_TYPE_TURN_90) {
            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
                Line enterLine1;
            // 车前轮或后轮轨迹越过触发线
            Line triggerLine;

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

                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
                    return i;
            }
            if (CrashTriggerLine(triggerLine, car, CarModelList))
                return i;
        }
        if (mapList[i].type == MAP_TYPE_CURVE) {
            Line startLine;
            Line carAxial;
            Line triggerLine;

            MakeLine(&startLine, &mapList[i].map.point[0], &mapList[i].map2.point[0]);
            MakeLine(&carAxial, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]]);

            if (IntersectionOf(startLine, carAxial) == GM_Intersection) {
                if (DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], startLine) > 0.1)
                    return i;
            }
            MakeLine(&triggerLine, &mapList[i].map2.point[0], &mapList[i].map.point[0]);
            if (CrashTriggerLine(triggerLine, car, CarModelList))
                return i;
        }
    }
    return -1;

 lib/src/main/cpp/driver_test.h

@@ -21,7 +21,13 @@
#define TIRE_OUTSIDE    0
#define TIRE_INSIDE     1
#define AXIAL_FRONT     0
#define AXIAL_REAR     1
#define AXIAL_REAR      1

#define MAP_TYPE_PARK_BUTTOM        1
#define MAP_TYPE_STOP_START         2
#define MAP_TYPE_PART_EDGE          3
#define MAP_TYPE_CURVE              4
#define MAP_TYPE_TURN_90            5

using namespace std;


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

@@ -60,12 +60,15 @@
#define ID_MS_SYS_SHUTDOWN      0x8010
#define ID_SM_LIGHT_EXAM_REQ    0x0012
#define ID_MS_LIGHT_EXAM_RES    0x8012
#define ID_SM_DISTANCE              0x0020


#define MA_OUT_GPS_BRIEF        0x0001
#define MA_OUT_RTK_BRIEF        0x0002
#define MA_OUT_CAR_BRIEF        0x0004
#define MA_OUT_RTCM_IND        0x0008
#define MA_OUT_DBG_INFO        0x0010


static int OnOff = 0;//0xFFFF;

@@ -352,6 +355,21 @@
    SendMsgToMainProcIndep(ID_SM_RTK_BRIEF, sb.GetString());
}

void MA_SendDistance(double l, double r)
{
    StringBuffer sb;
    Writer<StringBuffer> writer(sb);

    writer.StartObject();
    writer.Key("left");
    writer.Double(l);
    writer.Key("right");
    writer.Double(r);
    writer.EndObject();

    SendMsgToMainProcIndep(ID_SM_DISTANCE, sb.GetString());
}

void MA_SendExamWrong(vector<ExamFault> &ExamFaultList)
{
    StringBuffer sb;

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

@@ -97,4 +97,6 @@
void MA_EnterMap(int map_id, int type, int enter);
void MA_ExamLight(void);

void MA_SendDistance(double l, double r);

#endif //MYAPPLICATION2_COMM_IF_H

 lib/src/main/cpp/test_items/driving_curve.cpp

@@ -7,6 +7,8 @@
#include "../driver_test.h"
#include "../common/apptimer.h"
#include "../utils/xconvert.h"
#include "../master/comm_if.h"
#include "../native-lib.h"

#include <vector>
#include <cstdlib>
@@ -18,6 +20,7 @@
const uint32_t STOP_CAR_TIME = D_SEC(2);

static bool testing = false;
static int mapIndex = 0;
static uint32_t stopTimepoint = 0;

static bool reportStopCarTimeout;
@@ -32,13 +35,14 @@

static bool UpdateStartLine(struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect);
static bool UpdateEndLine(bool mode, struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect);
static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone);
static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone, int &who);

void StartDrivingCurve(int moveDirect, const struct RtkTime *rtkTime)
void StartDrivingCurve(int index, int moveDirect, const struct RtkTime *rtkTime)
{
    DEBUG("进入曲线行驶场地");

    testing = true;
    mapIndex = index;

    prevMoveDirect = moveDirect;
    if (moveDirect == 0) {
@@ -48,12 +52,14 @@
    crashRedLine = false;

    scanWindow.leftStart = scanWindow.leftEnd = scanWindow.rightStart = scanWindow.rightEnd = 0;

    MA_EnterMap(mapIndex, MAP_TYPE_CURVE, 1);
}

int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    Polygon tireRect;

    int who = 0;
    MakePolygon(&tireRect, {car->carXY[car->left_front_tire[TIRE_OUTSIDE]],
                            car->carXY[car->right_front_tire[TIRE_OUTSIDE]],
                            car->carXY[car->right_rear_tire[TIRE_OUTSIDE]],
@@ -71,12 +77,17 @@

    DEBUG("scanWindow leftStart %d leftEnd %d rightStart %d rightEnd %d", scanWindow.leftStart, scanWindow.leftEnd, scanWindow.rightStart, scanWindow.rightEnd);

    if (CrashRedLine(map, map2, car, &scanWindow)) {
    if (CrashRedLine(map, map2, car, &scanWindow, who)) {
        if (!crashRedLine) {
            crashRedLine = true;
            // 车轮压边线，不合格
            AddExamFault(27, rtkTime);
            DEBUG("车轮压边线");
            if (who == 1) {
                PlayTTS("压左曲线");
            } else if (who == 2) {
                PlayTTS("压右曲线");
            }
        }
    } else {
        crashRedLine = false;
@@ -105,6 +116,11 @@

TEST_END:
    CleanPolygon(&tireRect);

    if (!testing) {
        MA_EnterMap(mapIndex, MAP_TYPE_CURVE, 0);
    }

    return testing ? 1 : 0;
}

@@ -276,7 +292,7 @@
}

// 车轮是否压边线
static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone)
static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone, int &who)
{
    bool ret = false;

@@ -290,9 +306,11 @@
    for (int e = zone->leftStart - 1; e >= zone->leftEnd; --e) {
        MakeLine(&redLine, &map->point[s], &map->point[e]);
        if (IntersectionOf(redLine, frontTireAxial) != GM_None) {
            who = 1;
            return true;
        }
        if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
            who = 1;
            return true;
        }
        s = e;
@@ -302,9 +320,11 @@
    for (int e = zone->rightStart - 1; e >= zone->rightEnd; --e) {
        MakeLine(&redLine, &map2->point[s], &map2->point[e]);
        if (IntersectionOf(redLine, frontTireAxial) != GM_None) {
            who = 2;
            return true;
        }
        if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
            who = 2;
            return true;
        }
        s = e;

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

@@ -10,7 +10,7 @@

using namespace std;

void StartDrivingCurve(int moveDirect, const struct RtkTime *rtkTime);
void StartDrivingCurve(int index, int moveDirect, const struct RtkTime *rtkTime);
bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model *car);

int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime);

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

@@ -9,6 +9,7 @@
#include "../jni_log.h"
#include "../driver_test.h"
#include "../utils/xconvert.h"
#include "../master/comm_if.h"
#include <vector>
#include <cstdlib>

@@ -27,7 +28,8 @@

const int PARK_TIMEOUT = D_SEC(210);

static bool testing = false;
static bool testing = false, reverseCar = false;
static int mapIndex = 0;

const uint32_t STOP_CAR_TIME = D_SEC(2);
static uint32_t stopTimepoint;
@@ -44,13 +46,15 @@

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 bool CrashRedLine(const Polygon *map, const car_model *car);
static bool CrashRedLine(const Polygon *map, const car_model *car, int &who);
static bool ExitParkArea(const Polygon *map, const car_model *car);

void StartParkBottom(int moveDirect, const struct RtkTime *rtkTime)
void StartParkBottom(int index, int moveDirect, const struct RtkTime *rtkTime)
{
    DEBUG("StartParkBottom");
    testing = true;
    reverseCar = false;
    mapIndex = index;
    memset(carray, 0, sizeof(carray));
    memset(darray, 0, sizeof(darray));
    memset(parkStatus, 0, sizeof(parkStatus));
@@ -69,12 +73,13 @@
{
    char crossCtrlLine = 0;
    uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
    int who = 0;

    if (ExitParkArea(map, car)) {
        DEBUG("离开场地");
        // 离开场地
        testing = false;
        if ((parkStatus[0] != 1 || parkStatus[1] != 1) && !reportParkFail) {
        if ((parkStatus[0] != 1 || parkStatus[1] != 1) && !reportParkFail && reverseCar) {
            // 倒库不入，不合格
            reportParkFail = true;
            AddExamFault(8, rtkTime);
@@ -83,12 +88,18 @@
        goto TEST_END;
    }

    if (CrashRedLine(map, car)) {

    if (CrashRedLine(map, car, who)) {
        if (!occurCrashRedLine) {
            occurCrashRedLine = true;
            // 车身出线，不合格
            AddExamFault(7, rtkTime);
            DEBUG("车轮压线");
            if (who == 1) {
                PlayTTS("压左库位线");
            } else if (who == 2) {
                PlayTTS("压右库位线");
            }
        }
    } else {
        occurCrashRedLine = false;
@@ -153,6 +164,10 @@
                }
            } else if (moveDirect == -1) {
                // 切换为倒车
                if (!reverseCar) {
                    reverseCar = true;
                    MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 1);
                }
                if (darray[parkCount] == 0) {
                    if (!crossCtrlLineSw) {
                        // 倒车前，前轮未驶过控制线
@@ -160,7 +175,7 @@
                        DEBUG("倒车前，前轮未驶过控制线");
                    }
                    darray[parkCount] = 1;
                    firstReverseTimepoint = tp;
                    firstReverseTimepoint = tp;         // 首次倒车，才意味着此项目开始
                }
            } else {
                // 切换为前进
@@ -209,6 +224,9 @@
    }

TEST_END:
    if (!testing && reverseCar) {
        MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 0);
    }
    return testing ? 1 : 0;
}

@@ -258,7 +276,7 @@
    return succ;
}

static bool CrashRedLine(const Polygon *map, const car_model *car)
static bool CrashRedLine(const Polygon *map, const car_model *car, int &who)
{
    bool ret = false;

@@ -276,6 +294,12 @@
    for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
        MakeLine(&red_line, &(map->point[red_lines[i][0]]), &(map->point[red_lines[i][1]]));
        if (IntersectionOf(red_line, &car_body) != GM_None) {
            if (i == 2 || i == 1) {
                who = 1;
            } else if (i == 4 || i == 5) {
                who = 2;
            }

            ret = true;
            break;
        }

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

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

using namespace std;

void StartParkBottom(int moveStatus, const struct RtkTime *rtkTime);
void StartParkBottom(int index, 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

@@ -10,6 +10,7 @@
#include "../native-lib.h"
#include "../utils/xconvert.h"
#include "../test_common/car_sensor.h"
#include "../master/comm_if.h"

#include <vector>
#include <cstdlib>
@@ -21,7 +22,10 @@
const int PARK_TIMEOUT = D_SEC(90);
const uint32_t STOP_CAR_TIME = D_SEC(2);

static int mapIndex = 0;
static bool reportExamTimeout;
static bool reportParkFail;

static uint32_t stopTimepoint = 0;
static bool occurCrashRedLine1, occurCrashRedLine2;
static int prevMoveStatus, storeMoveStatusBeforeStop;
@@ -36,15 +40,16 @@
static bool ExitParkArea(const Polygon *map, const car_model *car);
static bool ExitParkArea2(const Polygon *map, const car_model *car);

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

    testing = true;
    mapIndex = index;

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

    reportParkFail = false;
    prevMoveStatus = moveStatus;
    parkSuccess = false;
    parkStatus = 0;
@@ -80,9 +85,10 @@
    }

    if (ExitParkArea(map, car) || ExitParkArea2(map, car)) {
        if (!parkSuccess || !occurMoveBack) {
        if (!parkSuccess && occurMoveBack && !reportParkFail) {
            // 直接驶离测试区，认为移库不入
            AddExamFault(21, rtkTime);
            reportParkFail = true;
            DEBUG("直接驶离测试区,移库不入");
        }
        testing = false;
@@ -130,9 +136,10 @@
                        }
                    }

                    if (!parkSuccess) {
                    if (!parkSuccess && !reportParkFail) {
                        // 停止后，车身出线，不合格
                        AddExamFault(21, rtkTime);
                        reportParkFail = true;
                        DEBUG("移库不入");
                    }

@@ -149,6 +156,7 @@
                DEBUG("开始倒车");
                occurMoveBack = true;
                moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
                MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 1);
            }
        }

@@ -165,7 +173,11 @@
            }
        }
    }

TEST_END:
    if (!testing && occurMoveBack) {
        MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 0);
    }
    return testing ? 1 : 0;
}


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

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

using namespace std;

void StartParkEdge(int moveStatus, const struct RtkTime *rtkTime);
void StartParkEdge(int index, int moveStatus, const struct RtkTime *rtkTime);
int TestParkEdge(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime);

#endif //RTKDRIVERTEST_PARK_EDGE_H

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

@@ -12,6 +12,7 @@
#include "../common/apptimer.h"
#include "../utils/xconvert.h"
#include "../test_common/car_sensor.h"
#include "../master/comm_if.h"

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

@@ -31,6 +32,7 @@

static PointF stopPoint;

static int mapIndex = 0;
static int prevMoveDirect;
static uint32_t stopTimepoint = 0;
static bool stopCar = false;
@@ -46,11 +48,11 @@
static double DistanceOfTire2Edge(const Polygon *map, const car_model *car);
static bool ExitTestArea(const Polygon *map, const car_model *car);

void StartSAS(int moveDirect, const struct RtkTime *rtkTime)
void StartSAS(int index, int moveDirect, const struct RtkTime *rtkTime)
{
    DEBUG("进入坡起项目");
    testing = true;

    mapIndex = index;
    prevMoveDirect = moveDirect;

    if (moveDirect == 0) {
@@ -63,6 +65,8 @@
    slideNormalDistance = false;
    reportSlideFault = false;
    reportStartTimeout = false;

    MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 1);
}

int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
@@ -81,6 +85,12 @@
    if (ExitTestArea(map, car)) {
        // 驶离测试区
        testing = false;
    }

    {
        double dis2 = DistanceOfTire2Edge(map, car);

        MA_SendDistance(-1, dis2);
    }

    if (prevMoveDirect != moveDirect) {
@@ -172,6 +182,10 @@
        }
    }

    if (!testing) {
        MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 0);
    }

    return testing ? 1 : 0;
}

@@ -245,10 +259,22 @@
// 整个车辆都要驶离该测试区域
static bool ExitTestArea(const Polygon *map, const car_model *car)
{
    // 在8->7线的右侧
    for (int i = 0; i < car->bodyNum; ++i) {
        if (IntersectionOfLine(map->point[8], map->point[7], car->carXY[car->body[i]]) != -1)
            return false;
    // 全车都需不在地图中
    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]];
    }
    return true;

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

    free(carBody.point);

    return ret;
}

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

@@ -9,7 +9,7 @@

using namespace std;

void StartSAS(int moveDirect, const struct RtkTime *rtkTime);
void StartSAS(int index, int moveDirect, const struct RtkTime *rtkTime);
int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime);

#endif //RTKDRIVERTEST_STOP_AND_START_H

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

@@ -10,6 +10,7 @@
#include "../utils/xconvert.h"
#include "../defs.h"
#include "../test_common/car_sensor.h"
#include "../master/comm_if.h"

#include <vector>
#include <cstdlib>
@@ -21,6 +22,7 @@
const uint32_t STOP_CAR_TIME = D_SEC(2);

static bool testing;
static int mapIndex;

static int enterAreaHeading;
static bool turnLeftFinished;
@@ -33,7 +35,7 @@
static bool CrashRedLine(const Polygon *map, const car_model *car);
static bool ExitTestArea(const Polygon *map, const car_model *car);

void StartTurnA90(int moveDirect, double heading, const struct RtkTime *rtkTime)
void StartTurnA90(int index, int moveDirect, double heading, const struct RtkTime *rtkTime)
{
    DEBUG("进入直角转弯场地");
    testing = true;
@@ -45,6 +47,9 @@
    reportStopCarTimeout = false;
    crashRedLine = false;
    turnLeftFinished = false;
    mapIndex = index;

    MA_EnterMap(mapIndex, MAP_TYPE_TURN_90, 1);
}

int TestTurnA90(const Polygon *map, const car_model *car, const car_model *carPrev, double heading, double speed, int moveDirect, const struct RtkTime *rtkTime)
@@ -118,6 +123,11 @@
    if (turnLeftFinished) {

    }

    if (!testing) {
        MA_EnterMap(mapIndex, MAP_TYPE_TURN_90, 0);
    }

TEST_END:
    return testing? 1:0;
}

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

@@ -10,7 +10,7 @@

using namespace std;

void StartTurnA90(int moveDirect, double heading, const struct RtkTime *rtkTime);
void StartTurnA90(int index, int moveDirect, double heading, const struct RtkTime *rtkTime);
int TestTurnA90(const Polygon *map, const car_model *car, const car_model *carPrev, double heading, double speed, int moveDirect, const struct RtkTime *rtkTime);

#endif //RTKDRIVERTEST_TURN_A90_H

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

@@ -7,6 +7,7 @@
#include "../native-lib.h"
#include "../jni_log.h"
#include "road_exam.h"
#include "../master/comm_if.h"
#include <cmath>

#define DEBUG(fmt, args...)     LOGD("<drive_straight> <%s>: " fmt, __func__, ##args)
@@ -43,6 +44,14 @@

    MakeLine(&road_edge, &RoadMapList[index].roadEdgeLine[0].point[0], &RoadMapList[index].roadEdgeLine[0].point[1]);

    {
        double l1 = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], road_edge);

        double l2 = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], road_edge);

        MA_SendDistance(-1, (l1+l2)/2.0);
    }

    if (!crossStartLine) {
        PointF p1, p2;