| | |
| | | |
| | | #define DEBUG(fmt, args...) LOGD("<drive_straight> <%s>: " fmt, __func__, ##args) |
| | | |
| | | static const double CHECK_STAGE_DISTANCE = 100.0; |
| | | static const double MAX_OFFSET_DISTANCE = 0.3; |
| | | |
| | | static bool crossStartLine; |
| | | static bool reportOffsetOver; |
| | | static double edgeDistance; |
| | |
| | | p2.Y = RoadMapList[index].stopLine.Y2; |
| | | |
| | | if (IntersectionOfLine(p1, p2, car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1) { |
| | | DEBUG("跨过标记线,开始直线测试"); |
| | | crossStartLine = true; |
| | | startPoint = car->basePoint; |
| | | edgeDistance = DistanceOf(car->basePoint, road_edge); |
| | | } |
| | | } else { |
| | | double runDistance; |
| | | runDistance = DistanceOf(car->basePoint, startPoint); |
| | | double distanceToStart = DistanceOf(car->basePoint, startPoint); |
| | | |
| | | if (runDistance < 100.0) { |
| | | if (!reportOffsetOver && fabs(DistanceOf(car->basePoint, road_edge) - edgeDistance) > 0.3) { |
| | | if (distanceToStart > CHECK_STAGE_DISTANCE) { |
| | | if (!reportOffsetOver && fabs(DistanceOf(car->basePoint, road_edge) - edgeDistance) > MAX_OFFSET_DISTANCE) { |
| | | DEBUG("直线偏移大于30厘米"); |
| | | // 偏移大于30厘米,不合格 |
| | | AddExamFault(30, rtkTime); |
| | | reportOffsetOver = true; |
| | | |
| | | ////////////////////////////////////////////// |
| | | startPoint = car->basePoint; |
| | | edgeDistance = DistanceOf(car->basePoint, road_edge); |
| | | reportOffsetOver = false; |
| | | } |
| | | } else { |
| | | startPoint = car->basePoint; |