坡起限制条件和手刹起步全阶段生效.

yy1717

2021-02-07 cea2a94fc97e79897cdfd217be8250c075974a1a

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

@@ -44,9 +44,17 @@
static double DistanceOfTire2Edge(const Polygon *map, const car_model *car);
static bool ExitTestArea(const Polygon *map, const car_model *car);

void StartSAS(int index, int moveDirect, const struct RtkTime *rtkTime)
void StartSAS(int index, const Polygon *map, const car_model *car, int moveDirect, const struct RtkTime *rtkTime)
{
    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;
@@ -68,6 +76,9 @@

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

    if (CrashRedLine(map, car)) {
        // 车轮压线，不合格
        if (!occurCrashRedLine) {
@@ -81,6 +92,10 @@

    if (ExitTestArea(map, car)) {
        // 驶离测试区
        if (!stopCar) {
            // 不停车直接离开
            AddExamFault(10103, rtkTime);
        }
        testing = false;
    }