| | |
| | | yaw += car->yaw; |
| | | } |
| | | yaw_stat++; |
| | | |
| | | DEBUG("角度矫正 car %f yaw %f", car->yaw, yaw); |
| | | |
| | | if (yaw_stat == 5) { |
| | | yaw = fmod(yaw, 360) / 5; |
| | | |
| | | DEBUG("创建基线 yaw %f", yaw); |
| | | PointF extPoint = PointExtend(car->basePoint, 100, yaw); |
| | | MakeLine(&baseLine, &car->basePoint, &extPoint); |
| | | beginOdo = ReadOdo(); |
| | | setup = 2; |
| | | } |
| | | } else if (setup == 2) { |
| | | if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine) > MAX_OFFSET_DISTANCE) { |
| | | DEBUG("直线偏移大于30厘米"); |
| | | double offset = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine); |
| | | |
| | | if (offset > MAX_OFFSET_DISTANCE) { |
| | | DEBUG("直线偏移大于30厘米 offset = %f", offset); |
| | | // 偏移大于30厘米,不合格 |
| | | AddExamFault(30, rtkTime); |
| | | return false; |