实现rtk平台基础配置

yy1717

2020-01-09 74bb2be0e23e9f2290ff8ecfb6506acf8a070339

实现rtk平台基础配置

 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)

 lib/src/main/cpp/driver_test.h

@@ -45,6 +45,14 @@
} car_model_cache_t;

void DriverTestInit(void);
void ReadDriverExamPrimer(void);
void ClearMap(void);
void AddMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2);
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);
void SetSensorCfg(int (*sensor)[2], int sensorNum);

void UpdateRTKInfo(struct rtk_info *s);
void UpdateCarCoord(void);
car_model_cache_t *GetCarModelCache(int node);

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

@@ -10,6 +10,7 @@
#include "../rapidjson/stringbuffer.h"
#include "../rtk_platform/platform.h"
#include "../utils/xconvert.h"
#include "../driver_test.h"

#include <string>
#include <iostream>
@@ -26,6 +27,13 @@
#define ID_MS_MAP               0x8006
#define ID_SM_READ_CAR          0x0007
#define ID_MS_CAR               0x8007
#define ID_SM_READ_SENSOR_CFG   0x0008
#define ID_MS_SENSOR_CFG        0x8008
#define ID_MS_START_EXAM        0x8009
#define ID_SM_EXAM_STATUS       0x0009
#define ID_SM_GPS_BRIEF         0x000A
#define ID_SM_RTK_BRIEF         0x000B
#define ID_SM_EXAM_BRIEF         0x000B

void MA_NdkStart(void)
{
@@ -102,6 +110,11 @@
    SendMsgToMainProc(ID_SM_READ_CAR, NULL);
}

void MA_ReadSensor(void)
{
    SendMsgToMainProc(ID_SM_READ_SENSOR_CFG, NULL);
}

