科目二修改

yy1717

2023-03-31 4bd08f0355b6b2cf3c027202d5ad301b4e182953

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

@@ -2,6 +2,21 @@
// Created by YY on 2019/10/31.
//

//                         |9
//                         |
//                         |
//                         |
//                         |
//       7-----------------|8
//       6_________________|5
//       3_________________|4
//                         |
//       2-----------------|1
//                         |
//                         |
//                         |
//                         |0

#include <cstdlib>
#include <vector>
#include <cmath>
@@ -21,7 +36,7 @@
using namespace std;


const uint32_t STOP_CAR_TIME = D_SEC(1);


const double EPSILON = 1e-3;

@@ -29,62 +44,66 @@

static PointF stopPoint;

static int mapIndex = 0;
static int prevMoveDirect;
static uint32_t stopTimepoint = 0;
static bool stopCar = false;
static uint32_t stopCarTime;
static bool check1 = false;

static bool stopConfirm;
static bool restartComplete = false;
static bool occurCrashRedLine = false;
static bool slideLongDistance = false;

static bool slideNormalDistance = false;
static bool reportSlideFault = false;
static bool reportStartTimeout = false;
static bool handBreakActive = false;
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 index, const Polygon *map, const car_model *car, int moveDirect, const struct RtkTime *rtkTime)
static bool CrashRedLine(prime_t &prime);
static double DistanceOfHead2Stopline(prime_t &prime);
static double DistanceOfTire2Edge(prime_t &prime);
static bool ExitTestArea(prime_t &prime);

void StartSAS(prime_t &prime)
{
    double yawEdge = YawOf(map->point[8], map->point[0]);

    if (moveDirect < 0 || DeltaYaw(car->yaw, yawEdge) >= 90.0) {
        testing = false;
        return;
    }

    DEBUG("进入坡起项目");

    testing = true;
    mapIndex = index;
    prevMoveDirect = moveDirect;

    if (moveDirect == 0) {
        stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
    }

    stopConfirm = false;
    restartComplete = false;
    occurCrashRedLine = false;
    stopCar = false;
    slideLongDistance = false;
    check1 = false;
    slideNormalDistance = false;
    reportSlideFault = false;
    reportStartTimeout = false;
    handBreakActive = false;

    MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 1);
    MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_UPHILL, 1);
}

int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
static void StoppingTimeout(apptimer_var_t val) {
    // 起步时间超过30秒，不合格
    AddExamFault(20303);
    DEBUG("起步时间超过30秒");
}

static void StopConfirm(apptimer_var_t val) {
    stopConfirm = true;
}

void MotionChange(move_status_t mv)
{
    AppTimer_delete(StopConfirm);

    if (mv == STOP) {
        AppTimer_add(StopConfirm, D_SEC(1));
    } else {

    }
}

