update

yy1717

2020-01-15 eced3d013f06b623a49cb9deaba42218c4e37bb8

update

 Documents/Ä£Äâ¼Ý¿¼APPÄÚ²¿Í¨Ñ¶.docx

Binary files differ

 app/src/main/assets/map.json

@@ -3,10 +3,7 @@
  "item": 1,
  "point": [{
    "x-y": [-11.9669, 28.013, -11.5114, 27.137, -10.8069, 27.5039, -10.2969, 26.4212, -9.2102, 26.8894, -9.6513, 28.0027, -8.9758, 28.3797, -9.5057, 29.3232]
  },
    {
      "x-y": [-11.9669, 28.013, -11.5114, 27.137, -10.8069, 27.5039, -10.2969, 26.4212, -9.2102, 26.8894, -9.6513, 28.0027, -8.9758, 28.3797, -9.5057, 29.3232]
    }
  }
  ]
},
  {
@@ -14,10 +11,7 @@
    "item": 2,
    "point": [{
      "x-y": [-11.9669, 28.013, -11.5114, 27.137, -10.8069, 27.5039, -10.2969, 26.4212, -9.2102, 26.8894, -9.6513, 28.0027, -8.9758, 28.3797, -9.5057, 29.3232]
    },
      {
        "x-y": [-11.9669, 28.013, -11.5114, 27.137, -10.8069, 27.5039, -10.2969, 26.4212, -9.2102, 26.8894, -9.6513, 28.0027, -8.9758, 28.3797, -9.5057, 29.3232]
      }
    }
    ]
  }
]

 lib/src/main/cpp/driver_test.cpp

@@ -57,10 +57,8 @@
int errs = 0;

vector<int> ErrorList;

static Polygon theParkEdgeMap;         // ä¾§ä½åœè½¦åœ°å›¾
static Polygon theTurn90Map;
static Polygon theSSMap;
vector<ExamFault> ExamFaultList;
static int examFaultIndex = 0;

static struct map_list {
    int id;
@@ -71,35 +69,8 @@

static int MapNum = 0;


#define CAR_COORD_STORE_SIZE        10

struct car_coord_ {
    uint32_t uptime;
    double azimuth;
    PointF coord;
};

struct car_desc_ {
    double distance;            // è·ç¦»ä¸»å¤©çº¿çš„距离
    double angle;               // ä»Žä¸­è½´çº¿é€†æ—¶é’ˆå½¢æˆçš„角度
};

struct car_desc_ *CarDesc = NULL;

typedef struct {
    PointF basePoint;
    int axial[2];
    int left_front_tire[2];
    int right_front_tire[2];
    int left_rear_tire[2];
    int right_rear_tire[2];
    int pointNum;
    struct car_desc_ *carDesc;
    PointF *carXY;
} car_model;

static car_model *CarModel = NULL;
static car_model *CarModelPrev = NULL;

#define MOV_AVG_SIZE                1
#define RTK_BUFFER_SIZE            100
@@ -110,15 +81,17 @@

static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b);
static void ReadDriverExamPrimerTimeout(union sigval sig);
static void UpdateCarBodyCoord(double azimuth, PointF main_ant, const car_model *carModel);
static void UpdateCarBodyCoord(double azimuth, PointF main_ant, car_model *carModel);
static bool UpdateCarCoord(double &spd, int &mov, int &idx);
static bool FrontTireEnterArea(const Polygon *car, const Polygon *map);

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

    RtkBuffer = (rtk_info *) malloc(RTK_BUFFER_SIZE * sizeof(rtk_info));
    RtkBufferNum = RtkBufferIn = 0;
@@ -140,6 +113,8 @@

