| | |
| | | static int setup; |
| | | static double beginOdo; |
| | | static int yaw_stat; |
| | | static double offsetBase; |
| | | |
| | | static void TtsBack(int seq) |
| | | { |
| | |
| | | PlayTTS("请保持直线行驶", TtsBack); |
| | | } |
| | | |
| | | bool TestDriveStraight(const car_model *car, const struct RtkTime *rtkTime) { |
| | | bool TestDriveStraight(road_exam_map &RoadMap, int roadIndex, const car_model *car, const struct RtkTime *rtkTime) { |
| | | static double yaw; |
| | | static Line baseLine; |
| | | static double offset1, offset2; |
| | | |
| | | if (roadIndex < 0 || roadIndex >= RoadMap.roads.size()) |
| | | return false; |
| | | |
| | | if (setup == 1) { |
| | | // 偏航角平均值 |
| | |
| | | yaws.push_back(car->yaw); |
| | | yaw_stat++; |
| | | |
| | | if (yaw_stat == 5) { |
| | | if (yaw_stat == 15) { |
| | | yaw = AvgYaw(yaws); |
| | | vector<double>().swap(yaws); |
| | | |
| | |
| | | PointF extPoint = PointExtend(car->basePoint, 100, yaw); |
| | | MakeLine(&baseLine, &car->basePoint, &extPoint); |
| | | beginOdo = ReadOdo(); |
| | | |
| | | PointF px = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, car->carXY[car->axial[AXIAL_FRONT]]); |
| | | offsetBase = DistanceOf(px, car->carXY[car->axial[AXIAL_FRONT]]); |
| | | DEBUG("起点偏离 %f", offsetBase); |
| | | |
| | | setup = 2; |
| | | } |
| | | } else if (setup == 2) { |
| | | double offset = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine); |
| | | offset1 = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine); |
| | | |
| | | if (offset > MAX_OFFSET_DISTANCE) { |
| | | DEBUG("直线偏移大于30厘米 offset = %f", offset); |
| | | if (offset1 > MAX_OFFSET_DISTANCE) { |
| | | DEBUG("虚拟直线偏移大于30厘米 offset1 = %f", offset1); |
| | | // // 偏移大于30厘米,不合格 |
| | | // AddExamFault(30, rtkTime); |
| | | // return false; |
| | | } |
| | | |
| | | PointF px = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, car->carXY[car->axial[AXIAL_FRONT]]); |
| | | offset2 = DistanceOf(px, car->carXY[car->axial[AXIAL_FRONT]]); |
| | | |
| | | if (fabs(offset2 - offsetBase) > MAX_OFFSET_DISTANCE) { |
| | | DEBUG("直线偏移大于30厘米 offset2 = %f", fabs(offset2 - offsetBase)); |
| | | // 偏移大于30厘米,不合格 |
| | | AddExamFault(30, rtkTime); |
| | | return false; |
| | |
| | | } |
| | | |
| | | if (setup == 2 && ReadOdo() - beginOdo > CHECK_STAGE_DISTANCE) { |
| | | DEBUG("直线行驶结束"); |
| | | DEBUG("直线行驶结束 offset1 = %f offset2 = %f", offset1, fabs(offset2 - offsetBase)); |
| | | |
| | | PlayTTS("直线行驶结束", NULL); |
| | | return false; |
| | | } |