| | |
| | | #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) |
| | | |
| | |
| | | |
| | | static PointF stopPoint; |
| | | |
| | | static int mapIndex = 0; |
| | | static int prevMoveDirect; |
| | | static uint32_t stopTimepoint = 0; |
| | | static bool stopCar = false; |
| | |
| | | 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) { |
| | |
| | | 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) |
| | |
| | | if (ExitTestArea(map, car)) { |
| | | // 驶离测试区 |
| | | testing = false; |
| | | } |
| | | |
| | | { |
| | | double dis2 = DistanceOfTire2Edge(map, car); |
| | | |
| | | MA_SendDistance(-1, dis2); |
| | | } |
| | | |
| | | if (prevMoveDirect != moveDirect) { |
| | |
| | | } |
| | | } |
| | | |
| | | if (!testing) { |
| | | MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 0); |
| | | } |
| | | |
| | | return testing ? 1 : 0; |
| | | } |
| | | |
| | |
| | | // 整个车辆都要驶离该测试区域 |
| | | 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; |
| | | } |