修改了上坡起步。

fctom1215

2020-02-11 3ce7d9cbccabf7f94d8203a98796599cd9dd5411

修改了上坡起步。

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

@@ -14,6 +14,7 @@
#include "../defs.h"
#include "../common/apptimer.h"
#include "../jni_log.h"
#include "../mcu/mcu_if.h"

#include <string>
#include <iostream>
@@ -47,6 +48,7 @@
#define ID_SM_CAR               0x000E
#define ID_SM_RTCM_IND           0x000F
#define ID_SM_DEBUG_INFO        0x0010
#define ID_MS_FILE              0x8100

#define MA_OUT_GPS_BRIEF        0x0001
#define MA_OUT_RTK_BRIEF        0x0002
@@ -660,3 +662,15 @@
        default:break;
    }
}

void MA_MainProcBinMsgEntry(int cmd, const uint8_t *value, int length)
{
    switch (cmd) {
        case ID_MS_FILE: {
            UploadDfuFile(value, length);
            break;
        }
        default:
            break;
    }
}

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

@@ -50,6 +50,7 @@
};

void MA_MainProcMsgEntry(int cmd, const char *value);
void MA_MainProcBinMsgEntry(int cmd, const uint8_t *value, int length);

void MA_NdkStart(void);
void MA_ReqRtkPlatformConfig(void);

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

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

#include <cstring>
#include <pthread.h>
#include <cstdlib>
#include "mcu_if.h"
#include "../common/apptimer.h"
#include "../utils/crc16.h"
@@ -29,8 +30,8 @@

#define ID_CM_APP_BOOT          0x0001
#define ID_MC_MCU_BOOT          0x8001
#define ID_CM_INQ_MCU_VER       0x0002
#define ID_MC_MCU_VER           0x8002
#define ID_CM_DFU_UPLOAD       0x0002
#define ID_MC_DFU_RSP           0x8002
#define ID_CM_RW_INFO           0x0003
#define ID_MC_RW_INFO_RSP       0x8003
#define ID_CM_MCU_DFU_REQ       0x0004
@@ -54,12 +55,27 @@
    uint16_t crc16;
}McuPkt;

static uint8_t *dfuFile = NULL;
static uint8_t dfuFileBitmap[128];

static int dfuFileLength = 0;
static int dfuTryCnt = 0;
const int DFU_MAX_TRY = 3;
const int DFU_FILE_BLOCK_SIZE = 896;

static void *UartThread1(void *p);
static void ParseMcuTimeout(union sigval sig);
static void McuCommandEntry(uint16_t id, const uint8_t *data, int lenth);

static void SendDfuFile(int fileLen, int sentLen, int blockLen, const uint8_t *data);
static void GoNextDfuLater(union sigval sig);
static void GoNextDfu(void);