void ClearMap(void)
{
    if (TestStart) return;

    for (int i = 0; i < MapNum; ++i) {
        if (MapList[i].map.point != NULL)
            free(MapList[i].map.point);
@@ -156,7 +131,7 @@

    AppTimer_delete(ReadDriverExamPrimerTimeout);

    if (map == NULL || pointNum == 0)
    if (map == NULL || pointNum == 0 || TestStart)
        return;

    MapList[MapNum].id = id;
@@ -187,13 +162,15 @@

void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
                    int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
                    double (*point)[2], int pointNum)
                    int *body, int bodyNum,double (*point)[2], int pointNum)
{
    DEBUG("SetCarMeasurePoint pointNum %d", pointNum);

    if (point == NULL || pointNum == 0) return;
    if (point == NULL || pointNum == 0 || TestStart) return;

    if (CarModel != NULL) {
        if (CarModel->body != NULL)
            free(CarModel->body);
        if (CarModel->carDesc != NULL)
            free(CarModel->carDesc);
        if (CarModel->carXY != NULL)
@@ -201,6 +178,23 @@
        free(CarModel);
        CarModel = NULL;
    }

    if (CarModelPrev != NULL) {
        if (CarModelPrev->body != NULL)
            free(CarModelPrev->body);
        if (CarModelPrev->carDesc != NULL)
            free(CarModelPrev->carDesc);
        if (CarModelPrev->carXY != NULL)
            free(CarModelPrev->carXY);
        free(CarModelPrev);
        CarModelPrev = NULL;
    }

    CarModelPrev = (car_model *)malloc(sizeof(car_model));
    CarModelPrev->body = NULL;
    CarModelPrev->carDesc = NULL;
    CarModelPrev->pointNum = 0;
    CarModelPrev->carXY = (PointF *) malloc(sizeof(PointF) * pointNum);

    CarModel = (car_model *)malloc(sizeof(car_model));
    CarModel->basePoint.X = basePoint[0];
@@ -215,8 +209,22 @@
    CarModel->left_rear_tire[1] = left_rear_tire[1];
    CarModel->right_rear_tire[0] = right_rear_tire[0];
    CarModel->right_rear_tire[1] = right_rear_tire[1];
    CarModel->pointNum = pointNum;

    CarModel->bodyNum = bodyNum;
    if (bodyNum == 0 || body == NULL) {
        CarModel->bodyNum = 6;
        CarModel->body = (int *) malloc(sizeof(int) * 6);
        for (int i = 0; i < 6; ++i) {
            CarModel->body[i] = i;
        }
    } else {
        CarModel->body = (int *) malloc(sizeof(int) * CarModel->bodyNum);
        for (int i = 0; i < CarModel->bodyNum; ++i) {
            CarModel->body[i] = body[i];
        }
    }

    CarModel->pointNum = pointNum;
    CarModel->carDesc = (struct car_desc_ *)malloc(sizeof(struct car_desc_) * pointNum);
    CarModel->carXY = (PointF *) malloc(sizeof(PointF) * pointNum);

@@ -264,8 +272,15 @@
        err = true;
        MA_SendExamStatus(0, -2);
    }
    if (!err)
    if (!err) {
        if (!TestStart) {
            ExamFaultList.clear();
            examFaultIndex = 0;

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

void UpdateRTKInfo(const rtk_info *s)
@@ -275,6 +290,8 @@
        RtkBufferIn = (RtkBufferIn + 1) % RTK_BUFFER_SIZE;
        if (RtkBufferNum < RTK_BUFFER_SIZE)
            RtkBufferNum++;
    } else {
        return;
    }

    double speed;
@@ -289,9 +306,9 @@
                RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);

        brief.qf = RtkBuffer[index].qf;
        brief.map_id = -1;
        brief.map_id = 863;
        brief.move = move;
        brief.speed = speed;
        brief.speed = speed * 3.6;
        brief.heading = RtkBuffer[index].heading;
        brief.main_ant[0] = RtkBuffer[index].x;
        brief.main_ant[1] = RtkBuffer[index].y;
