GPS信息合并到RTK中，以RTK包，发送

fctom1215

2020-02-21 814c006d124ef3ed344e790f1f74812fbaa11119

GPS信息合并到RTK中，以RTK包，发送

 lib/src/main/cpp/driver_test.cpp

@@ -27,6 +27,7 @@
#include "master/comm_if.h"
#include "utils/xconvert.h"
#include "test_items/comm_test.h"
#include "mcu/mcu_if.h"

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

@@ -76,6 +77,8 @@

static int MapNum = 0;
static int CurrExamMapIndex = -1;
static int CurrEnterMapIndex = -1;

static int CurrExamStatus = 0;      // 1 测试完成 0 测试中 -1 测试错误退出

static car_model *CarModel = NULL;
@@ -102,7 +105,7 @@

static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b);
static void ReadDriverExamPrimerTimeout(union sigval sig);
static void UpdateCarBodyCoord(double azimuth, PointF main_ant, car_model *carModel);
static void UpdateCarBodyCoord(double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel);
static bool UpdateCarCoord(double &spd, int &mov, int &idx);
static bool FrontTireEnterArea(const Polygon *car, const Polygon *map);
static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum);
@@ -358,6 +361,7 @@

    DEBUG("StartDriverExam %d", start);
    if (start == 0) {
        CurrExamMapIndex = -1;
        TestStart = false;
        CommTestStart(false);
        MA_SendExamStatus(0, 0);
@@ -381,6 +385,16 @@
            CommTestStart(true);
        }
        MA_SendExamStatus(1, 0);
    }
}

void StartMapExam(int map_id, int exam)
{
    DEBUG("测试该场地 %d: %d", map_id, exam);

    if (map_id >= 0 && exam == 0) {
        CurrExamMapIndex = map_id;
        CurrExamStatus = 0;
    }
}

@@ -473,9 +487,27 @@

        if (TestStart) {
            if (CurrExamMapIndex < 0) {
                CurrExamMapIndex = EnterMap(CarModel, MapList, MapNum);
                if (CurrEnterMapIndex < 0) {
                    CurrEnterMapIndex = EnterMap(CarModel, MapList, MapNum);
                    if (CurrEnterMapIndex >= 0) {
                        DEBUG("发送进入场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
                        MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), 1);
                    }
                } else {
                    if (ExitMap(CarModel, CurrEnterMapIndex, MapList, MapNum)) {
                        DEBUG("发送离开场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
                        MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), 0);
                        CurrEnterMapIndex = -1;
                    }
                }
            }
                //ExitMap(const car_model *car, int index, const struct map_list *mapList, int mapNum)
        }

        if (CurrExamMapIndex >= 0) {
                int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);

            if (CurrExamStatus == 0) {
                DEBUG("CurrExamMapIndex %d mtype %d", GetMapId(CurrExamMapIndex, MapList, MapNum), mtype);

                switch (mtype) {
@@ -509,12 +541,9 @@
                        break;
                    default:break;
                }
            }
        }

        if (CurrExamMapIndex >= 0) {
            if (CurrExamStatus == 0) {
                int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);
                CurrExamStatus = 1;
            } else if (CurrExamStatus == 1) {
                switch (mtype) {
                    case MAP_TYPE_PARK_BUTTOM:
                        CurrExamStatus = TestParkBottom(&MapList[CurrExamMapIndex].map,
@@ -532,16 +561,17 @@
                        CurrExamStatus = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, azimuth, speed, move, &rtkTime);
                        break;
                    default:
                        CurrExamStatus = -1;
                        break;
                }
            }
            if (CurrExamStatus != 0) {
                // 某项结束，
                if (ExitMap(CarModel, CurrExamMapIndex, MapList, MapNum)) {
                    DEBUG("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    MA_SendDebugInfo("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));

            if (CurrExamStatus < 0) {
                // 某项结束
                //DEBUG("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                //MA_SendDebugInfo("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));

                    CurrExamMapIndex = -1;
                }
            }
        }
    }