void ParseMcuInit(void)
{
    dfuFile = NULL;
    dfuFileLength = 0;

    parse_status = SYNC_HEAD_ONE;
    AppTimer_delete(ParseMcuTimeout);

@@ -284,8 +300,54 @@
        case ID_MC_MCU_BOOT:
            DEBUG("MCU BOOT");
            break;
        case ID_MC_MCU_VER:
            DEBUG("MCU VER");
        case ID_MC_DFU_RSP:
            DEBUG("ID_MC_DFU_RSP %d len %d", data[0], lenth);

            if (data[0] == 0) {
                // 第一包传输成功
                dfuFileBitmap[0] |= 0x01;
            } else if (data[0] == 10) {
                // 升级完成
                memset(dfuFileBitmap, 0xFF, sizeof(dfuFileBitmap));
            } else if (data[0] == 11) {
                // 放弃传输
                UploadDfuFileEnd();
            } else if (data[0] == 12) {
                // 全部重传
                memset(dfuFileBitmap, 0, sizeof(dfuFileBitmap));
            } else if (data[0] == 13) {
                // 部分重传，有后续字段
                DEBUG("BITMAP %02X %02X %02X %02X %02X", data[1], data[2], data[3], data[4], data[5]);

                int total = dfuFileLength / DFU_FILE_BLOCK_SIZE + ((dfuFileLength % DFU_FILE_BLOCK_SIZE)?1:0);
                int a = 0, b = 0;
                for (int i = 1; i < lenth; ++i) {
                    for (int j = 0; j < 8; ++j) {
                        if ((data[i] & BV(j))) b++;
                        a++;
                        if (a == total) {
                            i = lenth;
                            break;
                        }
                    }
                }

                DEBUG("BITMAP total %d succ %d", total, b);

                //memset(dfuFileBitmap, 0, sizeof(dfuFileBitmap));
                memcpy(dfuFileBitmap, data + 1, lenth - 1);

                if (total % 8) {
                    dfuFileBitmap[total/8] &= ~BV((total%8) - 1);
                } else {
                    dfuFileBitmap[(total+7)/8 - 1] &= ~BV(7);
                }
//                dfuFileBitmap[total/8] &= ~BV(total%8);
            }

            dfuTryCnt = 0;
            AppTimer_delete(GoNextDfuLater);
            GoNextDfu();
            break;
        case ID_MC_RW_INFO_RSP:
            break;
@@ -293,9 +355,16 @@
            break;
        case ID_MC_MCU_DFU_DATA_RSP:
            break;
        case ID_MC_CAR_INFO:
            DEBUG("ID_MC_CAR_INFO");
        case ID_MC_CAR_INFO: {
            DEBUG("ID_MC_CAR_INFO %d", lenth);
            char ver[64] = {0};

            memcpy(ver, data, 32);

            DEBUG("ID_MC_CAR_INFO %s", ver);

            break;
        }
        case ID_MC_RTK_DATA:
            DEBUG("ID_MC_RTK_DATA");
            break;
@@ -306,3 +375,113 @@
            break;
    }
}

static void SendDfuFile(int fileLen, int sentLen, int blockLen, const uint8_t *data) {
    uint8_t buffer[1024];
    int x = 0;

    DEBUG("SendDfuFile fileLen %d sentLen %d blockLen %d", fileLen, sentLen, blockLen);

    buffer[x++] = BREAK_UINT32(fileLen, 3);
    buffer[x++] = BREAK_UINT32(fileLen, 2);
    buffer[x++] = BREAK_UINT32(fileLen, 1);
    buffer[x++] = BREAK_UINT32(fileLen, 0);

    buffer[x++] = BREAK_UINT32(sentLen, 3);
    buffer[x++] = BREAK_UINT32(sentLen, 2);
    buffer[x++] = BREAK_UINT32(sentLen, 1);
    buffer[x++] = BREAK_UINT32(sentLen, 0);

    buffer[x++] = HI_UINT16(blockLen);
    buffer[x++] = LO_UINT16(blockLen);

    memcpy(buffer + x, data, blockLen);
    x += blockLen;

    SendMcuCommand(ID_CM_DFU_UPLOAD, buffer, x);
}

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

    GoNextDfu();
}

static void GoNextDfu(void)
{
    int dfuFileSent = 0, currDfuBlockLength = 0;

    if (dfuFileLength == 0 || dfuFile == NULL)
        return;

    int row = 0, col = 0;

    dfuFileSent = dfuFileLength;

    for (row = 0; row < sizeof(dfuFileBitmap); ++row) {
        for (col = 0; col < 8; ++col) {
            if ((dfuFileBitmap[row] & BV(col)) == 0) {
                DEBUG("ROW = %d COL = %d", row, col);
                dfuFileSent = row * DFU_FILE_BLOCK_SIZE * 8 + col * DFU_FILE_BLOCK_SIZE;
                goto GET_FILE_START;
            }
        }
    }

GET_FILE_START:
    currDfuBlockLength = (dfuFileLength - dfuFileSent > DFU_FILE_BLOCK_SIZE) ? DFU_FILE_BLOCK_SIZE : (dfuFileLength - dfuFileSent);

    if (dfuFileSent >= dfuFileLength || currDfuBlockLength == 0) {
        UploadDfuFileEnd();
        return;
    }

    SendDfuFile(dfuFileLength, dfuFileSent, currDfuBlockLength, dfuFile + dfuFileSent);

    if (dfuFileSent > 0 && dfuFileSent + currDfuBlockLength < dfuFileLength) {
        dfuFileBitmap[row] |= BV(col);
    }

    if (dfuFileSent == 0 || dfuFileSent + currDfuBlockLength == dfuFileLength) {
        dfuTryCnt++;
        if (dfuTryCnt > DFU_MAX_TRY) {
            UploadDfuFileEnd();
            return;
        }
        DEBUG("GoNextDfuLater 3 sec...");
        AppTimer_add(GoNextDfuLater, D_SEC(3));
    } else {
        AppTimer_add(GoNextDfuLater, 10);
    }
}

