实现rtk平台基础配置

yy1717

2020-01-09 74bb2be0e23e9f2290ff8ecfb6506acf8a070339

 lib/src/main/cpp/driver_test.cpp

@@ -24,6 +24,7 @@
#include "test_items/turn_a90.h"
#include "test_items/driving_curve.h"
#include "test_items/stop_and_start.h"
#include "master/comm_if.h"

#define DEBUG(fmt, args...)     LOGD("<driver_test> <%s>: " fmt, __func__, ##args)

@@ -36,6 +37,7 @@
#define RTK_FLOAT            4

#define CAR_MODEL_POINT_NUM             32
#define MAP_LIST_SIZE               32

enum {
    TEST_NONE,
@@ -58,6 +60,16 @@
static Polygon theParkEdgeMap;         // 侧位停车地图
static Polygon theTurn90Map;
static Polygon theSSMap;

static struct map_list {
    int id;
    int type;
    Polygon map;
    Polygon map2;
} MapList[MAP_LIST_SIZE];

static int MapNum = 0;


#define CAR_COORD_STORE_SIZE        10

@@ -101,12 +113,16 @@
static car_model_cache_t carModelCache[CAR_MODEL_CACHE_SIZE];
static int carModelCacheIn, carModelCacheNum;

static void ReadDriverExamPrimerTimeout(union sigval sig);
static void UpdateCarBodyCoord(double azimuth, PointF coord);
static void UpdateCarBodyCoord(double azimuth, PointF coord, car_model_cache_t *carModel, int &carModelIn, int &carModelNum);
static bool FrontTireEnterArea(const Polygon *car, const Polygon *map);

void DriverTestInit(void)
{
    memset(&MapList, 0, sizeof(MapList));
    MapNum = 0;

    carModelDescFile = (carModelDesc_t *)malloc(sizeof(carModelDesc_t));

    carModelDescFile->body_num = 6;
@@ -243,6 +259,79 @@
    ErrorList.clear();

    TextSpeak("开始测试");

    AppTimer_add(ReadDriverExamPrimerTimeout, D_SEC(2));
    ReadDriverExamPrimer();
}

static void ReadDriverExamPrimerTimeout(union sigval sig)
{
    AppTimer_delete(ReadDriverExamPrimerTimeout);
    ReadDriverExamPrimer();
}

void ReadDriverExamPrimer(void)
{
//    MA_ReadMap();
//    MA_ReadCar();
//    MA_ReadSensor();
}

void ClearMap(void)
{
    for (int i = 0; i < MapNum; ++i) {
        if (MapList[i].map.point != NULL)
            free(MapList[i].map.point);
        if (MapList[i].map2.point != NULL)
            free(MapList[i].map2.point);
    }
    memset(&MapList, 0, sizeof(MapList));
    MapNum = 0;
}

void AddMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2)
{
    DEBUG("AddMap id %d type %d pointNum %d point2Num %d", id, type, pointNum, pointNum2);

    AppTimer_delete(ReadDriverExamPrimerTimeout);

    if (map == NULL || pointNum == 0)
        return;

    MapList[MapNum].id = id;
    MapList[MapNum].type = type;

    MapList[MapNum].map.num = pointNum;
    if (pointNum > 0) {
        MapList[MapNum].map.point = (PointF *)malloc(sizeof(PointF) * pointNum);
        for (int i = 0; i < pointNum; ++i) {
            MapList[MapNum].map.point[i].X = map[i][0];
            MapList[MapNum].map.point[i].Y = map[i][0];
        }
    }

    MapList[MapNum].map2.num = pointNum2;
    if (pointNum2 > 0 && map2 != NULL) {
        MapList[MapNum].map2.point = (PointF *)malloc(sizeof(PointF) * pointNum2);
        for (int i = 0; i < pointNum2; ++i) {
            MapList[MapNum].map2.point[i].X = map2[i][0];
            MapList[MapNum].map2.point[i].Y = map2[i][0];
        }
    }

    MapNum++;
}

void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
                    int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
                    double (*point)[2], int pointNum)
{
    DEBUG("SetCarMeasurePoint pointNum %d", pointNum);
}

void SetSensorCfg(int (*sensor)[2], int sensorNum)
{
    DEBUG("SetSensorCfg sensorNum %d", sensorNum);
}

void UpdateRTKInfo(struct rtk_info *s)