| | |
| | | |
| | | #define DEBUG(fmt, args...) LOGD("<road_exam drive_straight> <%s>: " fmt, __func__, ##args) |
| | | |
| | | static const double CHECK_STAGE_DISTANCE = 100.0; |
| | | static const double MAX_OFFSET_DISTANCE = 0.3; |
| | | |
| | | static int setup; |
| | | static double beginOdo; |
| | | static int yaw_stat; |
| | |
| | | DEBUG("开始直线行驶"); |
| | | setup = 0; |
| | | yaw_stat = 0; |
| | | PlayTTS("请保持直线行驶", TtsBack); |
| | | PlayTTS(examParam.straight_begin_tts, TtsBack); |
| | | } |
| | | |
| | | bool TestDriveStraight(road_exam_map &RoadMap, int roadIndex, const car_model *car, const struct RtkTime *rtkTime) { |
| | |
| | | } else if (setup == 2) { |
| | | offset1 = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine); |
| | | |
| | | if (offset1 > MAX_OFFSET_DISTANCE) { |
| | | if (offset1 > examParam.straight_max_offset) { |
| | | DEBUG("虚拟直线偏移大于30厘米 offset1 = %f", offset1); |
| | | // // 偏移大于30厘米,不合格 |
| | | // AddExamFault(30, rtkTime); |
| | |
| | | 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) { |
| | | if (fabs(offset2 - offsetBase) > examParam.straight_max_offset) { |
| | | DEBUG("直线偏移大于30厘米 offset2 = %f", fabs(offset2 - offsetBase)); |
| | | // 偏移大于30厘米,不合格 |
| | | AddExamFault(30, rtkTime); |
| | |
| | | } |
| | | } |
| | | |
| | | if (setup == 2 && ReadOdo() - beginOdo > CHECK_STAGE_DISTANCE) { |
| | | if (setup == 2 && ReadOdo() - beginOdo > examParam.straight_limit_distance) { |
| | | DEBUG("直线行驶结束 offset1 = %f offset2 = %f", offset1, fabs(offset2 - offsetBase)); |
| | | |
| | | PlayTTS("直线行驶结束", NULL); |
| | | PlayTTS(examParam.straight_end_tts, NULL); |
| | | return false; |
| | | } |
| | | |