@@ -307,6 +324,12 @@
        brief.right_rear_tire[0] = CarModel->right_rear_tire[0];
        brief.right_rear_tire[1] = CarModel->right_rear_tire[1];

        brief.bodyNum = CarModel->bodyNum;
        brief.body = (int *) malloc(sizeof(int) * CarModel->bodyNum);
        for (int i = 0; i < CarModel->bodyNum; ++i) {
            brief.body[i] = CarModel->body[i];
        }

        brief.pointNum = CarModel->pointNum;
        brief.point = (double *) malloc(CarModel->pointNum * 2 * sizeof(double));
        for (int i = 0, j = 0; i < CarModel->pointNum; ++i) {
@@ -315,6 +338,13 @@
        }

        MA_SendCarPosition(&brief);

        free(brief.body);
        free(brief.point);

        if (TestStart) {
//            TestParkBottom(ErrorList, 0, CarModel, CarModelPrev, speed, move);
        }
    }
}

@@ -345,6 +375,8 @@
    // å¦‚果一定的时间都没有有效定位，删除之前的值
    uint32_t tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2]);

    DEBUG("tmDiff = %ld, p1 = %d p2 = %d dss = %d dss2 = %d", tmDiff, p1, p2, RtkBuffer[p1].dss, RtkBuffer[p2].dss);

    if (tmDiff > D_SEC(5)) {
        if (p1 != 0)
            RtkBuffer[0] = RtkBuffer[p1];
@@ -357,7 +389,22 @@
    main_ant_coord.X = RtkBuffer[p1].x;
    main_ant_coord.Y = RtkBuffer[p1].y;

    // ä¿å­˜ä¸Šä¸€ä¸ªè½¦èº«è½®å»“点
    if (CarModelPrev->pointNum > 0) {
        CarModelPrev->pointNum = CarModel->pointNum;
        CarModelPrev->basePoint = CarModel->basePoint;
        for (int i = 0; i < CarModel->pointNum; ++i) {
            CarModelPrev->carXY[i] = CarModel->carXY[i];
        }
    }
    UpdateCarBodyCoord(RtkBuffer[p1].heading, main_ant_coord, CarModel);
    if (CarModelPrev->pointNum == 0) {
        CarModelPrev->pointNum = CarModel->pointNum;
        CarModelPrev->basePoint = CarModel->basePoint;
        for (int i = 0; i < CarModel->pointNum; ++i) {
            CarModelPrev->carXY[i] = CarModel->carXY[i];
        }
    }

    // è®¡ç®—速度(ç±³/秒)、前进后退
    double speed = sqrt(pow(RtkBuffer[p1].x - RtkBuffer[p2].x, 2) + pow(RtkBuffer[p1].y - RtkBuffer[p2].y, 2)) * 1000 /
@@ -505,6 +552,21 @@
        }*/
}

void AddExamFault(int wrong, const char *utc)
{
    if (!TestStart) return;

    struct ExamFault fault;

    fault.sn = examFaultIndex++;
    strcpy(fault.utc, utc);
    fault.wrong_id = wrong;

    ExamFaultList.push_back(fault);

    MA_SendExamWrong(ExamFaultList);
}

car_model_cache_t *GetCarModelCache(int node)
{
    return NULL;
@@ -515,8 +577,10 @@
 * @param azimuth
 * @param coord
 */
static void UpdateCarBodyCoord(double azimuth, PointF main_ant, const car_model *carModel)
static void UpdateCarBodyCoord(double azimuth, PointF main_ant, car_model *carModel)
{
    carModel->basePoint = main_ant;

    for (int i = 0; i < carModel->pointNum; ++i) {
        double tx = main_ant.X + carModel->carDesc[i].distance*sin(toRadians(azimuth));
        double ty = main_ant.Y + carModel->carDesc[i].distance*cos(toRadians(azimuth));

 lib/src/main/cpp/driver_test.h

@@ -8,8 +8,31 @@
#include "Geometry.h"
#include "rtk_module/rtk.h"

#include <vector>

#define TIRE_OUTSIDE    0
#define TIRE_INSIDE     1

using namespace std;

struct car_desc_ {
    double distance;            // è·ç¦»ä¸»å¤©çº¿çš„距离
    double angle;               // ä»Žä¸­è½´çº¿é€†æ—¶é’ˆå½¢æˆçš„角度
};

typedef struct {
    PointF basePoint;               // çŽ°åœºæµ‹é‡çš„主天线坐标
    int axial[2];
    int left_front_tire[2];
    int right_front_tire[2];
    int left_rear_tire[2];
    int right_rear_tire[2];
    int bodyNum;
    int *body;
    int pointNum;
    struct car_desc_ *carDesc;      // ç»çŽ°åœºæµ‹é‡ç‚¹è®¡ç®—后的值
    PointF *carXY;                  // è®¡ç®—后的车身各点坐标
} car_model;

// æ€»æ•°
// ä¸­è½´çº¿å‰ç‚¹,id 0，中轴线后点id
@@ -31,17 +54,26 @@
    carModelDesc_t *desc;
} car_model_cache_t;

struct ExamFault {
    int sn;
    char utc[32];
    int wrong_id;
};

//vector<ExamFault> ExamFaultList;

void DriverTestInit(void);
void ReadDriverExamPrimer(void);
void ClearMap(void);
void AddMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2);
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
                        int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
                        double (*point)[2], int pointNum);
                        int *body, int bodyNum, double (*point)[2], int pointNum);
