修改了上坡起步。

fctom1215

2020-02-11 3ce7d9cbccabf7f94d8203a98796599cd9dd5411

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

@@ -10,6 +10,9 @@
#include "../driver_test.h"
#include "../jni_log.h"
#include "../common/apptimer.h"
#include "../utils/xconvert.h"

#define DEBUG(fmt, args...)     LOGD("<stop_and_start> <%s>: " fmt, __func__, ##args)

using namespace std;

@@ -24,138 +27,166 @@
const double EDGE_DISTANCE_THRESHOLD_YELLOW = 0.3;
const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;
const int CAR_START_TIMEOUT = 30;
const uint32_t CAR_START_TIMEOUT = D_SEC(30);
const uint32_t STOP_CAR_TIME = D_SEC(2);

const double EPSILON = 1e-3;

static bool SASTesting = false;

static double slideDistance;
static bool startCarTimeout;
static int currTarget;
static PointF stopPoint;
static int startCarConfirm;             // 起步时，持续前进一小段才算

static void StartCarTimeout(union sigval sig);
static bool CrashRedLine(const Polygon *map, const car_model_cache_t *car);
static double DistanceOfHead2Stopline(const Polygon *map, const car_model_cache_t *car);
static double DistanceOfTire2Edge(const Polygon *map, const car_model_cache_t *car);
static bool ExitTestArea(const Polygon *map, const car_model_cache_t *car);
static int prevMoveDirect;
static uint64_t stopTimepoint = 0;
static bool stopCar = false;
static uint32_t stopCarTime;
static bool occurCrashRedLine = false;
static bool slideLongDistance = false;
static bool slideNormalDistance = false;
static bool reportSlideFault = false;
static bool reportStartTimeout = false;

void StartSAS(void)
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 moveDirect, const struct RtkTime *rtkTime)
{
    DEBUG("进入坡起项目");
    SASTesting = true;
    slideDistance = 0.0;
    startCarTimeout = false;
    currTarget = STOP_CAR;

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

    occurCrashRedLine = false;
    stopCar = false;
    slideLongDistance = false;
    slideNormalDistance = false;
    reportSlideFault = false;
    reportStartTimeout = false;
}

void StopSAS(void)
{
    SASTesting = false;
    AppTimer_delete(StartCarTimeout);
}

