科目二修改

yy1717

2023-03-31 4bd08f0355b6b2cf3c027202d5ad301b4e182953

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

@@ -2,6 +2,17 @@
// Created by YY on 2019/11/4.
//

// 4----------------------|5
//                        |
//                        |
// 3----------|2          |
//            |           |
//            |           |
//            |           |
//            |           |
//            |           |
//            |1          |0

#include "turn_a90.h"
#include "../test_common/Geometry.h"
#include "../driver_test.h"
@@ -21,90 +32,60 @@
using namespace std;

static bool testing;
static int mapIndex;

static int enterAreaHeading;
static bool turnLeftFinished;
static uint32_t stopTimepoint = 0;

static bool reportStopCarTimeout;
static int prevMoveDirect;
static bool crashRedLine;

static bool CrashRedLine(const Polygon *map, const car_model *car);
static bool ExitTestArea(const Polygon *map, const car_model *car);
static bool CrashRedLine(prime_t &prime);
static bool ExitTestArea(prime_t &prime);

void StartTurnA90(int index, int moveDirect, double heading, const struct RtkTime *rtkTime)
void StartTurnA90(prime_t &prime)
{
    DEBUG("进入直角转弯场地");
    testing = true;
    enterAreaHeading = (int) heading;
    prevMoveDirect = moveDirect;
    if (moveDirect == 0) {
        stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
    }
    reportStopCarTimeout = false;
    enterAreaHeading = (int) prime.pModeling->yaw;
    crashRedLine = false;
    turnLeftFinished = false;
    mapIndex = index;

    MA_EnterMap(mapIndex, MAP_TYPE_TURN_90, 1);
    MA_EnterMap(prime.curr_exam_map.map_idx, 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)
static void StoppingTimeout(apptimer_var_t val) {
    DEBUG("中途停车");
    AddExamFault(20703);
}

void MotionChange(move_status_t mv)
{
    int az = (int) heading;
    if (!testing)
        return;

    AppTimer_delete(StoppingTimeout);

    if (mv == STOP) {
        AppTimer_add(StoppingTimeout, D_SEC(2));
    } else {

    }
}

void TestTurnA90(prime_t &prime)
{
    int az = (int) prime.pModeling->yaw;
    vector<double> dtox;
    vector<Line> line_set;
    Line distance_line;

    if (ExitTestArea(map, car)) {
        testing = false;
        goto TEST_END;
    }

    // 距离检测
    MakeLine(&distance_line, &map->point[0], &map->point[5]);
    line_set.push_back(distance_line);
    MakeLine(&distance_line, &map->point[5], &map->point[4]);
    line_set.push_back(distance_line);
    MakeLine(&distance_line, &map->point[1], &map->point[2]);
    line_set.push_back(distance_line);
    MakeLine(&distance_line, &map->point[2], &map->point[3]);
    line_set.push_back(distance_line);
    DistanceOfTire2X(dtox, car, line_set);
    MA_SendDistance(dtox[0], dtox[1]);

    if (CrashRedLine(map, car)) {
    if (CrashRedLine(prime)) {
        if (!crashRedLine) {
            crashRedLine = true;
            // 碾压道路边缘,不合格
            AddExamFault(20701, rtkTime);
            AddExamFault(20701);
            DEBUG("碾压道路边缘");
        }
    } else {
        crashRedLine = false;
    }

    if (moveDirect != prevMoveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, 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) {
        uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

        if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.turn_a90_pause_criteria) && !reportStopCarTimeout) {
            // 停车超2秒，每次扣5分
            AddExamFault(20703, rtkTime);
            DEBUG("中途停车");
            reportStopCarTimeout = true;
        }
    }

    // 检查转向状态
@@ -117,7 +98,7 @@
    if (az >= 30) {
        if (!turnLeftFinished) {
            char turn_direct;
            if((( ((int)heading) + 360 - enterAreaHeading) % 360) < 180) {
            if((( az + 360 - enterAreaHeading) % 360) < 180) {
                DEBUG("右转");
                turn_direct = 'R';
            } else {
@@ -128,39 +109,38 @@
            if ((turn_direct == 'R' && ReadCarStatus(TURN_SIGNAL_LAMP) != RIGHT_TURN_LIGHT) ||
                    (turn_direct == 'L' && ReadCarStatus(TURN_SIGNAL_LAMP) != LEFT_TURN_LIGHT)) {
                DEBUG("转向灯未开启");
                AddExamFault(20702, rtkTime);
                AddExamFault(20702);
            }
        }
        turnLeftFinished = true;
    }

    if (turnLeftFinished) {

    if (ExitTestArea(prime)) {
        testing = false;
        MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_TURN_90, 0);
    }

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

// 车轮是否压边线
static bool CrashRedLine(const Polygon *map, const car_model *car)
static bool CrashRedLine(prime_t &prime)
{
    bool ret = false;

    Line red_line;
    const int red_lines[][2] = {{0, 5}, {5, 4}, {1, 2}, {2, 3}};

    Line frontAxle, rearAxle;

    // 仅看车轮外侧
    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]]);
    MAKE_LINE(frontAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
              prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]]);
    MAKE_LINE(rearAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
              prime.pModeling[prime.curr_modeling_index].points[prime.pModel->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]]);
        Line red_line;
        MAKE_LINE(red_line, prime.pMap->turn_a90_map[prime.curr_exam_map.map_idx].map[red_lines[i][0]],
                  prime.pMap->turn_a90_map[prime.curr_exam_map.map_idx].map[red_lines[i][1]]);

        if (IntersectionOf(red_line, frontAxle) == GM_Intersection ||
            IntersectionOf(red_line, rearAxle) == GM_Intersection) {
            ret = true;
@@ -171,25 +151,41 @@
    return ret;
}

// 整个车辆都要驶离该测试区域
static bool ExitTestArea(const Polygon *map, const car_model *car)
// 4个车轮和车头点不在场地中
static bool ExitTestArea(prime_t &prime)
{
    bool ret = false;
    Polygon polygon;

    // 全车都需不在地图中
    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]];
    polygon.num = prime.pMap->turn_a90_map[prime.curr_exam_map.map_idx].map.size();
    polygon.point = new PointF[polygon.num];
    for (int i = 0; i < polygon.num; ++i) {
        polygon.point[i] = prime.pMap->turn_a90_map[prime.curr_exam_map.map_idx].map[i];
    }

    if (IntersectionOf(&carBody, map) == GM_None) {
        ret = true;
    int num = 0;

    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
                       &polygon) == GM_None) {
        num++;
    }
    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
                       &polygon) == GM_None) {
        num++;
    }
    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
                       &polygon) == GM_None) {
        num++;
    }
    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]],
                       &polygon) == GM_None) {
        num++;
    }
    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[prime.pModel->axial[AXIAL_FRONT]]],
                       &polygon) == GM_None) {
        num++;
    }

    free(carBody.point);
    delete []polygon.point;

    return ret;
    return num == 5? true : false;
}