void SetSensorCfg(int (*sensor)[2], int sensorNum);
void StartDriverExam(int start);

void UpdateRTKInfo(const rtk_info *s);
void AddExamFault(int wrong, const char *utc);

car_model_cache_t *GetCarModelCache(int node);

#endif //RTKDRIVERTEST_DRIVER_TEST_H

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

@@ -12,10 +12,14 @@
#include "../utils/xconvert.h"
#include "../driver_test.h"
#include "../defs.h"
#include "../common/apptimer.h"
#include "../jni_log.h"

#include <string>
#include <iostream>
#include <vector>

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

using namespace rapidjson;
using namespace std;
@@ -207,9 +211,36 @@
    SendMsgToMainProc(ID_SM_RTK_BRIEF, sb.GetString());
}

void MA_SendExamWrong(vector<int>&err)
void MA_SendExamWrong(vector<ExamFault> &ExamFaultList)
{
    StringBuffer sb;
    Writer<StringBuffer> writer(sb);

    writer.StartArray();

    for (int i = 0; i < 3; ++i) {
        writer.StartObject();               // Between StartObject()/EndObject(),
        writer.Key("wrong_id");                // output a key,
        writer.Int(i+1);
        writer.EndObject();
    }

    for (vector<ExamFault>::iterator iter = ExamFaultList.begin(); iter != ExamFaultList.end(); ++iter) {
        writer.StartObject();

        writer.Key("sn");
        writer.Int(iter->sn);
        writer.Key("utc");
        writer.String(iter->utc);
        writer.Key("wrong_id");
        writer.Int(iter->wrong_id);

        writer.EndObject();
    }

    writer.EndArray();

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

void MA_SendCarPosition(const struct carBrief *brief)
@@ -270,6 +301,13 @@
    writer.Int(brief->right_rear_tire[1]);
    writer.EndArray();

    writer.Key("body");
    writer.StartArray();
    for (int i = 0; i < brief->bodyNum; ++i) {
        writer.Int(brief->body[i]);
    }
    writer.EndArray();

    writer.Key("point");
    writer.StartArray();
    for (int i = 0; i < brief->pointNum; ++i) {
@@ -279,7 +317,9 @@
    writer.EndArray();
    writer.EndObject();

    uint32_t a = AppTimer_GetTickCount();
    SendMsgToMainProc(ID_SM_CAR, sb.GetString());
    DEBUG("耗时 %ld", AppTimer_GetTickCount() - a);
}

void MA_MainProcMsgEntry(int cmd, const char *value)
@@ -421,6 +461,8 @@
            if (!doc.HasParseError()) {
                double basePoint[2];
                int axial[2], left_front_tire[2], right_front_tire[2], left_rear_tire[2], right_rear_tire[2];
                int bodyNum = 0;
                int *body = NULL;
                int pointNum = 0;
                double (*point)[2] = NULL;

@@ -484,6 +526,18 @@
                    }
                }

                if (doc.HasMember("body")) {
                    const Value& s = doc["body"];
                    if (s.IsArray()) {
                        int i = 0;
                        bodyNum = s.Size();
                        body = new int[bodyNum];
                        for(Value::ConstValueIterator itr = s.Begin(); itr != s.End(); ++itr) {
                            body[i++] = itr->GetInt();
                        }
                    }
                }

                if (doc.HasMember("point")) {
                    const Value& s = doc["point"];
                    if (s.IsArray()) {
@@ -503,8 +557,9 @@
                }

                SetCarMeasurePoint(basePoint, axial, left_front_tire, right_front_tire,
                        left_rear_tire, right_rear_tire, point, pointNum);
                        left_rear_tire, right_rear_tire, body, bodyNum, point, pointNum);

                if (body != NULL) delete []body;
                if (point != NULL) delete []point;
            }
            break;

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