void UploadDfuFileEnd(void)
{
    DEBUG("UploadDfuFileEnd");

    if (dfuFile != NULL)
        free(dfuFile);
    dfuFileLength = 0;

    AppTimer_delete(GoNextDfuLater);
}

void UploadDfuFile(const uint8_t *file, int length)
{
    DEBUG("UploadDfuFile %ld", length);

    if (length > 0) {
        if (dfuFile != NULL)
            free(dfuFile);

        dfuTryCnt = 0;

        dfuFile = (uint8_t *) malloc(length);
        dfuFileLength = length;
        memcpy(dfuFile, file, length);
        memset(dfuFileBitmap, 0, sizeof(dfuFileBitmap));
        AppTimer_delete(GoNextDfuLater);

        GoNextDfu();
    }
}

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

@@ -13,4 +13,7 @@
void ParseMcu(const uint8_t *data, int length);
void SendMcuCommand(uint16_t id, const uint8_t *data, int length);

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

#endif //RTKDRIVERTEST_MCU_IF_H

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

@@ -266,3 +266,16 @@
        MA_MainProcMsgEntry(cmd, NULL);
    }
}

extern "C"
JNIEXPORT void JNICALL
Java_com_anyun_exam_lib_RemoteService_MainProcBinMsgEntry(JNIEnv *env, jobject thiz, jint cmd,
                                                          jbyteArray data, jint length) {
    // TODO: implement MainProcBinMsgEntry()
    jbyte *c_dat = env->GetByteArrayElements(data, NULL);
    int len = env->GetArrayLength(data);

    MA_MainProcBinMsgEntry(cmd, (uint8_t *)c_dat, len);

    env->ReleaseByteArrayElements(data, c_dat, NULL);
}

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

@@ -98,15 +98,18 @@

    if (moveStatus != prevMoveStatus) {
        if (moveStatus == 0) {
            DEBUG("停车了");
            parkStatus = 0;
            stopTimepoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
            stopTimepoint = stopTimepoint * 1000 + rtkTime->mss*10;
            storeMoveStatusBeforeStop = prevMoveStatus;

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

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

            if (moveStatus == storeMoveStatusBeforeStop) {
                // 同方向再启动，继续判断是否停车超时
@@ -127,6 +130,13 @@
                        AddExamFault(21, rtkTime);
                        DEBUG("移库不入");
                        status = -1;
                    }

                    // 在这里检查转向灯状态
                    if (true) {
                        // 不开转向灯，扣10分
                        AddExamFault(25, rtkTime);
                        DEBUG("未开启转向灯");
                    }
                }
            }
@@ -232,7 +242,6 @@
    CleanPolygon(&parking);
    free(car_body.point);

    DEBUG("检查停车到位结束");
    return succ;
}


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

@@ -10,6 +10,9 @@
#include "../driver_test.h"
#include "../jni_log.h"
#include "../common/apptimer.h"
#include "../utils/xconvert.h"

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

using namespace std;

@@ -24,138 +27,166 @@
const double EDGE_DISTANCE_THRESHOLD_YELLOW = 0.3;
const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;
const int CAR_START_TIMEOUT = 30;
const uint32_t CAR_START_TIMEOUT = D_SEC(30);
const uint32_t STOP_CAR_TIME = D_SEC(2);

