增加单片机信息传递。

fctom1215

2020-02-14 7c5b25c8617807635e9f272ebe9e98304d07cf2f

 lib/src/main/cpp/driver_test.cpp

@@ -80,6 +80,13 @@
static car_model *CarModel = NULL;
static car_model *CarModelPrev = NULL;

static struct sensor_cfg {
    int gpioId;
    int funId;
    int validLvl;
} SensorConfig[32];
static int SensorNum = 0;

#define MOV_AVG_SIZE                1
#define RTK_BUFFER_SIZE            100
#define CAR_MODEL_CACHE_SIZE      10
@@ -104,6 +111,9 @@
    MapNum = 0;
    CarModel = NULL;
    CarModelPrev = NULL;

    SensorNum = 0;
    memset(SensorConfig, 0, sizeof(SensorConfig));

    RtkBuffer = (rtk_info *) malloc(RTK_BUFFER_SIZE * sizeof(rtk_info));
    RtkBufferNum = RtkBufferIn = 0;
@@ -287,9 +297,26 @@
    DEBUG("SetCarMeasurePoint Calc Over");
}

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

    SensorNum = sensorNum;
    for (int i = 0; i < sensorNum; ++i) {
        SensorConfig[i].gpioId = sensor[i][0];
        SensorConfig[i].funId = sensor[i][1];
        SensorConfig[i].validLvl = sensor[i][2];
    }
}

void GetFuncGpio(int func, int &gpio, int &lvl)
{
    for (int i = 0; i < SensorNum; ++i) {
        if (SensorConfig[i].funId == func) {
            gpio = SensorConfig[i].gpioId;
            lvl = SensorConfig[i].validLvl;
        }
    }
}

void StartDriverExam(int start)
@@ -345,7 +372,7 @@
                RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);

        brief.qf = RtkBuffer[index].qf;
        brief.map_id = 865;//GetMapId(CurrExamMapIndex, MapList, MapNum);
        brief.map_id = 866;//GetMapId(CurrExamMapIndex, MapList, MapNum);
        brief.move = move;
        brief.speed = speed * 3.6;
        brief.heading = RtkBuffer[index].heading;
@@ -469,15 +496,14 @@
}

/*************************************************
 * 2次采样相差的时间
 * 2次采样相差的时间, a 最近的，b 先前的
 * @param a
 * @param b
 * @return ms
 */
static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b)
{
    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));
    return TimeGetDiff(a->hh, a->mm, a->ss, a->dss*10, b->hh, b->mm, b->ss, b->dss*10);
}

static bool UpdateCarCoord(double &spd, int &mov, int &idx)
@@ -686,7 +712,7 @@
{
    // 车的最前点是否进入地图
    for (int i = 0; i < mapNum && car != NULL; ++i) {
        if (mapList[i].type == MAP_TYPE_STOP_START) {
        /*if (mapList[i].type == MAP_TYPE_STOP_START) {
            // 构造虚拟的左上角点
            double x9, y9, xo, yo;

@@ -721,7 +747,7 @@
            free(map.point);

            if (enter) return i;
        } /*else if (mapList[i].type == MAP_TYPE_CURVE) {
        } else if (mapList[i].type == MAP_TYPE_CURVE) {

        } else if (mapList[i].type == MAP_TYPE_PARK_BUTTOM || mapList[i].type == MAP_TYPE_PART_EDGE) {
            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
@@ -734,7 +760,7 @@
                    DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
                    return i;
            }
        } else if (mapList[i].type == MAP_TYPE_TURN_90) {
        } else */if (mapList[i].type == MAP_TYPE_TURN_90) {
            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
                Line enterLine1;

@@ -743,7 +769,7 @@
                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
                    return i;
            }
        }*/
        }
    }
    return -1;
}