修改了上坡起步。

fctom1215

2020-02-12 7b6dd5603277ac8fddba735c456f99d4c07e2e9b

修改了上坡起步。

 .idea/misc.xml

@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
  <component name="ProjectRootManager" version="2" languageLevel="JDK_1_7" project-jdk-name="JDK" project-jdk-type="JavaSDK">
  <component name="ProjectRootManager" version="2" languageLevel="JDK_1_7" project-jdk-name="1.8" project-jdk-type="JavaSDK">
    <output url="file://$PROJECT_DIR$/build/classes" />
  </component>
  <component name="ProjectType">

 app/src/main/java/safeluck/drive/evaluation/MainActivity.java

@@ -98,7 +98,7 @@
                        String strs = new String(datas, Charset.forName("ISO-8859-1"));
                        MyLog.i(TAG, "mcu upgrade datas.legnth=="+strs.getBytes("ISO-8859-1").length);
                        AYSdk.getInstance().sendCmd(Constant.UPGRADE_MCU_CONTENT_FILE, strs);
                        FileUtil.deleteFile("",Constant.MCU_FILE_NAME);
//                        FileUtil.deleteFile("",Constant.MCU_FILE_NAME);
                    }else{
                        MyLog.i(TAG,"mcu升级文件不存在");
                    }

 lib/src/main/cpp/driver_test.cpp

@@ -148,8 +148,6 @@

    MapList[MapNum].id = id;

    if (type == 1) type = 3;            //////////////////////////////////////

    MapList[MapNum].type = type;

    MapList[MapNum].map.num = pointNum;
@@ -347,7 +345,7 @@
                RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);

        brief.qf = RtkBuffer[index].qf;
        brief.map_id = 863;//GetMapId(CurrExamMapIndex, MapList, MapNum);
        brief.map_id = 865;//GetMapId(CurrExamMapIndex, MapList, MapNum);
        brief.move = move;
        brief.speed = speed * 3.6;
        brief.heading = RtkBuffer[index].heading;
@@ -384,6 +382,7 @@
        free(brief.point);

        struct RtkTime rtkTime;
        double azimuth = RtkBuffer[index].heading;

        rtkTime.YY = RtkBuffer[index].YY;
        rtkTime.MM = RtkBuffer[index].MM;
@@ -409,6 +408,10 @@
                        CurrExamStatus = 0;
                        break;
                    case MAP_TYPE_STOP_START:
                        DEBUG("进入上坡起步场地");
                        MA_SendDebugInfo("进入上坡起步场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                        StartSAS(move, &rtkTime);
                        CurrExamStatus = 0;
                        break;
                    case MAP_TYPE_PART_EDGE:
                        DEBUG("进入侧方位停车场地");
@@ -420,6 +423,10 @@
                    case MAP_TYPE_CURVE:
                        break;
                    case MAP_TYPE_TURN_90:
                        DEBUG("进入直角转弯场地");
                        MA_SendDebugInfo("进入直角转弯场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                        StartTurnA90(move, azimuth, &rtkTime);
                        CurrExamStatus = 0;
                        break;
                    default:break;
                }
@@ -435,6 +442,7 @@
                                                        CarModel, CarModelPrev, speed, move, &rtkTime);
                        break;
                    case MAP_TYPE_STOP_START:
                        CurrExamStatus = TestSAS(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, &rtkTime);
                        break;
                    case MAP_TYPE_PART_EDGE:
                        CurrExamStatus = TestParkEdge(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, &rtkTime);
@@ -442,6 +450,7 @@
                    case MAP_TYPE_CURVE:
                        break;
                    case MAP_TYPE_TURN_90:
                        CurrExamStatus = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, azimuth, speed, move, &rtkTime);
                        break;
                    default:
                        break;
@@ -678,8 +687,41 @@
    // 车的最前点是否进入地图
    for (int i = 0; i < mapNum && car != NULL; ++i) {
        if (mapList[i].type == MAP_TYPE_STOP_START) {
            // 构造虚拟的左上角点
            double x9, y9, xo, yo;

        } else if (mapList[i].type == MAP_TYPE_CURVE) {
            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 (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
@@ -692,7 +734,16 @@
                    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;
            }
        }*/
    }
    return -1;
}

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