const double EPSILON = 1e-3;

static bool SASTesting = false;

static double slideDistance;
static bool startCarTimeout;
static int currTarget;
static PointF stopPoint;
static int startCarConfirm;             // 起步时，持续前进一小段才算

static void StartCarTimeout(union sigval sig);
static bool CrashRedLine(const Polygon *map, const car_model_cache_t *car);
static double DistanceOfHead2Stopline(const Polygon *map, const car_model_cache_t *car);
static double DistanceOfTire2Edge(const Polygon *map, const car_model_cache_t *car);
static bool ExitTestArea(const Polygon *map, const car_model_cache_t *car);
static int prevMoveDirect;
static uint64_t stopTimepoint = 0;
static bool stopCar = false;
static uint32_t stopCarTime;
static bool occurCrashRedLine = false;
static bool slideLongDistance = false;
static bool slideNormalDistance = false;
static bool reportSlideFault = false;
static bool reportStartTimeout = false;

void StartSAS(void)
static bool CrashRedLine(const Polygon *map, const car_model *car);
static double DistanceOfHead2Stopline(const Polygon *map, const car_model *car);
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)
{
    DEBUG("进入坡起项目");
    SASTesting = true;
    slideDistance = 0.0;
    startCarTimeout = false;
    currTarget = STOP_CAR;

    prevMoveDirect = moveDirect;

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

    occurCrashRedLine = false;
    stopCar = false;
    slideLongDistance = false;
    slideNormalDistance = false;
    reportSlideFault = false;
    reportStartTimeout = false;
}

void StopSAS(void)
{
    SASTesting = false;
    AppTimer_delete(StartCarTimeout);
}