@@ -605,7 +635,7 @@
            CarModelPrev->carXY[i] = CarModel->carXY[i];
        }
    }
    UpdateCarBodyCoord(RtkBuffer[p1].heading, main_ant_coord, CarModel);
    UpdateCarBodyCoord(RtkBuffer[p1].heading, RtkBuffer[p1].pitch, RtkBuffer[p1].roll, main_ant_coord, CarModel);
    if (CarModelPrev->pointNum == 0) {
        CarModelPrev->pointNum = CarModel->pointNum;
        CarModelPrev->basePoint = CarModel->basePoint;
@@ -834,7 +864,9 @@
    bool ret = false;
    if (index < 0 || mapList == NULL || mapNum == 0) return true;

    if (mapList[index].type == MAP_TYPE_PARK_BUTTOM || mapList[index].type == MAP_TYPE_PART_EDGE) {
    if (mapList[index].type == MAP_TYPE_PARK_BUTTOM ||
        mapList[index].type == MAP_TYPE_PART_EDGE ||
        mapList[index].type == MAP_TYPE_TURN_90) {
        // 全车都需不在地图中
        Polygon carBody;

@@ -849,6 +881,45 @@
        }

        free(carBody.point);
    }
    if (mapList[index].type == MAP_TYPE_STOP_START) {
            // 构造虚拟的左上角点
            double x9, y9, xo, yo;

            bool enter = false;

            xo = (mapList[index].map.point[0].X + mapList[index].map.point[7].X) / 2;
            yo = (mapList[index].map.point[0].Y + mapList[index].map.point[7].Y) / 2;

            x9 = 2*xo - mapList[index].map.point[8].X;
            y9 = 2*yo - mapList[index].map.point[8].Y;

            Polygon map;

            map.num = 4;
            map.point = (PointF *) malloc(map.num * sizeof(PointF));

            map.point[0] = mapList[index].map.point[0];
            map.point[1] = mapList[index].map.point[8];
            map.point[2] = mapList[index].map.point[7];
            map.point[3].X = x9;
            map.point[3].Y = y9;

            // 全车都需不在地图中
            Polygon carBody;

            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, &map) == GM_None) {
                ret = true;
            }

            free(carBody.point);
            free(map.point);
    }

    return ret;
@@ -898,18 +969,18 @@
 * @param azimuth
 * @param coord
 */
static void UpdateCarBodyCoord(double azimuth, PointF main_ant, car_model *carModel)
static void UpdateCarBodyCoord(double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel)
{
    carModel->basePoint = main_ant;

    for (int i = 0; i < carModel->pointNum; ++i) {
        double tx = main_ant.X + carModel->carDesc[i].distance*sin(toRadians(azimuth));
        double ty = main_ant.Y + carModel->carDesc[i].distance*cos(toRadians(azimuth));
        double tx = carModel->carDesc[i].distance*sin(toRadians(azimuth));
        double ty = carModel->carDesc[i].distance*cos(toRadians(azimuth));

        carModel->carXY[i].X = (tx-main_ant.X)*cos(toRadians(carModel->carDesc[i].angle)) -
                                   (ty-main_ant.Y)*sin(toRadians(carModel->carDesc[i].angle)) + main_ant.X;
        carModel->carXY[i].Y = (tx-main_ant.X)*sin(toRadians(carModel->carDesc[i].angle)) +
                                   (ty-main_ant.Y)*cos(toRadians(carModel->carDesc[i].angle)) + main_ant.Y;
        carModel->carXY[i].X = (tx)*cos(toRadians(carModel->carDesc[i].angle)) -
                                   (ty)*sin(toRadians(carModel->carDesc[i].angle)) + main_ant.X;
        carModel->carXY[i].Y = (tx)*sin(toRadians(carModel->carDesc[i].angle)) +
                                   (ty)*cos(toRadians(carModel->carDesc[i].angle)) + main_ant.Y;
    }
}

@@ -921,4 +992,14 @@
    return false;
}

void SystemShutdown(int event, int timeout)
{
    // 关机
    if (event == 1) {
        ShutdownInd(timeout);
    }
    // 重启
    if (event == 0) {

    }
}