| | |
| | | test_items/driving_curve.cpp |
| | | test_items/turn_a90.cpp |
| | | rtk_module/rtk.cpp |
| | | rtk_module/virtual_rtk.cpp |
| | | master/comm_if.cpp |
| | | |
| | | utils/crc16.cpp |
| | |
| | | } MapList[MAP_LIST_SIZE]; |
| | | |
| | | static int MapNum = 0; |
| | | static int CurrExamMapId = -1; |
| | | static int CurrExamMapIndex = -1; |
| | | static int CurrExamStatus = 0; // 1 测试完成 0 测试中 -1 测试错误退出 |
| | | |
| | | static car_model *CarModel = NULL; |
| | |
| | | static bool FrontTireEnterArea(const Polygon *car, const Polygon *map); |
| | | static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum); |
| | | static bool ExitMap(const car_model *car, int mapId, const struct map_list *mapList, int mapNum); |
| | | static int GetMapType(int id, const struct map_list *mapList, int mapNum); |
| | | static int GetMapId(int index, const struct map_list *mapList, int mapNum); |
| | | static int GetMapType(int index, const struct map_list *mapList, int mapNum); |
| | | |
| | | void DriverTestInit(void) |
| | | { |
| | |
| | | CarModelPrev = NULL; |
| | | } |
| | | |
| | | // 仅分配carXY, body和carDesc均没有 |
| | | CarModelPrev = (car_model *)malloc(sizeof(car_model)); |
| | | CarModelPrev->body = NULL; |
| | | CarModelPrev->carDesc = NULL; |
| | |
| | | RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss); |
| | | |
| | | brief.qf = RtkBuffer[index].qf; |
| | | brief.map_id = 863;//CurrExamMapId; |
| | | brief.map_id = 863;//GetMapId(CurrExamMapIndex, MapList, MapNum); |
| | | brief.move = move; |
| | | brief.speed = speed * 3.6; |
| | | brief.heading = RtkBuffer[index].heading; |
| | |
| | | free(brief.point); |
| | | |
| | | if (TestStart) { |
| | | if (CurrExamMapId < 0) { |
| | | CurrExamMapId = EnterMap(CarModel, MapList, MapNum); |
| | | int mtype = GetMapType(CurrExamMapId, MapList, MapNum); |
| | | if (CurrExamMapIndex < 0) { |
| | | CurrExamMapIndex = EnterMap(CarModel, MapList, MapNum); |
| | | int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum); |
| | | |
| | | DEBUG("CurrExamMapId %d mtype %d", CurrExamMapId, mtype); |
| | | DEBUG("CurrExamMapIndex %d mtype %d", GetMapId(CurrExamMapIndex, MapList, MapNum), mtype); |
| | | |
| | | switch (mtype) { |
| | | case MAP_TYPE_PARK_BUTTOM: |
| | | DEBUG("进入倒车入库场地"); |
| | | MA_SendDebugInfo("进入倒车入库场地 %d", CurrExamMapId); |
| | | MA_SendDebugInfo("进入倒车入库场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); |
| | | |
| | | StartParkBottom(); |
| | | CurrExamStatus = 0; |
| | |
| | | rtkTime.ss = RtkBuffer[index].ss; |
| | | rtkTime.mss = RtkBuffer[index].dss; |
| | | |
| | | if (CurrExamMapId >= 0) { |
| | | if (CurrExamMapIndex >= 0) { |
| | | if (CurrExamStatus == 0) { |
| | | int mtype = GetMapType(CurrExamMapId, MapList, MapNum); |
| | | int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum); |
| | | switch (mtype) { |
| | | case MAP_TYPE_PARK_BUTTOM: |
| | | CurrExamStatus = TestParkBottom(ErrorList, &MapList[CurrExamMapId].map, |
| | | CurrExamStatus = TestParkBottom(ErrorList, &MapList[CurrExamMapIndex].map, |
| | | CarModel, CarModelPrev, speed, move, &rtkTime); |
| | | break; |
| | | case MAP_TYPE_STOP_START: |
| | |
| | | } |
| | | } |
| | | if (CurrExamStatus != 0) { |
| | | if (ExitMap(CarModel, CurrExamMapId, MapList, MapNum)) { |
| | | DEBUG("退出场地 %d", CurrExamMapId); |
| | | MA_SendDebugInfo("退出场地 %d", CurrExamMapId); |
| | | CurrExamMapId = -1; |
| | | if (ExitMap(CarModel, CurrExamMapIndex, MapList, MapNum)) { |
| | | DEBUG("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); |
| | | MA_SendDebugInfo("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); |
| | | CurrExamMapIndex = -1; |
| | | } |
| | | } |
| | | } |
| | |
| | | */ |
| | | static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b) |
| | | { |
| | | return ABS(TimeMakeComposite(2000 + a->YY, a->MM, a->DD, a->hh, a->mm, a->ss) * 1000 + a->dss*10 - |
| | | (TimeMakeComposite(2000 + b->YY, b->MM, b->DD, b->hh, b->mm, b->ss) * 1000 + b->dss*10)); |
| | | return ABS((long)(TimeMakeComposite(2000 + a->YY, a->MM, a->DD, a->hh, a->mm, a->ss) * 1000 + a->dss*10) - |
| | | (long)(TimeMakeComposite(2000 + b->YY, b->MM, b->DD, b->hh, b->mm, b->ss) * 1000 + b->dss*10)); |
| | | } |
| | | |
| | | static bool UpdateCarCoord(double &spd, int &mov, int &idx) |
| | |
| | | |
| | | if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 && |
| | | DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1) |
| | | return mapList[i].id; |
| | | return i; |
| | | } |
| | | } |
| | | } |
| | | return -1; |
| | | } |
| | | |
| | | static bool ExitMap(const car_model *car, int mapId, const struct map_list *mapList, int mapNum) |
| | | static bool ExitMap(const car_model *car, int index, const struct map_list *mapList, int mapNum) |
| | | { |
| | | // 车的最后点是否进入地图 |
| | | for (int i = 0; i < mapNum && car != NULL; ++i) { |
| | | if (mapList[i].id == mapId) { |
| | | if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) { |
| | | if (IntersectionOf(car->carXY[ car->axial[AXIAL_REAR] ], &mapList[i].map) == GM_None) { |
| | | Line enterLine1, enterLine2; |
| | | if (index < 0 || mapList == NULL || mapNum == 0) return true; |
| | | |
| | | MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1])); |
| | | MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7])); |
| | | if (mapList[index].type == MAP_TYPE_PARK_BUTTOM) { |
| | | if (IntersectionOf(car->carXY[car->axial[AXIAL_REAR]], &mapList[index].map) == GM_None) { |
| | | Line enterLine1, enterLine2; |
| | | |
| | | if (DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine1) > 0.1 && |
| | | DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine2) > 0.1) |
| | | return true; |
| | | } |
| | | } |
| | | return false; |
| | | 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; |
| | | } |
| | | } |
| | | |
| | | return true; |
| | | } |
| | | |
| | | static int GetMapType(int id, const struct map_list *mapList, int mapNum) |
| | | static int GetMapId(int index, const struct map_list *mapList, int mapNum) |
| | | { |
| | | for (int i = 0; i < mapNum && id >= 0; ++i) { |
| | | if (mapList[i].id == id) return mapList[i].type; |
| | | } |
| | | return -1; |
| | | if (index < 0 || mapList == NULL || mapNum == 0) |
| | | return -1; |
| | | |
| | | return mapList[index].id; |
| | | } |
| | | |
| | | static int GetMapType(int index, const struct map_list *mapList, int mapNum) |
| | | { |
| | | if (index < 0 || mapList == NULL || mapNum == 0) |
| | | return -1; |
| | | |
| | | return mapList[index].type; |
| | | } |
| | | |
| | | void AddExamFault(int wrong, const struct RtkTime *rtkTime) |
| | |
| | | #define ID_SM_RTCM_IND 0x000F |
| | | #define ID_SM_DEBUG_INFO 0x0010 |
| | | |
| | | static int OnOff = 0xFFFF; |
| | | #define MA_OUT_GPS_BRIEF 0x0001 |
| | | #define MA_OUT_RTK_BRIEF 0x0002 |
| | | #define MA_OUT_CAR_BRIEF 0x0004 |
| | | #define MA_OUT_RTCM_IND 0x0008 |
| | | #define MA_OUT_DBG_INFO 0x0010 |
| | | |
| | | static int OnOff = 0;//0xFFFF; |
| | | |
| | | void MA_NdkStart(void) |
| | | { |
| | |
| | | |
| | | void MA_SendGpsBrief(const struct gpsBrief *brief) |
| | | { |
| | | if (!(OnOff & BV(0))) |
| | | if (!(OnOff & MA_OUT_GPS_BRIEF)) |
| | | return; |
| | | |
| | | StringBuffer sb; |
| | |
| | | |
| | | void MA_SendRtkBrief(const struct rtkBrief *brief) |
| | | { |
| | | if (!(OnOff & BV(1))) |
| | | if (!(OnOff & MA_OUT_RTK_BRIEF)) |
| | | return; |
| | | |
| | | char a[2] = {brief->coord_x_dir, 0}; |
| | |
| | | |
| | | void MA_SendCarPosition(const struct carBrief *brief) |
| | | { |
| | | if (!(OnOff & BV(2))) return; |
| | | if (!(OnOff & MA_OUT_CAR_BRIEF)) return; |
| | | |
| | | StringBuffer sb; |
| | | Writer<StringBuffer> writer(sb); |
| | |
| | | |
| | | void MA_SendRtcmInd(int length) |
| | | { |
| | | if (!(OnOff & BV(3))) return; |
| | | if (!(OnOff & MA_OUT_RTCM_IND)) return; |
| | | |
| | | StringBuffer sb; |
| | | Writer<StringBuffer> writer(sb); |
| | |
| | | |
| | | void MA_SendDebugInfo(const char *str, ...) |
| | | { |
| | | if (!(OnOff & BV(4))) |
| | | if (!(OnOff & MA_OUT_DBG_INFO)) |
| | | return; |
| | | |
| | | va_list argp; |
| | |
| | | { |
| | | switch (cmd) { |
| | | case ID_MS_RTK_PLAT_CFG: { |
| | | if (OnOff == 0) OnOff = MA_OUT_GPS_BRIEF + MA_OUT_RTK_BRIEF + MA_OUT_CAR_BRIEF; |
| | | |
| | | rtk_platform_cfg_t cfg; |
| | | memset(&cfg, 0, sizeof(cfg)); |
| | | |
| | |
| | | #include "mcu/mcu_if.h" |
| | | #include "driver_test.h" |
| | | #include "master/comm_if.h" |
| | | #include "rtk_module/virtual_rtk.h" |
| | | |
| | | #define DEBUG(fmt, args...) LOGD("<native-lib> <%s>: " fmt, __func__, ##args) |
| | | |
| | |
| | | |
| | | InitPlatform(phone, RTK_PLATFORM_IP, RTK_PLATFORM_PORT); |
| | | AppTimer_add(SendBootIndicate, 500); |
| | | |
| | | InitVirtualDevice("192.168.16.104", 9001); |
| | | } |
| | | |
| | | static void SendBootIndicate(union sigval sig) { |
| | |
| | | }*/ |
| | | |
| | | if (RxBufLen > 0) { |
| | | const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen); |
| | | if(ptr != RxBuf) { |
| | | memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf)); |
| | | RxBufLen -= ptr - RxBuf; |
| | | } else if(RxBufLen == PARSE_BUFF_SIZE) { //填满了,且没有一个\r,都抛弃 |
| | | DEBUG("Parse GPS error"); |
| | | RxBufLen = 0; |
| | | } |
| | | // const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen); |
| | | // if(ptr != RxBuf) { |
| | | // memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf)); |
| | | // RxBufLen -= ptr - RxBuf; |
| | | // } else if(RxBufLen == PARSE_BUFF_SIZE) { //填满了,且没有一个\r,都抛弃 |
| | | // DEBUG("Parse GPS error"); |
| | | // RxBufLen = 0; |
| | | // } |
| | | |
| | | |
| | | RxBufLen = 0; ///////////////////////////// |
| | | } |
| | | } |
| | | if (res == 0) { |
New file |
| | |
| | | // |
| | | // Created by YY on 2020/1/20. |
| | | // |
| | | |
| | | #include <pthread.h> |
| | | #include <cstdlib> |
| | | #include <cstring> |
| | | #include "virtual_rtk.h" |
| | | #include "../jni_log.h" |
| | | #include "../common/net.h" |
| | | #include "../native-lib.h" |
| | | #include "../common/apptimer.h" |
| | | #include "../defs.h" |
| | | #include "parse_gps.h" |
| | | |
| | | #define DEBUG(fmt, args...) LOGD("<virtual_device> <%s>: " fmt, __func__, ##args) |
| | | |
| | | struct vSocket { |
| | | char domain_name[32]; |
| | | int port; |
| | | }; |
| | | |
| | | static void *VDataListenThread(void *p); |
| | | |
| | | void InitVirtualDevice(const char *domain_name, int port) |
| | | { |
| | | DEBUG("InitVirtualDevice"); |
| | | |
| | | struct vSocket *ptr = (struct vSocket *)malloc(sizeof(struct vSocket)); |
| | | |
| | | strcpy(ptr->domain_name, domain_name); |
| | | ptr->port = port; |
| | | |
| | | pthread_t pid; |
| | | pthread_attr_t attr; |
| | | pthread_attr_init(&attr); |
| | | pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);//detached |
| | | pthread_create(&pid, &attr, VDataListenThread, ptr); |
| | | } |
| | | |
| | | #define PARSE_BUFF_SIZE 4096 |
| | | |
| | | static void *VDataListenThread(void *p) { |
| | | struct vSocket *vs = (struct vSocket *)p; |
| | | |
| | | uint8_t RxBuf[PARSE_BUFF_SIZE]; |
| | | |
| | | int fds_ret; |
| | | struct timeval tv; |
| | | fd_set rdfds; |
| | | fd_set exfds; |
| | | |
| | | int fd = -1; |
| | | int RxBufLen = 0; |
| | | |
| | | fd = ConnectTCP(vs->domain_name, vs->port); |
| | | |
| | | while (fd > 0) { |
| | | tv.tv_sec = 5; |
| | | tv.tv_usec = 0; |
| | | FD_ZERO(&rdfds); //clean |
| | | FD_SET(fd, &rdfds); //set |
| | | |
| | | fds_ret = select(fd + 1, &rdfds, NULL, NULL, &tv); |
| | | |
| | | if (fds_ret < 0) { |
| | | break; |
| | | } else if(fds_ret == 0) { |
| | | //Occur failure(such as line disconnect) |
| | | } else if(FD_ISSET(fd, &rdfds)) { |
| | | int lx = ReadTCP(fd, RxBuf + RxBufLen, sizeof(RxBuf) - RxBufLen); |
| | | |
| | | if (lx < 0) { |
| | | break; |
| | | } else if (lx > 0) { |
| | | RxBufLen += lx; |
| | | const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen); |
| | | if(ptr != RxBuf) { |
| | | memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf)); |
| | | RxBufLen -= ptr - RxBuf; |
| | | } else if(RxBufLen == PARSE_BUFF_SIZE) { //填满了,且没有一个\r,都抛弃 |
| | | DEBUG("Parse GPS error"); |
| | | RxBufLen = 0; |
| | | } |
| | | } |
| | | } |
| | | } |
| | | |
| | | if (fd > 0) { |
| | | DisconnectTCP(fd); |
| | | } |
| | | |
| | | free(vs); |
| | | pthread_exit(NULL); |
| | | } |
New file |
| | |
| | | // |
| | | // Created by YY on 2020/1/20. |
| | | // |
| | | |
| | | #ifndef MYAPPLICATION2_VIRTUAL_RTK_H |
| | | #define MYAPPLICATION2_VIRTUAL_RTK_H |
| | | |
| | | void InitVirtualDevice(const char *domain_name, int port); |
| | | |
| | | #endif //MYAPPLICATION2_VIRTUAL_RTK_H |
| | |
| | | Line track1; |
| | | PointF p1, p2; |
| | | |
| | | MakeLine(&leftCtrlLine, &map->point[0], &map->point[1]); |
| | | MakeLine(&rightCtrlLine, &map->point[6], &map->point[7]); |
| | | |
| | | MakeLine(&leftCtrlLine, &(map->point[0]), &(map->point[1])); |
| | | MakeLine(&rightCtrlLine, &(map->point[6]), &map->point[7]); |
| | | // 左前轮,取轮宽的中点 |
| | | p1.X = (car->carXY[car->left_front_tire[TIRE_OUTSIDE]].X + car->carXY[car->left_front_tire[TIRE_INSIDE]].X) / 2; |
| | | p1.Y = (car->carXY[car->left_front_tire[TIRE_OUTSIDE]].Y + car->carXY[car->left_front_tire[TIRE_INSIDE]].Y) / 2; |
| | | |
| | | p2.X = (prev_car->carXY[prev_car->left_front_tire[TIRE_OUTSIDE]].X + prev_car->carXY[prev_car->left_front_tire[TIRE_INSIDE]].X) / 2; |
| | | p2.Y = (prev_car->carXY[prev_car->left_front_tire[TIRE_OUTSIDE]].Y + prev_car->carXY[prev_car->left_front_tire[TIRE_INSIDE]].Y) / 2; |
| | | |
| | | p2.X = (prev_car->carXY[car->left_front_tire[TIRE_OUTSIDE]].X + prev_car->carXY[car->left_front_tire[TIRE_INSIDE]].X) / 2; |
| | | 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 && |
| | | IntersectionOf(p1, map) == GM_None) { |
| | | IntersectionOfLine(map->point[0], map->point[1], p1) == -1 |
| | | /*IntersectionOf(p1, map) == GM_None*/) { |
| | | leftTireCrossLeftLine = true; |
| | | DEBUG("左轮跨越左控制线"); |
| | | } |
| | | |
| | | if (IntersectionOf(track1, rightCtrlLine) == GM_Intersection && |
| | | IntersectionOf(p1, map) == GM_None) { |
| | | IntersectionOfLine(map->point[6], map->point[7], p1) == -1 |
| | | /*IntersectionOf(p1, map) == GM_None*/) { |
| | | leftTireCrossRightLine = true; |
| | | DEBUG("左轮跨越右控制线"); |
| | | } |
| | | |
| | | // 右前轮 |
| | | p1.X = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].X + car->carXY[car->right_front_tire[TIRE_INSIDE]].X) / 2; |
| | | p1.Y = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].Y + car->carXY[car->right_front_tire[TIRE_INSIDE]].Y) / 2; |
| | | |
| | | p2.X = (prev_car->carXY[prev_car->right_front_tire[TIRE_OUTSIDE]].X + prev_car->carXY[prev_car->right_front_tire[TIRE_INSIDE]].X) / 2; |
| | | p2.Y = (prev_car->carXY[prev_car->right_front_tire[TIRE_OUTSIDE]].Y + prev_car->carXY[prev_car->right_front_tire[TIRE_INSIDE]].Y) / 2; |
| | | |
| | | p2.X = (prev_car->carXY[car->right_front_tire[TIRE_OUTSIDE]].X + prev_car->carXY[car->right_front_tire[TIRE_INSIDE]].X) / 2; |
| | | 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 && |
| | | IntersectionOf(p1, map) == GM_None) { |
| | | IntersectionOfLine(map->point[0], map->point[1], p1) == -1 |
| | | /*IntersectionOf(p1, map) == GM_None*/) { |
| | | rightTireCrossLeftLine = true; |
| | | DEBUG("右轮跨越左控制线"); |
| | | } |
| | | if (IntersectionOf(track1, rightCtrlLine) == GM_Intersection && |
| | | IntersectionOf(p1, map) == GM_None) { |
| | | IntersectionOfLine(map->point[6], map->point[7], p1) == -1 |
| | | /*IntersectionOf(p1, map) == GM_None*/) { |
| | | rightTireCrossRightLine = true; |
| | | DEBUG("右轮跨越右控制线"); |
| | | } |
| | | } |
| | | |
| | |
| | | } |
| | | |
| | | for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) { |
| | | MakeLine(&red_line, &map->point[red_lines[i][0]], &map->point[red_lines[i][1]]); |
| | | MakeLine(&red_line, &(map->point[red_lines[i][0]]), &(map->point[red_lines[i][1]])); |
| | | if (IntersectionOf(red_line, &car_body) != GM_None) { |
| | | ret = true; |
| | | break; |