int TestSAS(vector<int>&err, const Polygon *map, const car_model_cache_t *car, double speed, int run_status)
int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    int status = 0;

    if (!SASTesting)
        return -2;

    if (currTarget >= STOP_CAR) {
        if (CrashRedLine(map, car)) {
            // 车轮压线
            err.push_back(13);
            status = -1;
    if (CrashRedLine(map, car)) {
        // 车轮压线，不合格
        if (!occurCrashRedLine) {
            AddExamFault(13, rtkTime);
            DEBUG("车轮压线");
        }
        occurCrashRedLine = true;
    } else {
        occurCrashRedLine = false;
    }

    if (currTarget == STOP_CAR) {
        if (run_status == 0) {
    if (ExitTestArea(map, car)) {
        // 驶离测试区
        status = 1;
    }

    if (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;
        }
        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 && !stopCar) {
            // 这里判断停车状态
            stopCar = true;
            stopCarTime = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);

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

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

            if (dis1 > STOP_DISTANCE_THRESHOLD_RED) {
                // 距离停止线前后超出50厘米
                err.push_back(12);
                AddExamFault(12, rtkTime);
                DEBUG("距离停止线前后超出50厘米，不合格");
                status = -1;
            } else if (fabs(dis1) > EPSILON) {
                // 前保险没有位于停止带内，但没有超出50厘米
                err.push_back(17);
                // 前保险没有位于停止带内，但没有超出50厘米，扣10分
                AddExamFault(17, rtkTime);
                DEBUG("前保险没有位于停止带内，但没有超出50厘米");
            }

            if (dis2 > EDGE_DISTANCE_THRESHOLD_RED) {
                // 距离边线超出50厘米
                err.push_back(14);
                // 距离边线超出50厘米,不合格
                AddExamFault(14, rtkTime);
                DEBUG("距离边线超出50厘米");
                status = -1;
            } else if (dis2 > EDGE_DISTANCE_THRESHOLD_YELLOW) {
                // 距离边线超出30厘米
                err.push_back(18);
                // 距离边线超出30厘米，不合格
                AddExamFault(18, rtkTime);
                DEBUG("距离边线超出30厘米");
            }

            // 检查是否拉住手刹

            AppTimer_delete(StartCarTimeout);
            AppTimer_add(StartCarTimeout, D_SEC(CAR_START_TIMEOUT));
            slideDistance = 0.0;
            stopPoint = car->points[0];
            startCarConfirm = 0;
            currTarget = START_CAR;
        } else if (run_status > 0) {
            if (ExitTestArea(map, car)) {
                // 车辆直接驶离测试区，直接淘汰
                err.push_back(12);
                status = -1;
            if (true) {
                AddExamFault(19, rtkTime);
                DEBUG("没拉手刹");
            }
        }
    } else if (currTarget == START_CAR) {
        if (startCarTimeout) {
            startCarTimeout = false;
            //起步时间超过30秒
            err.push_back(15);
            status = -1;
        }
    }

        if (run_status > 0) {
            startCarConfirm++;
            if (startCarConfirm == 2) {
                AppTimer_delete(StartCarTimeout);           // 起步完成
    // 判断起步后滑状态
    if (stopCar) {
        if (IntersectionOfLine(map->point[4], stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) == 1) {
            // 发生后滑
            slideDistance = DistanceOf(stopPoint, car->carXY[car->axial[AXIAL_FRONT]]);

            if (slideLongDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) {
                slideNormalDistance = true;
            }

            if (slideDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) {
            if (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED && !slideLongDistance && !reportSlideFault) {
                // 后滑超过30厘米, 不合格
                AddExamFault(16, 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(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss) - stopCarTime > CAR_START_TIMEOUT) {
                // 起步时间超过30秒，不合格
                AddExamFault(15, 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厘米
                err.push_back(20);
            }
        } else if (run_status < 0) {
            // 后滑了
            slideDistance = DistanceOf(stopPoint, car->points[0]);
            if (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED) {
                // 后滑超过30厘米
                err.push_back(16);
                status = -1;
                AddExamFault(20, rtkTime);
                DEBUG("后滑超过10厘米，但没超过30厘米");
            }
        }

        if (ExitTestArea(map, car)) {
            // 测试完成了
            status = 1;
        }
    }

    if (status != 0) {
        StopSAS();
    }

    return status;
}

static void StartCarTimeout(union sigval sig) {
    AppTimer_delete(StartCarTimeout);
    startCarTimeout = true;
}

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

@@ -164,8 +195,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]]);
@@ -179,48 +210,48 @@
    return ret;
}

static double DistanceOfHead2Stopline(const Polygon *map, const car_model_cache_t *car)
static double DistanceOfHead2Stopline(const Polygon *map, const car_model *car)
{
    double dis = 0.0;

    int rel1 = IntersectionOfLine(map->point[4], map->point[3], car->points[0]);
    int rel2 = IntersectionOfLine(map->point[5], map->point[6], car->points[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]]);

    if (rel1 == 1) {
        Line line1;

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

        dis = DistanceOf(car->points[0], line1);
        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->points[0], line2);
        dis = DistanceOf(car->carXY[car->body[0]], line2);
    }

    return dis;
}

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

    MakeLine(&edge,  &map->point[0], &map->point[8]);

    double l1 = DistanceOf(car->points[car->desc->front_right_tire[TIRE_OUTSIDE]], edge);
    double l1 = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], edge);

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

    return (l1+l2)/2.0;
}

// 整个车辆都要驶离该测试区域
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[8], map->point[7], car->points[i]) != -1)
    for (int i = 0; i < car->bodyNum; ++i) {
        if (IntersectionOfLine(map->point[8], map->point[7], car->carXY[car->body[i]]) != -1)
            return false;
    }
    return true;