void MA_MainProcMsgEntry(int cmd, const char *value)
{
    switch (cmd) {
@@ -160,5 +173,203 @@
            ConfigPlatform(&cfg);
            break;
        }
        case ID_MS_MAP: {
            Document doc;
            doc.Parse(value);
            if (!doc.HasParseError()) {
                ClearMap();

                const Value &a = doc.GetArray();

                if (a.IsArray()) {
                    for (Value::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr) {
                        // a Map
                        int id, type, pointNum = 0, point2Num = 0;
                        double (*map)[2] = NULL, (*map2)[2] = NULL;

                        if (itr->IsObject()) {
                            if (itr->HasMember("id")) {
                                const Value &s = (*itr)["id"];
                                id = s.GetInt();
                            }
                            if (itr->HasMember("item")) {
                                const Value &s = (*itr)["item"];
                                type = s.GetInt();
                            }
                            if (itr->HasMember("point")) {
                                const Value &s = (*itr)["point"];
                                int map_index = 0;

                                for (Value::ConstValueIterator itr2 = s.Begin();
                                     itr2 != s.End(); ++itr2, ++map_index) {
                                    // 曲线驾驶有2组
                                    const Value &s2 = (*itr2)["x-y"];

                                    if (map_index == 0) {
                                        int i = 0, j = 0;
                                        pointNum = s2.Size()/2;
                                        map = new double[pointNum][2];
                                        for (Value::ConstValueIterator itr3 = s2.Begin();
                                             itr3 != s2.End(); ++itr3) {
                                            map[i][j] = (*itr3).GetDouble();
                                            if (++j == 2) {
                                                j = 0;
                                                i++;
                                            }
                                        }
                                    } else if (map_index == 1) {
                                        int i = 0, j = 0;
                                        point2Num = s2.Size()/2;
                                        map2 = new double[s2.Size()][2];
                                        for (Value::ConstValueIterator itr3 = s2.Begin();
                                             itr3 != s2.End(); ++itr3) {
                                            map2[i][j] = (*itr3).GetDouble();
                                            if (++j == 2) {
                                                j = 0;
                                                i++;
                                            }
                                        }
                                    }
                                }
                            }

                            AddMap(id, type, map, pointNum, map2, point2Num);
                            if (map) delete []map;
                            if (map2) delete []map2;
                        }
                    }
                }
            }
            break;
        }
        case ID_MS_CAR: {
            Document doc;

            doc.Parse(value);

            if (!doc.HasParseError()) {
                double basePoint[2];
                int axial[2], left_front_tire[2], right_front_tire[2], left_rear_tire[2], right_rear_tire[2];
                int pointNum = 0;
                double (*point)[2] = NULL;

                if (doc.HasMember("main_ant")) {
                    const Value& s = doc["main_ant"];
                    if (s.IsArray()) {
                        int i = 0;
                        for(Value::ConstValueIterator itr = s.Begin(); itr != s.End(); ++itr) {
                            if (i < 2) basePoint[i++] = itr->GetDouble();
                        }
                    }
                }

                if (doc.HasMember("axial")) {
                    const Value& s = doc["axial"];
                    if (s.IsArray()) {
                        int i = 0;
                        for(Value::ConstValueIterator itr = s.Begin(); itr != s.End(); ++itr) {
                            if (i < 2) axial[i++] = itr->GetInt();
                        }
                    }
                }

                if (doc.HasMember("left_front_tire")) {
                    const Value& s = doc["left_front_tire"];
                    if (s.IsArray()) {
                        int i = 0;
                        for(Value::ConstValueIterator itr = s.Begin(); itr != s.End(); ++itr) {
                            if (i < 2) left_front_tire[i++] = itr->GetInt();
                        }
                    }
                }

                if (doc.HasMember("right_front_tire")) {
                    const Value& s = doc["right_front_tire"];
                    if (s.IsArray()) {
                        int i = 0;
                        for(Value::ConstValueIterator itr = s.Begin(); itr != s.End(); ++itr) {
                            if (i < 2) right_front_tire[i++] = itr->GetInt();
                        }
                    }
                }

                if (doc.HasMember("left_rear_tire")) {
                    const Value& s = doc["left_rear_tire"];
                    if (s.IsArray()) {
                        int i = 0;
                        for(Value::ConstValueIterator itr = s.Begin(); itr != s.End(); ++itr) {
                            if (i < 2) left_rear_tire[i++] = itr->GetInt();
                        }
                    }
                }

                if (doc.HasMember("right_rear_tire")) {
                    const Value& s = doc["right_rear_tire"];
                    if (s.IsArray()) {
                        int i = 0;
                        for(Value::ConstValueIterator itr = s.Begin(); itr != s.End(); ++itr) {
                            if (i < 2) right_rear_tire[i++] = itr->GetInt();
                        }
                    }
                }

                if (doc.HasMember("point")) {
                    const Value& s = doc["point"];
                    if (s.IsArray()) {
                        int i = 0, j = 0;
                        pointNum = s.Size()/2;
                        point = new double[pointNum][2];

                        for(Value::ConstValueIterator itr = s.Begin(); itr != s.End(); ++itr) {
                            point[i][j] = itr->GetDouble();
                            if (++j == 2) {
                                j = 0;
                                i++;
                            }
                        }
                    }
                }

                SetCarMeasurePoint(basePoint, axial, left_front_tire, right_front_tire,
                        left_rear_tire, right_rear_tire, point, pointNum);

                if (point != NULL) delete []point;
            }
            break;
        }
        case ID_MS_SENSOR_CFG: {
            Document doc;
            doc.Parse(value);
            if (!doc.HasParseError()) {
                const Value &a = doc.GetArray();

                if (a.IsArray() && a.Size() > 0) {
                    int n = a.Size();
                    int i = 0;
                    int (*sensor)[2] = new int[n][2];

                    for (Value::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr) {
                        // a gpio mapping
                        if (itr->HasMember("gpio_num")) {
                            const Value &s = (*itr)["gpio_num"];
                            sensor[i][0] = s.GetInt();
                        }
                        if (itr->HasMember("func_id")) {
                            const Value &s = (*itr)["func_id"];
                            sensor[i][1] = s.GetInt();
                        }
                        ++i;
                    }
                    SetSensorCfg(sensor, n);
                    delete []sensor;
                }
            }
            break;
        }
        case ID_MS_START_EXAM: {

            break;
        }
        default:break;
    }
}

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

@@ -16,5 +16,6 @@
void MA_RtkPlatformLogin(int login);
void MA_ReadMap(void);
void MA_ReadCar(void);
void MA_ReadSensor(void);

#endif //MYAPPLICATION2_COMM_IF_H

 lib/src/main/cpp/rtk_platform/platform.cpp

@@ -19,6 +19,7 @@
#include "../master/comm_if.h"
#include "../utils/xconvert.h"
#include "../utils/num.h"
#include "../driver_test.h"

#define PARSE_BUFF_SIZE         4096

@@ -109,9 +110,6 @@

    AppTimer_add(ReqRtkPlatformConfigTimeout, D_SEC(2));
    MA_ReqRtkPlatformConfig();

    MA_ReadMap();
    MA_ReadCar();
}

static void ReqRtkPlatformConfigTimeout(union sigval sig)
@@ -125,6 +123,7 @@
void ConfigPlatform(const rtk_platform_cfg_t *p)
{
    DEBUG("ConfigPlatform");

    AppTimer_delete(ReqRtkPlatformConfigTimeout);

    strcpy(exceptSocket.domain_name, p->domain_name);