updatecrrDOC1

yy1717

2020-01-21 7c66884c800b0b48c2b3aac2ac29ff0b4529b95e

updatecrrDOC1

 lib/src/main/cpp/CMakeLists.txt

@@ -34,6 +34,7 @@
        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

 lib/src/main/cpp/driver_test.cpp

@@ -74,7 +74,7 @@
} 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;
@@ -94,7 +94,8 @@
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)
{
@@ -201,6 +202,7 @@
        CarModelPrev = NULL;
    }

    // 仅分配carXY, body和carDesc均没有
    CarModelPrev = (car_model *)malloc(sizeof(car_model));
    CarModelPrev->body = NULL;
    CarModelPrev->carDesc = NULL;
@@ -337,7 +339,7 @@
                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;
@@ -374,16 +376,16 @@
        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;
@@ -411,12 +413,12 @@
        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:
@@ -432,10 +434,10 @@
                }
            }
            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;
                }
            }
        }
@@ -450,8 +452,8 @@
 */
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)
@@ -663,42 +665,48 @@

                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)

 lib/src/main/cpp/master/comm_if.cpp

@@ -48,7 +48,13 @@
#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)
{
@@ -149,7 +155,7 @@

void MA_SendGpsBrief(const struct gpsBrief *brief)
{
    if (!(OnOff & BV(0)))
    if (!(OnOff & MA_OUT_GPS_BRIEF))
        return;

    StringBuffer sb;
@@ -179,7 +185,7 @@

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};
@@ -240,7 +246,7 @@

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);
@@ -317,7 +323,7 @@

void MA_SendRtcmInd(int length)
{
    if (!(OnOff & BV(3))) return;
    if (!(OnOff & MA_OUT_RTCM_IND)) return;

    StringBuffer sb;
    Writer<StringBuffer> writer(sb);
@@ -332,7 +338,7 @@

void MA_SendDebugInfo(const char *str, ...)
{
    if (!(OnOff & BV(4)))
    if (!(OnOff & MA_OUT_DBG_INFO))
        return;

    va_list argp;
@@ -358,6 +364,8 @@
{
    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));


 lib/src/main/cpp/native-lib.cpp

@@ -15,6 +15,7 @@
#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)

@@ -243,6 +244,8 @@

    InitPlatform(phone, RTK_PLATFORM_IP, RTK_PLATFORM_PORT);
    AppTimer_add(SendBootIndicate, 500);

    InitVirtualDevice("192.168.16.104", 9001);
}

static void SendBootIndicate(union sigval sig) {

 lib/src/main/cpp/rtk_module/rtk.cpp

@@ -202,14 +202,17 @@
        }*/

        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) {

 lib/src/main/cpp/rtk_module/virtual_rtk.cpp

New file
@@ -0,0 +1,95 @@
//
// 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);
}

 lib/src/main/cpp/rtk_module/virtual_rtk.h

New file
@@ -0,0 +1,10 @@
//
// 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

 lib/src/main/cpp/test_items/park_bottom.cpp

@@ -292,44 +292,43 @@
    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("右轮跨越右控制线");
    }
}

@@ -373,7 +372,7 @@
    }

    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;