| | |
| | | DEBUG("加入点<%d> 距离 %f 角度 %f", i, CarModel->carDesc[i].distance, CarModel->carDesc[i].angle); |
| | | } |
| | | |
| | | CarModel->carDesc[0].distance = 0.2465; |
| | | CarModel->carDesc[0].angle = 0; |
| | | |
| | | CarModel->carDesc[1].distance = 0.2635; |
| | | CarModel->carDesc[1].angle = 20.7; |
| | | |
| | | CarModel->carDesc[2].distance = 0.14; |
| | | CarModel->carDesc[2].angle = 138.9; |
| | | |
| | | CarModel->carDesc[3].distance = 0.1055; |
| | | CarModel->carDesc[3].angle = 180.0; |
| | | |
| | | CarModel->carDesc[4].distance = 0.14; |
| | | CarModel->carDesc[4].angle = 221.1; |
| | | |
| | | CarModel->carDesc[5].distance = 0.2635; |
| | | CarModel->carDesc[5].angle = 339.3; |
| | | // CarModel->carDesc[0].distance = 0.2465; |
| | | // CarModel->carDesc[0].angle = 0; |
| | | // |
| | | // CarModel->carDesc[1].distance = 0.2635; |
| | | // CarModel->carDesc[1].angle = 20.7; |
| | | // |
| | | // CarModel->carDesc[2].distance = 0.14; |
| | | // CarModel->carDesc[2].angle = 138.9; |
| | | // |
| | | // CarModel->carDesc[3].distance = 0.1055; |
| | | // CarModel->carDesc[3].angle = 180.0; |
| | | // |
| | | // CarModel->carDesc[4].distance = 0.14; |
| | | // CarModel->carDesc[4].angle = 221.1; |
| | | // |
| | | // CarModel->carDesc[5].distance = 0.2635; |
| | | // CarModel->carDesc[5].angle = 339.3; |
| | | |
| | | DEBUG("SetCarMeasurePoint Calc Over"); |
| | | } |
| | |
| | | if (start == 0) { |
| | | DEBUG("-------------结束考试----------------"); |
| | | |
| | | CurrExamMapIndex = -1; |
| | | TestStart = false; |
| | | CommTestStart(false); |
| | | MA_SendExamStatus(0, 0); |
| | | // CurrExamMapIndex = -1; |
| | | // TestStart = false; |
| | | // CommTestStart(false); |
| | | // MA_SendExamStatus(0, 0); |
| | | return; |
| | | } |
| | | |
| | |
| | | |
| | | if (map_id >= 0 && exam == 0) { |
| | | CurrExamMapIndex = map_id; |
| | | CurrExamStatus = 0; |
| | | CurrExamStatus = 2; |
| | | } |
| | | } |
| | | |
| | |
| | | if (CurrEnterMapIndex >= 0) { |
| | | DEBUG("发送进入场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum)); |
| | | MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), 1); |
| | | |
| | | CurrExamMapIndex = CurrEnterMapIndex; |
| | | CurrExamStatus = 2; |
| | | } |
| | | } else { |
| | | if (ExitMap(CarModel, CurrEnterMapIndex, MapList, MapNum)) { |
| | |
| | | if (CurrExamMapIndex >= 0) { |
| | | int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum); |
| | | |
| | | if (CurrExamStatus == 0) { |
| | | if (CurrExamStatus == 2) { |
| | | DEBUG("CurrExamMapIndex %d mtype %d", GetMapId(CurrExamMapIndex, MapList, MapNum), mtype); |
| | | |
| | | switch (mtype) { |
| | |
| | | break; |
| | | default:break; |
| | | } |
| | | |
| | | CurrExamStatus = 1; |
| | | } else if (CurrExamStatus == 1) { |
| | | } else if (CurrExamStatus == 0) { |
| | | switch (mtype) { |
| | | case MAP_TYPE_PARK_BUTTOM: |
| | | CurrExamStatus = TestParkBottom(&MapList[CurrExamMapIndex].map, |
| | |
| | | CurrExamStatus = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, azimuth, speed, move, &rtkTime); |
| | | break; |
| | | default: |
| | | CurrExamStatus = -1; |
| | | CurrExamStatus = 1; |
| | | break; |
| | | } |
| | | } |
| | | |
| | | if (CurrExamStatus < 0) { |
| | | if (CurrExamStatus == 1) { |
| | | // 某项结束 |
| | | //DEBUG("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); |
| | | //MA_SendDebugInfo("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); |
| | |
| | | spd = speed; |
| | | mov = move; |
| | | idx = p1; |
| | | DEBUG("tmDiff = %ld speed = %f m/Sec move = %d", tmDiff, speed, move); |
| | | // DEBUG("tmDiff = %ld speed = %f m/Sec move = %d", tmDiff, speed, move); |
| | | |
| | | return true; |
| | | /* if (!TestStart) return; |
| | |
| | | // if (mapList[i].type == MAP_TYPE_CURVE) { |
| | | // |
| | | // } |
| | | if (mapList[i].type == MAP_TYPE_PARK_BUTTOM /*|| mapList[i].type == MAP_TYPE_PART_EDGE*/) { |
| | | if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) { |
| | | if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) { |
| | | Line enterLine1, enterLine2; |
| | | |
| | |
| | | return i; |
| | | } |
| | | } |
| | | // if (mapList[i].type == MAP_TYPE_PART_EDGE) { |
| | | // if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) { |
| | | // Line enterLine; |
| | | // |
| | | // MakeLine(&enterLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1])); |
| | | // |
| | | // if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine) > 0.1) |
| | | // return i; |
| | | // } |
| | | // } |
| | | // if (mapList[i].type == MAP_TYPE_TURN_90) { |
| | | // if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) { |
| | | // Line enterLine1; |
| | |
| | | projectAngle = 360 - projectAngle; |
| | | } |
| | | |
| | | double tx = projectDistance*sin(toRadians(azimuth)); |
| | | double ty = projectDistance*cos(toRadians(azimuth)); |
| | | // double tx = projectDistance*sin(toRadians(azimuth)); |
| | | // double ty = projectDistance*cos(toRadians(azimuth)); |
| | | |
| | | carModel->carXY[i].X = tx * cos(toRadians(projectAngle)) - |
| | | ty*sin(toRadians(projectAngle)) + main_ant.X; |
| | | carModel->carXY[i].Y = tx * sin(toRadians(projectAngle)) + |
| | | ty * cos(toRadians(projectAngle)) + main_ant.Y; |
| | | |
| | | // double tx = carModel->carDesc[i].distance*sin(toRadians(azimuth)); |
| | | // double ty = carModel->carDesc[i].distance*cos(toRadians(azimuth)); |
| | | // |
| | | // carModel->carXY[i].X = (tx)*cos(toRadians(carModel->carDesc[i].angle)) - |
| | | // (ty)*sin(toRadians(carModel->carDesc[i].angle)) + main_ant.X; |
| | | // carModel->carXY[i].Y = (tx)*sin(toRadians(carModel->carDesc[i].angle)) + |
| | | // (ty)*cos(toRadians(carModel->carDesc[i].angle)) + main_ant.Y; |
| | | carModel->carXY[i].X = projectDistance*sin(toRadians(azimuth)) * cos(toRadians(projectAngle)) - |
| | | projectDistance*cos(toRadians(azimuth)) * sin(toRadians(projectAngle)) + main_ant.X; |
| | | carModel->carXY[i].Y = projectDistance*sin(toRadians(azimuth)) * sin(toRadians(projectAngle)) + |
| | | projectDistance*cos(toRadians(azimuth)) * cos(toRadians(projectAngle)) + main_ant.Y; |
| | | } |
| | | } |
| | | |