int TestSAS(vector<int>&err, const Polygon *map, const car_model_cache_t *car, double speed, int run_status)
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 (!SASTesting)
        return -2;

    if (currTarget >= STOP_CAR) {
        if (CrashRedLine(map, car)) {
            // 车轮压线
            err.push_back(13);
            status = -1;
    if (CrashRedLine(map, car)) {
        // 车轮压线，不合格
        if (!occurCrashRedLine) {
            AddExamFault(13, rtkTime);
            DEBUG("车轮压线");
        }
        occurCrashRedLine = true;
    } else {
        occurCrashRedLine = false;
    }

    if (currTarget == STOP_CAR) {
        if (run_status == 0) {
    if (ExitTestArea(map, car)) {
        // 驶离测试区
        status = 1;
    }

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

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

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

            double dis1 = DistanceOfHead2Stopline(map, car);
            double dis2 = DistanceOfTire2Edge(map, car);

            if (dis1 > STOP_DISTANCE_THRESHOLD_RED) {
                // 距离停止线前后超出50厘米
                err.push_back(12);
                AddExamFault(12, rtkTime);
                DEBUG("距离停止线前后超出50厘米，不合格");
                status = -1;
            } else if (fabs(dis1) > EPSILON) {
                // 前保险没有位于停止带内，但没有超出50厘米
                err.push_back(17);
                // 前保险没有位于停止带内，但没有超出50厘米，扣10分
                AddExamFault(17, rtkTime);
                DEBUG("前保险没有位于停止带内，但没有超出50厘米");
            }

            if (dis2 > EDGE_DISTANCE_THRESHOLD_RED) {
                // 距离边线超出50厘米
                err.push_back(14);
                // 距离边线超出50厘米,不合格
                AddExamFault(14, rtkTime);
                DEBUG("距离边线超出50厘米");
                status = -1;
            } else if (dis2 > EDGE_DISTANCE_THRESHOLD_YELLOW) {
                // 距离边线超出30厘米
                err.push_back(18);
                // 距离边线超出30厘米，不合格
                AddExamFault(18, rtkTime);
                DEBUG("距离边线超出30厘米");
            }

            // 检查是否拉住手刹

            AppTimer_delete(StartCarTimeout);
            AppTimer_add(StartCarTimeout, D_SEC(CAR_START_TIMEOUT));
            slideDistance = 0.0;
            stopPoint = car->points[0];
            startCarConfirm = 0;
            currTarget = START_CAR;
        } else if (run_status > 0) {
            if (ExitTestArea(map, car)) {
                // 车辆直接驶离测试区，直接淘汰
                err.push_back(12);
                status = -1;
            if (true) {
                AddExamFault(19, rtkTime);
                DEBUG("没拉手刹");
            }
        }
    } else if (currTarget == START_CAR) {
        if (startCarTimeout) {
            startCarTimeout = false;
            //起步时间超过30秒
            err.push_back(15);
            status = -1;
        }
    }

        if (run_status > 0) {
            startCarConfirm++;
            if (startCarConfirm == 2) {
                AppTimer_delete(StartCarTimeout);           // 起步完成
    // 判断起步后滑状态
    if (stopCar) {
        if (IntersectionOfLine(map->point[4], stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) == 1) {
            // 发生后滑
            slideDistance = DistanceOf(stopPoint, car->carXY[car->axial[AXIAL_FRONT]]);

            if (slideLongDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) {
                slideNormalDistance = true;
            }

            if (slideDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) {
            if (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED && !slideLongDistance && !reportSlideFault) {
                // 后滑超过30厘米, 不合格
                AddExamFault(16, rtkTime);
                DEBUG("后滑超过30厘米");
                slideLongDistance = true;
                reportSlideFault = true;
            }
        }

        if (!reportStartTimeout && (IntersectionOfLine(map->point[4], stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) != -1 ||
                DistanceOf(stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) < 0.1)) {
            if (TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss) - stopCarTime > CAR_START_TIMEOUT) {
                // 起步时间超过30秒，不合格
                AddExamFault(15, rtkTime);
                DEBUG("起步时间超过30秒");
                reportStartTimeout = true;
            }
        }

        if (IntersectionOfLine(map->point[5], map->point[6], car->carXY[car->axial[AXIAL_REAR]]) == -1) {
            // 车尾驶过停止杆
            if (slideNormalDistance && !slideLongDistance && !reportSlideFault) {
                reportSlideFault = true;
                // 后滑超过10厘米，但没超过30厘米
                err.push_back(20);
            }
        } else if (run_status < 0) {
            // 后滑了
            slideDistance = DistanceOf(stopPoint, car->points[0]);
            if (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED) {
                // 后滑超过30厘米
                err.push_back(16);
                status = -1;
                AddExamFault(20, rtkTime);
                DEBUG("后滑超过10厘米，但没超过30厘米");
            }
        }

        if (ExitTestArea(map, car)) {
            // 测试完成了
            status = 1;
        }
    }

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

    return status;
}

static void StartCarTimeout(union sigval sig) {
    AppTimer_delete(StartCarTimeout);
    startCarTimeout = true;
}

// 车轮是否压边线
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;

@@ -164,8 +195,8 @@

    Line frontAxle, rearAxle;

    MakeLine(&frontAxle, &car->points[car->desc->front_left_tire[TIRE_OUTSIDE]], &car->points[car->desc->front_right_tire[TIRE_OUTSIDE]]);
    MakeLine(&rearAxle, &car->points[car->desc->rear_left_tire[TIRE_OUTSIDE]], &car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]]);
    MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
    MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);

    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]]);
@@ -179,48 +210,48 @@
    return ret;
}

static double DistanceOfHead2Stopline(const Polygon *map, const car_model_cache_t *car)
static double DistanceOfHead2Stopline(const Polygon *map, const car_model *car)
{
    double dis = 0.0;

    int rel1 = IntersectionOfLine(map->point[4], map->point[3], car->points[0]);
    int rel2 = IntersectionOfLine(map->point[5], map->point[6], car->points[0]);
    int rel1 = IntersectionOfLine(map->point[4], map->point[3], car->carXY[car->body[0]]);
    int rel2 = IntersectionOfLine(map->point[5], map->point[6], car->carXY[car->body[0]]);

    if (rel1 == 1) {
        Line line1;

        MakeLine(&line1, &map->point[4], &map->point[3]);

        dis = DistanceOf(car->points[0], line1);
        dis = DistanceOf(car->carXY[car->body[0]], line1);
    } else if (rel2 == -1) {
        Line line2;

        MakeLine(&line2, &map->point[5], &map->point[6]);

        dis = DistanceOf(car->points[0], line2);
        dis = DistanceOf(car->carXY[car->body[0]], line2);
    }

    return dis;
}