@@ -6,6 +6,7 @@
#define MYAPPLICATION2_COMM_IF_H

#include <cstdint>
#include "../driver_test.h"

struct gpsBrief {
    char utc[32];
@@ -42,6 +43,8 @@
    int right_front_tire[2];
    int left_rear_tire[2];
    int right_rear_tire[2];
    int *body;
    int bodyNum;
    double *point;
    int pointNum;
};
@@ -60,6 +63,6 @@
void MA_SendGpsBrief(const struct gpsBrief *brief);
void MA_SendRtkBrief(const struct rtkBrief *brief);
void MA_SendCarPosition(const struct carBrief *brief);

void MA_SendExamWrong(vector<ExamFault> &ExamFaultList);

#endif //MYAPPLICATION2_COMM_IF_H

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

@@ -182,7 +182,7 @@
        int ul = ReadSerialPort(RTK_MODULE_UART, (uint8_t *)RxBuf + RxBufLen, sizeof(RxBuf) - RxBufLen);
        RxBufLen += ul;

        {
        /*{
            static char buffd[16384];

            buffd[0] = 0;
@@ -199,7 +199,7 @@
            }
            if (strlen(buffd) > 0)
                DEBUG("%s <- %s", "UART", buffd);
        }
        }*/

        if (RxBufLen > 0) {
            const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
@@ -400,6 +400,11 @@

//    CurrRTKInfo.heading = 60;

    static double deg = 0;
    CurrRTKInfo.heading = deg;

    deg += 2.0;

    if (CurrRTKInfo.hh == hh && CurrRTKInfo.mm == mm && CurrRTKInfo.ss == ss && CurrRTKInfo.dss == dss) {
        PlatformStatusChanged(RTK_UPDATE_EVT, (uint8_t *)&CurrRTKInfo, sizeof(CurrRTKInfo));
//        UpdateRTKInfo(&CurrRTKInfo);

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

@@ -152,10 +152,6 @@

void PlatformStatusChanged(uint32_t event, const uint8_t *data, int length)
{
    pthread_mutex_lock(&events_mutex);
    eventMask |= event;
    pthread_mutex_unlock(&events_mutex);

    if (data != NULL && length > 0) {
        int x = BitCount(event-1);

@@ -163,7 +159,13 @@
        eventData[x] = malloc(length);
        memcpy(eventData[x], data, length);
        eventDataLength[x] = length;

//        DEBUG("ADD %d len %d", x, length);
    }

    pthread_mutex_lock(&events_mutex);
    eventMask |= event;
    pthread_mutex_unlock(&events_mutex);

    sem_post(&sem_status_changed);
}
@@ -208,6 +210,9 @@

        while (events > 0) {
            int x = BitCount(((events - 1) ^ events) - 1);

//            DEBUG("FETCH %d len %d", x, eventDataLength[x]);

            PlatformChangeEntry(BV(x), (uint8_t *)eventData[x], eventDataLength[x]);
            if (eventData[x] != NULL) {
                free(eventData[x]);
@@ -279,6 +284,7 @@
        MA_RtkPlatformLogin(data[0]);
    }
    if (events & GPS_UPDATE_EVT) {
        DEBUG("GPS_UPDATE_EVT length %d", length);
        const gpsStatus_t *gps = (gpsStatus_t *)data;
        struct gpsBrief brief;

@@ -294,6 +300,7 @@
        RequestRtkDownload(gps, 1);
    }
    if (events & RTK_UPDATE_EVT) {
        DEBUG("RTK_UPDATE_EVT length %d", length);
        const rtk_info *rtk = (rtk_info *)data;
        struct rtkBrief brief;


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

@@ -40,10 +40,10 @@

static void StopCarTimeout(union sigval sig);
static void LeaveTestAreaLongtime(union sigval sig);
static void CrossCtrlLine(const Polygon *map, const car_model_cache_t *car);
static bool EnterParking(const Polygon *map, const car_model_cache_t *car);
static void 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_cache_t *car);
static bool CrashRedLine(const Polygon *map, const car_model *car);

void StartParkBottom(void)
{
@@ -56,7 +56,7 @@
    currTarget = FIRST_TOUCH_CTRL_LINE;
    leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;

    TextOsd(0, "ParkBottom");
    DEBUG("StartParkBottom");
}

void StopParkBottom(void)
@@ -67,10 +67,10 @@
    AppTimer_delete(LeaveTestAreaLongtime);
    currTarget = NONE;

    TextOsd(0, "ParkBottom End");
    DEBUG("ParkBottom End");
}

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

@@ -84,6 +84,8 @@
        if (parkTimeout) {
            // ä¸åˆæ ¼ï¼šåŠ¨ä½œè¶…æ—¶
            err.push_back(10);


            status = -1;
        }
        // æ˜¯å¦åŽ‹çº¿
@@ -94,8 +96,8 @@
        }

        if (trigLeaveTestAreaDetect) {
            if (IntersectionOf(car->points[car->desc->front_left_tire[TIRE_OUTSIDE]], map) == GM_Containment &&
                IntersectionOf(car->points[car->desc->front_right_tire[TIRE_OUTSIDE]], map) == GM_Containment) {
            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);
            }
@@ -114,29 +116,27 @@
            if (!((leftTireCrossLeftLine && rightTireCrossLeftLine) ||
                    (leftTireCrossRightLine && rightTireCrossRightLine))) {
                DEBUG("CrossCtrlLine");
                CrossCtrlLine(map, car);
                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;
                        TextOsd(0, "来道场玩的");
                        DEBUG("来道场玩的");
                    } else {
                        // ä¸åˆæ ¼ï¼šæœªæŒ‰è§„定线路行驶（直接跑出测试区了）
                        err.push_back(6);
                        status = -1;
                        TextOsd(0, "直接跑出测试区了");
                        DEBUG("直接跑出测试区了");
                    }
                } else if (!trigLeaveTestAreaDetect) {
                    Polygon car_body;

                    car_body.num = car->desc->body_num;
                    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->points[car->desc->body[i]];
                        car_body.point[i] = car->carXY[car->body[i]];
                    }

                    if (IntersectionOf(map, &car_body) == GM_None) {
@@ -196,12 +196,12 @@
                parkTimeout = false;
                leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;

                TextOsd(0, "第一次倒库");
                DEBUG("第一次倒库");
            } else if (the_ctrl_line_crossed > 0) {
                currTarget = SECOND_PARK;
                carParkSuccess = false;
                leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;
                TextOsd(0, "第二次倒库");
                DEBUG("第二次倒库");
            } else if (currTarget != THIRD_TOUCH_CTRL_LINE) {
                // ä¸åˆæ ¼ï¼šå€’车前，2前轮没驶过控制线
                err.push_back(9);
@@ -243,10 +243,10 @@
                    status = -1;
                } else if (currTarget == FIRST_PARK) {
                    currTarget = SECOND_TOUCH_CTRL_LINE;
                    TextOsd(0, "过另一根控制线");
                    DEBUG("过另一根控制线");
                } else {
                    currTarget = THIRD_TOUCH_CTRL_LINE;
                    TextOsd(0, "再过第一根控制线");
                    DEBUG("再过第一根控制线");
                }
            }
            carStopEvent = false;