int TestSAS(prime_t &prime)
{
    static double distanceToStopLine = 0, distanceToEdge = 0;
    if (!testing)
        return 0;

    if (CrashRedLine(map, car)) {
    if (CrashRedLine(prime)) {
        // 车轮压线，不合格
        if (!occurCrashRedLine) {
            AddExamFault(10116, rtkTime);
            AddExamFault(10116);
            DEBUG("车轮压线");
        }
        occurCrashRedLine = true;
@@ -92,158 +111,125 @@
        occurCrashRedLine = false;
    }

    if (ExitTestArea(map, car)) {
        // 驶离测试区
        if (!stopCar) {
            // 不停车直接离开
            AddExamFault(10103, rtkTime);
        }
        testing = false;
    }
    // 检测到停车
    if (prime.pMotion->move == STOP && stopConfirm) {
        if (!check1) {
            check1 = true;
            // 存储停止点
            stopPoint = prime.pModeling->base_point;
            // 开始停车计时
            AppTimer_delete(StoppingTimeout);
            AppTimer_add(StoppingTimeout, examParam.ramp_start_car_limit_time);

    if (prevMoveDirect != moveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
        } else if (stopCar && !handBreakActive) {
            // 检查是否拉住手刹
            handBreakActive = true;
            AddExamFault(20306, rtkTime);
            DEBUG("没拉手刹");
        }
        prevMoveDirect = moveDirect;
    } else if (moveDirect == 0) {
        uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

        if (tp - stopTimepoint >= STOP_CAR_TIME && !stopCar) {
            // 这里判断停车状态
            stopCar = true;
            stopCarTime = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

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

            distanceToStopLine = DistanceOfHead2Stopline(map, car);
            distanceToEdge = DistanceOfTire2Edge(map, car);
            // 检查车头和停车带的距离
            distanceToStopLine = DistanceOfHead2Stopline(prime);
            distanceToEdge = DistanceOfTire2Edge(prime);

            DEBUG("DIS1 = %f  DIS2 = %f", distanceToStopLine, distanceToEdge);

            if (distanceToStopLine > examParam.ramp_stoppoint_red_distance) {
                // 距离停止线前后超出50厘米
                AddExamFault(20301, rtkTime);
                AddExamFault(20301);
                DEBUG("距离停止线前后超出50厘米，不合格");
            } else if (fabs(distanceToStopLine) > EPSILON) {
                // 前保险没有位于停止带内，但没有超出50厘米，扣10分
                AddExamFault(20304, rtkTime);
                AddExamFault(20304);
                DEBUG("前保险没有位于停止带内，但没有超出50厘米");
            }

            if (distanceToEdge > examParam.ramp_edge_red_distance) {
                // 距离边线超出50厘米,不合格
                AddExamFault(20302, rtkTime);
                AddExamFault(20302);
                DEBUG("距离边线超出50厘米");
            } else if (distanceToEdge > examParam.ramp_edge_yellow_distance) {
                // 距离边线超出30厘米，扣10分
                AddExamFault(20305, rtkTime);
                AddExamFault(20305);
                DEBUG("距离边线超出30厘米");
            }
        }

        if (stopCar && !handBreakActive && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
        // 停车后，检查手刹拉起情况
        if (!handBreakActive && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
            handBreakActive = true;
        }
    }

    if (!stopCar) {
        // 距离检测
            vector<double> dtox;
            vector<Line> line_set;
            Line distance_line;
    if (prime.pMotion->move != STOP && stopConfirm) {
        // 车辆从停止状态再次移动
        double juli = DistanceOf(prime.pModeling->base_point, stopPoint);

            MakeLine(&distance_line, &map->point[0], &map->point[8]);
            line_set.push_back(distance_line);
            DistanceOfTire2X(dtox, car, line_set);
            MA_SendDistance(dtox[0], dtox[1]);
    } else {
        MA_SendDistance(distanceToStopLine, distanceToEdge);
    }

    // 判断起步后滑状态
    if (stopCar) {
        if (IntersectionOfLine(map->point[4], stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) == 1) {
            // 发生后滑
            double slideDistance = DistanceOf(stopPoint, car->carXY[car->axial[AXIAL_FRONT]]);

            if (slideDistance > examParam.ramp_slide_yellow_distance) {
        if (juli > examParam.ramp_slide_yellow_distance) {
            double deg = YawOf(stopPoint, prime.pModeling->base_point);
            deg = fabs(prime.pModeling->yaw - deg);
            if (deg > 180) {
                deg = 360 - deg;
            }
            if (deg < 90) {
                // 车辆上坡
                if (!restartComplete) {
                    restartComplete = true;
                    AppTimer_delete(StoppingTimeout);
                    if (slideNormalDistance && !reportSlideFault) {
                        // 后滑超过10厘米，但没超过30厘米
                        DEBUG("后滑超过10厘米，但没超过30厘米");
                        reportSlideFault = true;
                        AddExamFault(10204);
                    }
                    if (!handBreakActive) {
                        // 检查是否拉住手刹
                        DEBUG("没拉手刹");
                        handBreakActive = true;
                        AddExamFault(20306);
                    }
                }
            } else {
                // 车辆后滑
                slideNormalDistance = true;
            }

            if (slideDistance > examParam.ramp_slide_red_distance && !slideLongDistance && !reportSlideFault) {
                // 后滑超过30厘米, 不合格
                AddExamFault(10106, 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(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10) - stopCarTime > examParam.ramp_start_car_limit_time) {
                // 起步时间超过30秒，不合格
                AddExamFault(20303, 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厘米
                AddExamFault(10204, rtkTime);
                DEBUG("后滑超过10厘米，但没超过30厘米");
                if (juli > examParam.ramp_slide_red_distance && !reportSlideFault) {
                    // 后滑超过30厘米, 不合格
                    DEBUG("后滑超过30厘米");
                    reportSlideFault = true;
                    AddExamFault(10106);
                }
            }
        }
    }

    if (!testing) {
        MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 0);
    if (ExitTestArea(prime)) {
        // 驶离测试区
        if (!stopConfirm) {
            // 不停车直接离开
            AddExamFault(10103);
        }
        MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_UPHILL, 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;
    int red_lines[2][2];
    int red_line_num = 0;
    int red_lines[][2] = {{0, 1}, {1, 4}, {4, 5}, {5, 8}, {8, 9}};

    if (map->num == 10) {
        red_lines[0][0] = 0;
        red_lines[0][1] = 8;
        red_lines[1][0] = 8;
        red_lines[1][1] = 9;
        red_line_num = 2;
    } else {
        red_lines[0][0] = 0;
        red_lines[0][1] = 8;
        red_line_num = 1;
    }

    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 < red_line_num; ++i) {
        MakeLine(&red_line, &map->point[red_lines[i][0]], &map->point[red_lines[i][1]]);
    for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
        Line red_line;
        MAKE_LINE(red_line, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[red_lines[i][0]],
                  prime.pMap->uphill_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) {
            IntersectionOf(red_line, rearAxle) == GM_Intersection) {
            ret = true;
            break;
        }
@@ -252,28 +238,21 @@
    return ret;
}

static double DistanceOfHead2Stopline(const Polygon *map, const car_model *car)
static double DistanceOfHead2Stopline(prime_t &prime)
{
    double dis = 0.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]]);
    Line upper_edge, lower_edge;

    DEBUG("%d %d %f, %f", car->body[0], car->axial[AXIAL_FRONT], car->carXY[car->body[0]].X, car->carXY[car->body[0]].Y);
    DEBUG("rel1 %d rel2 %d", rel1, rel2);
    MAKE_LINE(upper_edge, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[5],
              prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[6]);
    MAKE_LINE(lower_edge, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[4],
              prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[3]);

    if (rel1 == 1) {
        Line line1;

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

        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->carXY[car->body[0]], line2);
    if (IntersectionOfLine(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], upper_edge) == REL_POS_RIGHT) {
        dis = DistanceOf(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], upper_edge);
    } else if (IntersectionOfLine(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], lower_edge) == REL_POS_LEFT) {
        dis = DistanceOf(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], lower_edge);
    }

    DEBUG("DistanceOfHead2Stopline dis %f", dis);
@@ -281,28 +260,27 @@
    return dis;
}

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

    MakeLine(&edge,  &map->point[0], &map->point[8]);
    MAKE_LINE(edge, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[0],
              prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[8]);

    double l1 = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], edge);
    double l1 = DistanceOf(prime.pModeling->points[prime.pModel->right_front_tire[TIRE_OUTSIDE]], edge);

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

//    return (l1+l2)/2.0;
    double l2 = DistanceOf(prime.pModeling->points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]], edge);

    return MAX(l1, l2);     // 取最远的
}

// 整个车辆都要驶离该测试区域
static bool ExitTestArea(const Polygon *map, const car_model *car)
static bool ExitTestArea(prime_t &prime)
{
    // 全车都需不在地图中
    bool ret = false;

    Polygon carBody, map2;
    /*Polygon carBody, map2;
    PointF vPoint = Calc3Point(map->point[8], map->point[0], DistanceOf(map->point[8], map->point[7]), 'R');
    PointF vPoint2;

@@ -329,6 +307,6 @@
    }

    free(carBody.point);
    free(map2.point);
    free(map2.point);*/
    return ret;
}