@@ -20,6 +20,7 @@
const int PARK_TIMEOUT = 90;
const uint32_t STOP_CAR_TIME = D_SEC(2);

static bool reportExamTimeout;
static uint64_t stopTimepoint = 0;
static bool occurCrashRedLine1, occurCrashRedLine2;
static int prevMoveStatus, storeMoveStatusBeforeStop;
@@ -31,10 +32,12 @@
static bool CrashRedLine2(const Polygon *map, const car_model *car);
static bool EnterParking(const Polygon *map, const car_model *car);
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)
{
    occurCrashRedLine1 = occurCrashRedLine2 = false;        // 这个科目规定特殊点，发生一次扣10分，而不直接淘汰
    reportExamTimeout = false;

    prevMoveStatus = moveStatus;
    parkSuccess = false;
@@ -74,25 +77,23 @@
        occurCrashRedLine2 = false;
    }

    if (ExitParkArea(map, car)) {
    if (ExitParkArea(map, car) || ExitParkArea2(map, car)) {
        if (!parkSuccess || !occurMoveBack) {
            // 直接驶离测试区，认为移库不入
            AddExamFault(21, rtkTime);
            DEBUG("直接驶离测试区,移库不入");
            status = -1;
        } else {
            status = 1;
        }
        status = 1;
    }

    if (occurMoveBack) {
        uint32_t tp = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);

        if (tp - moveBackTimePoint >= PARK_TIMEOUT) {
        if (!reportExamTimeout && tp - moveBackTimePoint >= PARK_TIMEOUT) {
            // 超时90秒，不合格
            AddExamFault(22, rtkTime);
            reportExamTimeout = true;
            DEBUG("移库90秒超时");
            status = -1;
        }
    }

@@ -129,7 +130,6 @@
                        // 停止后，车身出线，不合格
                        AddExamFault(21, rtkTime);
                        DEBUG("移库不入");
                        status = -1;
                    }

                    // 在这里检查转向灯状态
@@ -254,3 +254,25 @@
    }
    return true;
}

static bool ExitParkArea2(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/stop_and_start.cpp

@@ -118,7 +118,6 @@
                // 距离停止线前后超出50厘米
                AddExamFault(12, rtkTime);
                DEBUG("距离停止线前后超出50厘米，不合格");
                status = -1;
            } else if (fabs(dis1) > EPSILON) {
                // 前保险没有位于停止带内，但没有超出50厘米，扣10分
                AddExamFault(17, rtkTime);
@@ -129,7 +128,6 @@
                // 距离边线超出50厘米,不合格
                AddExamFault(14, rtkTime);
                DEBUG("距离边线超出50厘米");
                status = -1;
            } else if (dis2 > EDGE_DISTANCE_THRESHOLD_YELLOW) {
                // 距离边线超出30厘米，不合格
                AddExamFault(18, rtkTime);
@@ -183,6 +181,8 @@
            }
        }
    }

    return status;
}

// 车轮是否压边线
@@ -250,6 +250,7 @@
// 整个车辆都要驶离该测试区域
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;

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

@@ -7,6 +7,8 @@
#include "../driver_test.h"
#include "../common/apptimer.h"
#include "../jni_log.h"
#include "../utils/xconvert.h"
#include "../defs.h"

#include <vector>
#include <cstdlib>
@@ -15,122 +17,103 @@

using namespace std;

enum {
    TURN_ANGLE_90,
    TURN_ANGLE_90_CMP
};

const uint32_t STOP_CAR_TIME = D_SEC(2);

static bool TA90Testing;
static bool carStopEvent;
static bool checked;
static int currTarget;

static int azimuth;
static uint32_t stopTimepoint = 0;
static bool turnLeftFinished;
static uint64_t stopTimepoint = 0;

static bool CrashRedLine(const Polygon *map, const car_model_cache_t *car);
static bool ExitTestArea(const Polygon *map, const car_model_cache_t *car);
static bool Turned(const Polygon *map, const car_model_cache_t *car);
static bool reportStopCarTimeout;
static int prevMoveDirect;
static bool crashRedLine;

