| | |
| | | bool err = false; |
| | | |
| | | DEBUG("StartDriverExam %d", start); |
| | | if (start == 0) { |
| | | TestStart = false; |
| | | MA_SendExamStatus(0, 0); |
| | | return; |
| | | } |
| | | |
| | | if (MapNum == 0) { |
| | | err = true; |
| | |
| | | |
| | | void UpdateRTKInfo(const rtk_info *s) |
| | | { |
| | | if (s->qf > 0) { |
| | | if (s->qf == 3) { |
| | | RtkBuffer[RtkBufferIn] = *s; |
| | | RtkBufferIn = (RtkBufferIn + 1) % RTK_BUFFER_SIZE; |
| | | if (RtkBufferNum < RTK_BUFFER_SIZE) |
| | |
| | | } |
| | | } |
| | | if (CurrExamStatus != 0) { |
| | | // 某项结束, |
| | | if (ExitMap(CarModel, CurrExamMapIndex, MapList, MapNum)) { |
| | | DEBUG("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); |
| | | MA_SendDebugInfo("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); |
| | |
| | | |
| | | static bool UpdateCarCoord(double &spd, int &mov, int &idx) |
| | | { |
| | | long tmDiff; |
| | | |
| | | if (CarModel == NULL) |
| | | return false; |
| | | |
| | | if (RtkBufferNum < 3) |
| | | if (RtkBufferNum < 2) |
| | | return false; |
| | | |
| | | int p1 = ((RtkBufferIn-1)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE; |
| | | int p2 = ((RtkBufferIn-2)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE; |
| | | int p3 = ((RtkBufferIn-3)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE; |
| | | int p1 = ((RtkBufferIn-1)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE; // 最近采样值 |
| | | int p2 = ((RtkBufferIn-2)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE; // 找到1秒前的采样值 |
| | | int pn = 0; |
| | | |
| | | for (pn = 1; pn < RtkBufferNum; ++pn) { |
| | | p2 = ((RtkBufferIn-1-pn)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE; |
| | | if ((tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2])) >= D_SEC(1)) break; |
| | | } |
| | | |
| | | if (pn == RtkBufferNum) |
| | | return false; |
| | | |
| | | // 如果一定的时间都没有有效定位,删除之前的值 |
| | | uint32_t tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2]); |
| | | /*tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2]); |
| | | |
| | | DEBUG("tmDiff = %ld, p1 = %d p2 = %d dss = %d dss2 = %d", tmDiff, p1, p2, RtkBuffer[p1].dss, RtkBuffer[p2].dss); |
| | | |
| | |
| | | RtkBuffer[0] = RtkBuffer[p1]; |
| | | RtkBufferIn = RtkBufferNum = 1; |
| | | return false; |
| | | } |
| | | }*/ |
| | | |
| | | // 计算车辆轮廓点 |
| | | PointF main_ant_coord; |
| | |
| | | spd = speed; |
| | | mov = move; |
| | | idx = p1; |
| | | DEBUG("speed = %f m/Sec move = %d", speed, move); |
| | | DEBUG("tmDiff = %ld speed = %f m/Sec move = %d", tmDiff, speed, move); |
| | | |
| | | return true; |
| | | /* if (!TestStart) return; |
| | |
| | | |
| | | static bool ExitMap(const car_model *car, int index, const struct map_list *mapList, int mapNum) |
| | | { |
| | | // 车的最后点是否进入地图 |
| | | bool ret = false; |
| | | if (index < 0 || mapList == NULL || mapNum == 0) return true; |
| | | |
| | | if (mapList[index].type == MAP_TYPE_PARK_BUTTOM) { |
| | | if (IntersectionOf(car->carXY[car->axial[AXIAL_REAR]], &mapList[index].map) == GM_None) { |
| | | Line enterLine1, enterLine2; |
| | | // 全车都需不在地图中 |
| | | Polygon carBody; |
| | | |
| | | MakeLine(&enterLine1, &(mapList[index].map.point[0]), &(mapList[index].map.point[1])); |
| | | MakeLine(&enterLine2, &(mapList[index].map.point[6]), &(mapList[index].map.point[7])); |
| | | |
| | | if (DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine1) > 0.1 && |
| | | DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine2) > 0.1) |
| | | return true; |
| | | carBody.num = car->bodyNum; |
| | | carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF)); |
| | | for (int i = 0; i < carBody.num; ++i) { |
| | | carBody.point[i] = car->carXY[car->body[i]]; |
| | | } |
| | | |
| | | if (IntersectionOf(&carBody, &mapList[index].map) == GM_None) { |
| | | ret = true; |
| | | } |
| | | |
| | | free(carBody.point); |
| | | } |
| | | |
| | | return true; |
| | | return ret; |
| | | } |
| | | |
| | | static int GetMapId(int index, const struct map_list *mapList, int mapNum) |
| | |
| | | uint32_t downloadRtk : 1; |
| | | } platformStatus; |
| | | |
| | | static struct event_queue_t { |
| | | uint32_t event; |
| | | int length; |
| | | void *data; |
| | | struct event_queue_t *next; |
| | | } *eventQueue; |
| | | |
| | | struct platformSocket exceptSocket, currSocket; |
| | | static uint32_t eventMask; |
| | | static void *eventData[32]; |
| | |
| | | static void TriggerHeartbeat(union sigval sig); |
| | | static void RequestRtkNoResp(union sigval sig); |
| | | |
| | | void AddEvnet(uint32_t event, const uint8_t *data, int length) |
| | | { |
| | | struct event_queue_t *p = eventQueue; |
| | | |
| | | while (p != NULL) { |
| | | struct event_queue_t *nw = (struct event_queue_t *)malloc(sizeof(struct event_queue_t)); |
| | | |
| | | nw->event = event; |
| | | nw->next = NULL; |
| | | nw->data = NULL; |
| | | nw->length = 0; |
| | | if (data != NULL && length > 0) { |
| | | nw->data = malloc(length); |
| | | nw->length = length; |
| | | } |
| | | } |
| | | } |
| | | |
| | | void InitPlatform(const uint8_t *phone, const char *domain_name, int port) |
| | | { |
| | | DEBUG("InitPlatform"); |
| | |
| | | pthread_mutex_init(&platform_tx_mutex, NULL); |
| | | pthread_mutex_init(&events_mutex, NULL); |
| | | |
| | | eventQueue = NULL; |
| | | |
| | | eventMask = 0; |
| | | memset(eventData, 0, sizeof(eventData)); |
| | | memset(eventDataLength, 0, sizeof(eventDataLength)); |
| | |
| | | // 是否超时 |
| | | if (parkTimeout) { |
| | | // 不合格:动作超时 |
| | | DEBUG("不合格:动作超时"); |
| | | err.push_back(10); |
| | | AddExamFault(10, rtkTime); |
| | | status = -1; |
| | |
| | | // 是否压线 |
| | | if (CrashRedLine(map, car)) { |
| | | // 不合格:车身出线 |
| | | DEBUG("不合格:车身出线"); |
| | | err.push_back(7); |
| | | AddExamFault(7, rtkTime); |
| | | status = -1; |
| | |
| | | if (run_status > 0) { |
| | | if (stopCar2S && currTarget != FIRST_TOUCH_CTRL_LINE) { |
| | | // 扣5分:中途停车超过2秒 |
| | | DEBUG("扣5分:中途停车超过2秒"); |
| | | err.push_back(11); |
| | | AddExamFault(11, rtkTime); |
| | | } |
| | |
| | | DEBUG("来道场玩的"); |
| | | } else { |
| | | // 不合格:未按规定线路行驶(直接跑出测试区了) |
| | | DEBUG("不合格:未按规定线路行驶(直接跑出测试区了)"); |
| | | err.push_back(6); |
| | | AddExamFault(6, rtkTime); |
| | | status = -1; |
| | |
| | | status = 1; |
| | | } else if (the_ctrl_line_crossed != 0) { |
| | | // 不合格:未按规定线路行驶(未回到起始点) |
| | | DEBUG("不合格:未按规定线路行驶(未回到起始点)"); |
| | | err.push_back(6); |
| | | AddExamFault(6, rtkTime); |
| | | status = -1; |
| | |
| | | |
| | | if (first_ctrl_line_id > 0 && first_ctrl_line_id == the_ctrl_line_crossed) { |
| | | // 不合格:未按规定线路行驶(试图做2次同方向的倒库) |
| | | DEBUG("不合格:未按规定线路行驶(试图做2次同方向的倒库)"); |
| | | err.push_back(6); |
| | | AddExamFault(6, rtkTime); |
| | | status = -1; |
| | |
| | | DEBUG("第二次倒库"); |
| | | } else if (currTarget != THIRD_TOUCH_CTRL_LINE) { |
| | | // 不合格:倒车前,2前轮没驶过控制线 |
| | | DEBUG("不合格:倒车前,2前轮没驶过控制线"); |
| | | err.push_back(9); |
| | | AddExamFault(9, rtkTime); |
| | | status = -1; |
| | |
| | | if (run_status < 0) { |
| | | if (stopCar2S) { |
| | | // 扣5分:中途停车 |
| | | DEBUG("扣5分:中途停车"); |
| | | err.push_back(11); |
| | | AddExamFault(11, rtkTime); |
| | | } |
| | |
| | | if (carStopEvent) { |
| | | if (!carParkSuccess) { |
| | | // 不合格:倒库不入 |
| | | DEBUG("不合格:倒库不入"); |
| | | err.push_back(8); |
| | | AddExamFault(8, rtkTime); |
| | | status = -1; |
| | |
| | | p2.Y = (prev_car->carXY[car->left_front_tire[TIRE_OUTSIDE]].Y + prev_car->carXY[car->left_front_tire[TIRE_INSIDE]].Y) / 2; |
| | | MakeLine(&track1, &p1, &p2); |
| | | if (IntersectionOf(track1, leftCtrlLine) == GM_Intersection && |
| | | IntersectionOfLine(map->point[0], map->point[1], p1) == -1 |
| | | /*IntersectionOf(p1, map) == GM_None*/) { |
| | | IntersectionOfLine(map->point[0], map->point[1], p1) == -1) { |
| | | leftTireCrossLeftLine = true; |
| | | DEBUG("左轮跨越左控制线"); |
| | | } |
| | | if (IntersectionOf(track1, rightCtrlLine) == GM_Intersection && |
| | | IntersectionOfLine(map->point[6], map->point[7], p1) == -1 |
| | | /*IntersectionOf(p1, map) == GM_None*/) { |
| | | IntersectionOfLine(map->point[6], map->point[7], p1) == -1) { |
| | | leftTireCrossRightLine = true; |
| | | DEBUG("左轮跨越右控制线"); |
| | | } |
| | |
| | | p2.Y = (prev_car->carXY[car->right_front_tire[TIRE_OUTSIDE]].Y + prev_car->carXY[car->right_front_tire[TIRE_INSIDE]].Y) / 2; |
| | | MakeLine(&track1, &p1, &p2); |
| | | if (IntersectionOf(track1, leftCtrlLine) == GM_Intersection && |
| | | IntersectionOfLine(map->point[0], map->point[1], p1) == -1 |
| | | /*IntersectionOf(p1, map) == GM_None*/) { |
| | | IntersectionOfLine(map->point[0], map->point[1], p1) == -1) { |
| | | rightTireCrossLeftLine = true; |
| | | DEBUG("右轮跨越左控制线"); |
| | | } |
| | | if (IntersectionOf(track1, rightCtrlLine) == GM_Intersection && |
| | | IntersectionOfLine(map->point[6], map->point[7], p1) == -1 |
| | | /*IntersectionOf(p1, map) == GM_None*/) { |
| | | IntersectionOfLine(map->point[6], map->point[7], p1) == -1) { |
| | | rightTireCrossRightLine = true; |
| | | DEBUG("右轮跨越右控制线"); |
| | | } |