yy1717
2020-04-02 24428a1b69afcf113883e9cede1159443201a1b8
lib/src/main/cpp/test_items/stop_and_start.cpp
@@ -12,6 +12,7 @@
#include "../common/apptimer.h"
#include "../utils/xconvert.h"
#include "../test_common/car_sensor.h"
#include "../master/comm_if.h"
#define DEBUG(fmt, args...)     LOGD("<stop_and_start> <%s>: " fmt, __func__, ##args)
@@ -31,6 +32,7 @@
static PointF stopPoint;
static int mapIndex = 0;
static int prevMoveDirect;
static uint32_t stopTimepoint = 0;
static bool stopCar = false;
@@ -46,11 +48,11 @@
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)
void StartSAS(int index, int moveDirect, const struct RtkTime *rtkTime)
{
    DEBUG("进入坡起项目");
    testing = true;
    mapIndex = index;
    prevMoveDirect = moveDirect;
    if (moveDirect == 0) {
@@ -63,6 +65,8 @@
    slideNormalDistance = false;
    reportSlideFault = false;
    reportStartTimeout = false;
    MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 1);
}
int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
@@ -81,6 +85,12 @@
    if (ExitTestArea(map, car)) {
        // 驶离测试区
        testing = false;
    }
    {
        double dis2 = DistanceOfTire2Edge(map, car);
        MA_SendDistance(-1, dis2);
    }
    if (prevMoveDirect != moveDirect) {
@@ -172,6 +182,10 @@
        }
    }
    if (!testing) {
        MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 0);
    }
    return testing ? 1 : 0;
}
@@ -245,10 +259,22 @@
// 整个车辆都要驶离该测试区域
static bool ExitTestArea(const Polygon *map, const car_model *car)
{
    // 在8->7线的右侧
    for (int i = 0; i < car->bodyNum; ++i) {
        if (IntersectionOfLine(map->point[8], map->point[7], car->carXY[car->body[i]]) != -1)
            return false;
    // 全车都需不在地图中
    bool ret = false;
    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]];
    }
    return true;
    if (IntersectionOf(&carBody, map) == GM_None) {
        ret = true;
    }
    free(carBody.point);
    return ret;
}