@@ -279,25 +279,21 @@
}

// æ£€æµ‹2前轮是否正向越过左右控制线
static void CrossCtrlLine(const Polygon *map, const car_model_cache_t *car)
static void CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car)
{
    Line leftCtrlLine, rightCtrlLine;
    Line track1;
    PointF p1, p2;

    car_model_cache_t *prev_car = GetCarModelCache(1);
    if (prev_car == NULL)
        return;

    MakeLine(&leftCtrlLine, &map->point[0], &map->point[1]);
    MakeLine(&rightCtrlLine, &map->point[6], &map->point[7]);

    // å·¦å‰è½®ï¼Œå–轮宽的中点
    p1.X = (car->points[car->desc->front_left_tire[TIRE_OUTSIDE]].X + car->points[car->desc->front_left_tire[TIRE_INSIDE]].X) / 2;
    p1.Y = (car->points[car->desc->front_left_tire[TIRE_OUTSIDE]].Y + car->points[car->desc->front_left_tire[TIRE_INSIDE]].Y) / 2;
    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->points[prev_car->desc->front_left_tire[TIRE_OUTSIDE]].X + prev_car->points[prev_car->desc->front_left_tire[TIRE_INSIDE]].X) / 2;
    p2.Y = (prev_car->points[prev_car->desc->front_left_tire[TIRE_OUTSIDE]].Y + prev_car->points[prev_car->desc->front_left_tire[TIRE_INSIDE]].Y) / 2;
    p2.X = (prev_car->carXY[prev_car->left_front_tire[TIRE_OUTSIDE]].X + prev_car->carXY[prev_car->left_front_tire[TIRE_INSIDE]].X) / 2;
    p2.Y = (prev_car->carXY[prev_car->left_front_tire[TIRE_OUTSIDE]].Y + prev_car->carXY[prev_car->left_front_tire[TIRE_INSIDE]].Y) / 2;

    MakeLine(&track1, &p1, &p2);
    
