修改了上坡起步。

fctom1215

2020-02-12 7b6dd5603277ac8fddba735c456f99d4c07e2e9b

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