智慧驾培首次提交

yy1717

2022-12-08 f7a18ec4494b9c5c9ef3fd440bbf68ffc6425e18

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

@@ -77,6 +77,7 @@

int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    static double distanceToStopLine = 0, distanceToEdge = 0;
    if (!testing)
        return 0;

@@ -100,18 +101,6 @@
        testing = false;
    }

    // 距离检测
    {
        vector<double> dtox;
        vector<Line> line_set;
        Line distance_line;

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

    if (prevMoveDirect != moveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
@@ -132,26 +121,26 @@

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

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

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

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

            if (dis2 > examParam.ramp_edge_red_distance) {
            if (distanceToEdge > examParam.ramp_edge_red_distance) {
                // 距离边线超出50厘米,不合格
                AddExamFault(20302, rtkTime);
                DEBUG("距离边线超出50厘米");
            } else if (dis2 > examParam.ramp_edge_yellow_distance) {
            } else if (distanceToEdge > examParam.ramp_edge_yellow_distance) {
                // 距离边线超出30厘米，扣10分
                AddExamFault(20305, rtkTime);
                DEBUG("距离边线超出30厘米");
@@ -161,6 +150,20 @@
        if (stopCar && !handBreakActive && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
            handBreakActive = true;
        }
    }

    if (!stopCar) {
        // 距离检测
            vector<double> dtox;
            vector<Line> line_set;
            Line distance_line;

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

    // 判断起步后滑状态
@@ -289,6 +292,7 @@
    double l2 = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], edge);

//    return (l1+l2)/2.0;

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