void StartTurnA90(double heading)
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)
{
    azimuth = (int) heading;
    checked = false;
    TA90Testing = true;
    carStopEvent = false;

    currTarget = TURN_ANGLE_90;
    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;
    }
    reportStopCarTimeout = false;
    crashRedLine = false;
    turnLeftFinished = false;
}

void StopTurnA90(void)
{
    TA90Testing = false;
}

int TestTurnA90(vector<int>&err, const Polygon *map, const car_model_cache_t *car, double speed, int run_status, double heading)
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;

    if (!TA90Testing)
        return -2;

    DEBUG("TestTurnA90 %d", run_status);

    if (CrashRedLine(map, car)) {
        // 压线了
        err.push_back(29);
        status = -1;
        DEBUG("错误 压线");
    }

    if (run_status != 0) {
        if (carStopEvent)
            DEBUG("TURN_ANGLE_90 停车时间 %ld", AppTimer_GetTickCount() - stopTimepoint);

        if (carStopEvent && AppTimer_GetTickCount() - stopTimepoint > STOP_CAR_TIME) {
            // 中途停车
            err.push_back(31);
            DEBUG("错误 停车1");
        }
        carStopEvent = false;
    } else if (!carStopEvent){
        carStopEvent = true;
        stopTimepoint = AppTimer_GetTickCount();
    }

    if (ExitTestArea(map, car)) {
        // 测试结束
        status = 1;
    }

    if (currTarget == TURN_ANGLE_90) {
        // 转向灯开启
        int az = (int) heading;

        if (abs(az - azimuth) > 180) {
            az = 360 - abs(az-azimuth);
        } else {
            az = abs(az - azimuth);
    if (CrashRedLine(map, car)) {
        if (!crashRedLine) {
            crashRedLine = true;
            // 碾压道路边缘,不合格
            AddExamFault(29, rtkTime);
            DEBUG("碾压道路边缘");
        }

        if (az >= 10 && !checked) {
            checked = true;
            // 转向灯未开启
            err.push_back(30);
            DEBUG("错误 灯没看");
        }

        if (Turned(map, car)) {
            currTarget = TURN_ANGLE_90_CMP;
            checked = false;
        }
        DEBUG("TURN_ANGLE_90 %d %d",run_status, az);
    } else {
        // 关闭转向灯
        Line line;

        MakeLine(&line, &map->point[1], &map->point[2]);

        // 大于2.5米后检查车灯
        if (!checked && DistanceOf(car->points[0], line) >= 2.5) {
            checked = true;
            // 转向灯未关闭
            err.push_back(30);
            DEBUG("错误 灯没管");
        }
        DEBUG("TURN_ANGLE_90_CMP");
        crashRedLine = false;
    }

    if (status != 0) {
        StopTurnA90();
    if (moveDirect != prevMoveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
            stopTimepoint = stopTimepoint * 1000 + rtkTime->mss*10;
            reportStopCarTimeout = false;

            DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
        } else {

        }
        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 && !reportStopCarTimeout) {
            // 停车超2秒，每次扣5分
            AddExamFault(31, rtkTime);
            DEBUG("中途停车");
            reportStopCarTimeout = true;
        }
    }

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

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

    if (az >= 30) {
        if (!turnLeftFinished) {
            // 转向灯未开启，扣10分
            AddExamFault(30, rtkTime);
            DEBUG("转向灯未开启");
        }
        turnLeftFinished = true;
    }

    if (turnLeftFinished) {

    }

    return status;
}

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

@@ -140,8 +123,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]]);
@@ -156,20 +139,24 @@
}

// 整个车辆都要驶离该测试区域
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[3], map->point[4], car->points[i]) != 1)
            return false;
    }
    return true;
}
    bool ret = false;

static bool Turned(const Polygon *map, const car_model_cache_t *car)
{
    for (int i = 0; i < car->point_num; ++i) {
        if (IntersectionOfLine(map->point[1], map->point[2], car->points[i]) != 1)
            return 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/turn_a90.h

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

using namespace std;

void StartTurnA90(double heading);
void StopTurnA90(void);
int TestTurnA90(vector<int>&err, const Polygon *map, const car_model_cache_t *car, double speed, int run_status, double heading);
void StartTurnA90(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