@@ -312,11 +308,11 @@
    }

    // å³å‰è½®
    p1.X = (car->points[car->desc->front_right_tire[TIRE_OUTSIDE]].X + car->points[car->desc->front_right_tire[TIRE_INSIDE]].X) / 2;
    p1.Y = (car->points[car->desc->front_right_tire[TIRE_OUTSIDE]].Y + car->points[car->desc->front_right_tire[TIRE_INSIDE]].Y) / 2;
    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->points[prev_car->desc->front_right_tire[TIRE_OUTSIDE]].X + prev_car->points[prev_car->desc->front_right_tire[TIRE_INSIDE]].X) / 2;
    p2.Y = (prev_car->points[prev_car->desc->front_right_tire[TIRE_OUTSIDE]].Y + prev_car->points[prev_car->desc->front_right_tire[TIRE_INSIDE]].Y) / 2;
    p2.X = (prev_car->carXY[prev_car->right_front_tire[TIRE_OUTSIDE]].X + prev_car->carXY[prev_car->right_front_tire[TIRE_INSIDE]].X) / 2;
    p2.Y = (prev_car->carXY[prev_car->right_front_tire[TIRE_OUTSIDE]].Y + prev_car->carXY[prev_car->right_front_tire[TIRE_INSIDE]].Y) / 2;

    MakeLine(&track1, &p1, &p2);

@@ -330,16 +326,16 @@
    }
}

static bool EnterParking(const Polygon *map, const car_model_cache_t *car) {
static bool EnterParking(const Polygon *map, const car_model *car) {
    bool succ = false;

    Polygon parking;
    Polygon car_body;

    car_body.num = car->desc->body_num;
    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->points[car->desc->body[i]];
        car_body.point[i] = car->carXY[car->body[i]];
    }

    MakePolygon(&parking, {map->point[2], map->point[3], map->point[4], map->point[5]});
@@ -354,7 +350,7 @@
    return succ;
}

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

@@ -363,10 +359,10 @@

    Polygon car_body;

    car_body.num = car->desc->body_num;
    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->points[car->desc->body[i]];
        car_body.point[i] = car->carXY[car->body[i]];
    }

    for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {

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

@@ -13,6 +13,6 @@

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

#endif //RTKDRIVERTEST_PARK_BOTTOM_H