static double DistanceOfTire2Edge(const Polygon *map, const car_model_cache_t *car)
static double DistanceOfTire2Edge(const Polygon *map, const car_model *car)
{
    Line edge;

    MakeLine(&edge,  &map->point[0], &map->point[8]);

    double l1 = DistanceOf(car->points[car->desc->front_right_tire[TIRE_OUTSIDE]], edge);
    double l1 = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], edge);

    double l2 = DistanceOf(car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]], edge);
    double l2 = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], edge);

    return (l1+l2)/2.0;
}

// 整个车辆都要驶离该测试区域
static bool ExitTestArea(const Polygon *map, const car_model_cache_t *car)
static bool ExitTestArea(const Polygon *map, const car_model *car)
{
    for (int i = 0; i < car->point_num; ++i) {
        if (IntersectionOfLine(map->point[8], map->point[7], car->points[i]) != -1)
    for (int i = 0; i < car->bodyNum; ++i) {
        if (IntersectionOfLine(map->point[8], map->point[7], car->carXY[car->body[i]]) != -1)
            return false;
    }
    return true;

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

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

using namespace std;

void StartSAS(void);
void StopSAS(void);
int TestSAS(vector<int>&err, const Polygon *map, const car_model_cache_t *car, double speed, int run_status);
void StartSAS(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/java/com/anyun/exam/lib/RemoteService.java

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

import androidx.annotation.Nullable;

import java.io.UnsupportedEncodingException;
import java.util.concurrent.atomic.AtomicBoolean;

/**
@@ -52,8 +53,18 @@

        @Override
        public void SendCmd(int cmd, String value) throws RemoteException {
            Log.d(TAG, "Process " + Process.myPid() + " Thread " + Thread.currentThread().getId() + " RecvMsgFromMainProc cmd = " + String.format(" %04X ", cmd) + " length " + value.length() + " value " + value.trim());
            MainProcMsgEntry(cmd, value.trim());
            if (cmd == 0x8100) {
                try {
                    byte [] file = value.getBytes("ISO-8859-1");
                    Log.d(TAG, byte2hex(file));
                    MainProcBinMsgEntry(cmd, file, file.length);
                } catch (UnsupportedEncodingException e) {
                    Log.d(TAG, "UnsupportedEncodingException");
                }
            } else {
                Log.d(TAG, "Process " + Process.myPid() + " Thread " + Thread.currentThread().getId() + " RecvMsgFromMainProc cmd = " + String.format(" %04X ", cmd) + " length " + value.length() + " value " + value.trim());
                MainProcMsgEntry(cmd, value.trim());
            }
        }
    };
    @Nullable
@@ -108,6 +119,9 @@
        if (!mIsServiceDestroyed.get()){
            onMessageArrived(cmd, value);
        }



        Log.d(TAG, "SendMsgToMainProc cmd = " + String.format("%04X", cmd) + " json = " + value);
    }

@@ -136,6 +150,23 @@
        }
    }

    private String byte2hex(byte [] buffer){
        StringBuilder h = new StringBuilder();

        Log.d(TAG, "byte buffer length = " + buffer.length);

        for(int i = 0; i < buffer.length; i++){
            String temp = Integer.toHexString(buffer[i] & 0xFF);
            if(temp.length() == 1){
                temp = "0" + temp;
            }
            h.append(temp);
            h.append(" ");
        }

        return h.toString();
    }

    // Used to load the 'native-lib' library on application startup.
    static {
        System.loadLibrary("native-lib");
@@ -143,4 +174,5 @@

    public native void startNative();
    public native void MainProcMsgEntry(int cmd, String value);
    public native void MainProcBinMsgEntry(int cmd, byte []data, int length);
}