更新传感器

yy1717

2020-03-12 6c7f9e67fdcb882725d2790279fe9d99fb343c06

更新传感器

 lib/src/main/cpp/CMakeLists.txt

@@ -27,13 +27,14 @@
        Geometry.cpp
        driver_test.cpp
        mcu/mcu_if.cpp

        test_common/car_sensor.cpp
        test_items/error_list.cpp
        test_items/park_edge.cpp
        test_items/park_bottom.cpp
        test_items/stop_and_start.cpp
        test_items/driving_curve.cpp
        test_items/turn_a90.cpp
        test_items/comm_test.cpp

        test_items2/common_check.cpp
        test_items2/dummy_light.cpp

 lib/src/main/cpp/driver_test.cpp

@@ -26,8 +26,9 @@
#include "test_items/stop_and_start.h"
#include "master/comm_if.h"
#include "utils/xconvert.h"
#include "test_items/comm_test.h"
#include "test_common/car_sensor.h"
#include "mcu/mcu_if.h"
#include "test_common/car_sensor.h"

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

@@ -65,8 +66,9 @@
    TEST_TYPE_ROAD_TRUE_LIGHT
};

static bool TestStart = false;
static int TestType;
static bool ExamStart = false;
static int ExamType;
static bool reportSeatbeltEject;

static int CarInArea = 0;
int errs = 0;
@@ -91,17 +93,9 @@
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;

static int SensorValidLevel;

static struct dummy_light_exam *DummyLightContent;
static int DummyLightContentSize;
static bool engineRuning = false;

#define MOV_AVG_SIZE                1
#define RTK_BUFFER_SIZE            100
@@ -113,11 +107,13 @@
static rtk_info *RtkBuffer = NULL;
static int RtkBufferNum = 0, RtkBufferIn = 0;

static void DetectEnterOrExitMap(void);
static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime);
static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b);
static void ReadDriverExamPrimerTimeout(union sigval sig);
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);
static bool ExitMap(const car_model *car, int mapId, const struct map_list *mapList, int mapNum);
static int GetMapId(int index, const struct map_list *mapList, int mapNum);
@@ -128,17 +124,13 @@
    pthread_mutex_init(&rtk_clock_mutex, NULL);
    memset(&rtkClock, 0, sizeof(rtkClock));

    TestStart = false;
    ExamStart = false;
    memset(&MapList, 0, sizeof(MapList));
    MapNum = 0;
    CarModel = NULL;
    CarModelPrev = NULL;

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

    SensorValidLevel = 0;
    CarSensorInit();

    DummyLightContentSize = 0;
    DummyLightContent = NULL;
@@ -165,7 +157,7 @@

void ClearMap(void)
{
    if (TestStart) return;
    if (ExamStart) return;

    for (int i = 0; i < MapNum; ++i) {
        if (MapList[i].map.point != NULL)
@@ -183,7 +175,7 @@

    AppTimer_delete(ReadDriverExamPrimerTimeout);

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

    MapList[MapNum].id = id;
@@ -219,7 +211,7 @@
{
    DEBUG("加入车辆信息 pointNum %d", pointNum);

    if (point == NULL || pointNum == 0 || TestStart) return;
    if (point == NULL || pointNum == 0 || ExamStart) return;

    if (CarModel != NULL) {
        if (CarModel->body != NULL)
@@ -329,54 +321,11 @@
    DEBUG("SetCarMeasurePoint Calc Over");
}

void SetSensorCfg(int (*sensor)[3], int sensorNum)
{
    DEBUG("加入传感器配置 sensorNum %d", sensorNum);
    SensorValidLevel = 0;

    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];

        if (sensor[i][2] != 0) {
            SensorValidLevel |= BV(i);
        }
    }
}

int GetSensorValidLevel(void)
{
    return SensorValidLevel;
}

void GetSensorCfg(int gpio, int &func, bool &lvl)
{
    for (int i = 0; i < SensorNum; ++i) {
        if (SensorConfig[i].gpioId == gpio) {
            func = SensorConfig[i].funId;
            lvl = SensorConfig[i].validLvl == 0 ? false : true;
        }
    }
}

void FindSensorCfg(int func, int &gpio, bool &lvl)
{
    gpio = -1;
    for (int i = 0; i < SensorNum; ++i) {
        if (SensorConfig[i].funId == func) {
            gpio = SensorConfig[i].gpioId;
            lvl = SensorConfig[i].validLvl == 0 ? false : true;
        }
    }
}

void SetDummyLightExam(int n, struct dummy_light_exam *cfg)
{
    DEBUG("获取模拟路考灯光测试项目 N = %d %d", n, TestStart);
    DEBUG("获取模拟路考灯光测试项目 N = %d %d", n, ExamStart);

//    if (TestStart) return;
//    if (ExamStart) return;

    if (DummyLightContent != NULL) {
        free(DummyLightContent);
@@ -401,7 +350,7 @@
    if (start == 0) {
        DEBUG("结束考试");
//        CurrExamMapIndex = -1;
//        TestStart = false;
//        ExamStart = false;
//        CommTestStart(false);
//        MA_SendExamStatus(0, 0);
        return;
@@ -421,14 +370,14 @@
    }

    if (!err) {
        if (!TestStart) {
        if (!ExamStart) {
            DEBUG("启动考试");
            ExamFaultList.clear();
            examFaultIndex = 0;

            TestStart = true;
            TestType = type;
            CommTestStart(true);
            ExamStart = true;
            ExamType = type;
            reportSeatbeltEject = false;

            if (type == TEST_TYPE_ROAD_DUMMY_LIGHT) {
                StartDummyLightExam(DummyLightContent, DummyLightContentSize);
@@ -535,107 +484,179 @@
        rtkTime.ss = RtkBuffer[index].ss;
        rtkTime.mss = RtkBuffer[index].dss;

        if (TestStart) {
            if (CurrExamMapIndex < 0) {
                if (CurrEnterMapIndex < 0) {
                    CurrEnterMapIndex = EnterMap(CarModel, MapList, MapNum);
                    if (CurrEnterMapIndex >= 0) {
                        DEBUG("发送进入场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
                        MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 1);
        DetectEnterOrExitMap();

                        CurrExamMapIndex = CurrEnterMapIndex;
                        CurrExamStatus = EXAM_AREA_START;
                    }
                } else {
                    if (ExitMap(CarModel, CurrEnterMapIndex, MapList, MapNum)) {
                        DEBUG("发送离开场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
                        MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 0);
                        CurrEnterMapIndex = -1;
                    }
                }
            }
                //ExitMap(const car_model *car, int index, const struct map_list *mapList, int mapNum)
        if (ExamStart) {
            ExecuteExam(speed, move, azimuth, &rtkTime);
        }
    }
}

        if (CurrExamMapIndex >= 0) {
            int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);
static void DetectEnterOrExitMap(void)
{
    if (ExamType == TEST_TYPE_AREA) {

            if (CurrExamStatus == EXAM_AREA_START) {
                DEBUG("CurrExamMapIndex %d mtype %d", GetMapId(CurrExamMapIndex, MapList, MapNum), mtype);
    }
    if (CurrExamMapIndex < 0) {
        if (CurrEnterMapIndex < 0) {
            CurrEnterMapIndex = EnterMap(CarModel, MapList, MapNum);
            if (CurrEnterMapIndex >= 0) {
                DEBUG("发送进入场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
                MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 1);

                switch (mtype) {
                    case MAP_TYPE_PARK_BUTTOM:
                        DEBUG("进入倒车入库场地");
                        MA_SendDebugInfo("进入倒车入库场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                CurrExamMapIndex = CurrEnterMapIndex;
                CurrExamStatus = EXAM_AREA_START;
            }
        } else {
            if (ExitMap(CarModel, CurrEnterMapIndex, MapList, MapNum)) {
                DEBUG("发送离开场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
                MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 0);
                CurrEnterMapIndex = -1;
            }
        }
    }
}

                        StartParkBottom(move, &rtkTime);
                        CurrExamStatus = EXAM_AREA_RUN;
                        break;
                    case MAP_TYPE_STOP_START:
                        DEBUG("进入上坡起步场地");
                        MA_SendDebugInfo("进入上坡起步场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                        StartSAS(move, &rtkTime);
                        CurrExamStatus = EXAM_AREA_RUN;
                        break;
                    case MAP_TYPE_PART_EDGE:
                        DEBUG("进入侧方位停车场地");
                        MA_SendDebugInfo("进入侧方位停车场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                        StartParkEdge(move, &rtkTime);
static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime)
{
    if (move != 0) {
        if (ReadSensor(SENSOR_SEATBELT) == 0 && !reportSeatbeltEject) {
            DEBUG("不系安全带");
            reportSeatbeltEject = true;
            AddExamFault(1, rtkTime);
        }
    }

                        CurrExamStatus = EXAM_AREA_RUN;
                        break;
                    case MAP_TYPE_CURVE:
                        DEBUG("进入曲线行驶场地");
                        MA_SendDebugInfo("进入曲线行驶场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                        StartDrivingCurve(move, &rtkTime);
    if (CurrExamMapIndex >= 0) {
        int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);

                        CurrExamStatus = EXAM_AREA_RUN;
                        break;
                    case MAP_TYPE_TURN_90:
                        DEBUG("进入直角转弯场地");
                        MA_SendDebugInfo("进入直角转弯场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                        StartTurnA90(move, azimuth, &rtkTime);
                        CurrExamStatus = EXAM_AREA_RUN;
                        break;
                    default:break;
                }
            } else if (CurrExamStatus == EXAM_AREA_RUN) {
                int testing = 0;
                switch (mtype) {
                    case MAP_TYPE_PARK_BUTTOM:
                        testing = TestParkBottom(&MapList[CurrExamMapIndex].map,
                                                        CarModel, CarModelPrev, speed, move, &rtkTime);
                        break;
                    case MAP_TYPE_STOP_START:
                        testing = TestSAS(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, &rtkTime);
                        break;
                    case MAP_TYPE_PART_EDGE:
                        testing = TestParkEdge(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, &rtkTime);
                        break;
                    case MAP_TYPE_CURVE:
                        testing = TestDrivingCurve(&MapList[CurrExamMapIndex].map, &MapList[CurrExamMapIndex].map2, CarModel, CarModelPrev, speed, move, &rtkTime);
                        break;
                    case MAP_TYPE_TURN_90:
                        testing = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, azimuth, speed, move, &rtkTime);
                        break;
                    default:
                        break;
                }
        if (CurrExamStatus == EXAM_AREA_START) {
            DEBUG("CurrExamMapIndex %d mtype %d", GetMapId(CurrExamMapIndex, MapList, MapNum), mtype);

                if (testing > 0) {
            switch (mtype) {
                case MAP_TYPE_PARK_BUTTOM:
                    DEBUG("进入倒车入库场地");
                    MA_SendDebugInfo("进入倒车入库场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));

                    StartParkBottom(move, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                } else {
                    CurrExamStatus = EXAM_AREA_END;
                }
                    break;
                case MAP_TYPE_STOP_START:
                    DEBUG("进入上坡起步场地");
                    MA_SendDebugInfo("进入上坡起步场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    StartSAS(move, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_PART_EDGE:
                    DEBUG("进入侧方位停车场地");
                    MA_SendDebugInfo("进入侧方位停车场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    StartParkEdge(move, rtkTime);

                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_CURVE:
                    DEBUG("进入曲线行驶场地");
                    MA_SendDebugInfo("进入曲线行驶场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    StartDrivingCurve(move, rtkTime);

                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_TURN_90:
                    DEBUG("进入直角转弯场地");
                    MA_SendDebugInfo("进入直角转弯场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    StartTurnA90(move, azimuth, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                default:break;
            }
        } else if (CurrExamStatus == EXAM_AREA_RUN) {
            int testing = 0;
            switch (mtype) {
                case MAP_TYPE_PARK_BUTTOM:
                    testing = TestParkBottom(&MapList[CurrExamMapIndex].map,
                                             CarModel, CarModelPrev, speed, move, rtkTime);
                    break;
                case MAP_TYPE_STOP_START:
                    testing = TestSAS(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, rtkTime);
                    break;
                case MAP_TYPE_PART_EDGE:
                    testing = TestParkEdge(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, rtkTime);
                    break;
                case MAP_TYPE_CURVE:
                    testing = TestDrivingCurve(&MapList[CurrExamMapIndex].map, &MapList[CurrExamMapIndex].map2, CarModel, CarModelPrev, speed, move, rtkTime);
                    break;
                case MAP_TYPE_TURN_90:
                    testing = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, azimuth, speed, move, rtkTime);
                    break;
                default:
                    break;
            }

            if (CurrExamStatus != EXAM_AREA_RUN) {
                // 某项结束
                //DEBUG("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                //MA_SendDebugInfo("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                CurrExamStatus = EXAM_AREA_NONE;
                CurrExamMapIndex = -1;
            if (testing > 0) {
                CurrExamStatus = EXAM_AREA_RUN;
            } else {
                CurrExamStatus = EXAM_AREA_END;
            }
        }
        if (CurrExamStatus != EXAM_AREA_RUN) {
            // 某项结束
            //DEBUG("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
            //MA_SendDebugInfo("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
            CurrExamStatus = EXAM_AREA_NONE;
            CurrExamMapIndex = -1;
        }
    }
}

static void EngineStartHold(union sigval sig) {
    AppTimer_delete(EngineStartHold);

    // 不及时松开启动开关，扣10分
    if (ReadSensor(SENSOR_ENGINE_START) == 1 && ExamStart) {
        if (ExamType == TEST_TYPE_AREA)
            AddExamFault(4, &rtkClock);
    }
}

void SensorChanged(int *sensorId, int *sensorValue, int num)
{
    const int ENGINE_MIN_ROTATE = 200;

    for (int i = 0; i < num; ++i) {
        switch (sensorId[i]) {
            case SENSOR_SEATBELT:
                break;
            case SENSOR_ENGINE_START:
                AppTimer_delete(EngineStartHold);

                if (sensorValue[i] == 1) {
                    if (ReadSensor(SENSOR_SHIFT_N) == 0) {
                        // 不是空挡点火，不合格
                        if (ExamType == TEST_TYPE_AREA)
                            AddExamFault(3, &rtkClock);
                        else
                            AddExamFault(4, &rtkClock);
                    }
                    AppTimer_add(EngineStartHold, D_SEC(2));
                }
                break;
            case SENSOR_ENGINE_RPM:
                if (sensorValue[i] < ENGINE_MIN_ROTATE) {
                    if (engineRuning) {
                        engineRuning = false;
                        if (ExamType == TEST_TYPE_AREA) {
                            // 熄火1次，扣10分
                            AddExamFault(5, &rtkClock);
                        } else {
                            AddExamFault(20, &rtkClock);
                        }
                    }
                } else {
                    engineRuning = true;
                }
                break;
            default:
                break;
        }
    }
}
@@ -770,87 +791,6 @@
//    DEBUG("tmDiff = %ld speed = %f m/Sec move = %d", tmDiff, speed, move);

    return true;
/*        if (!TestStart) return;

        if (CarInArea == 0) {
//            if (FrontTireEnterArea(&py, &theParkEdgeMap)) {
//                CarInArea = TEST_PARK_BOTTOM;
//                TestItem = TEST_PARK_BOTTOM;
//                StartParkBottom();
//            }

            CarInArea = TEST_SLOPE;
            TestItem = TEST_SLOPE;
            StartSAS();
        }

        switch (TestItem) {
            case TEST_NONE: {
                break;
            }
            case TEST_PARK_BOTTOM: {
                errs = TestParkBottom(ErrorList, &theParkEdgeMap, GetCarModelCache(0), currSpeed, currDirect);

                if (errs != 0) {
                    StopParkBottom();
                    TestItem = TEST_NONE;
                }
                break;
            }
            case TEST_PARK_EDGE: {
                errs = TestParkEdge(ErrorList, &theParkEdgeMap, GetCarModelCache(0), currSpeed, currDirect);

                if (errs != 0) {
                    StopParkEdge();
                    TestItem = TEST_NONE;
                }
                break;
            }

            case TEST_TUNE_90: {
                errs = TestTurnA90(ErrorList, &theTurn90Map, GetCarModelCache(0), currSpeed, currDirect, currAzimuth);

                if (errs != 0) {
                    StopTurnA90();
                    TestItem = TEST_NONE;
                }

                break;
            }

            case TEST_SLOPE: {
                errs = TestSAS(ErrorList, &theSSMap, GetCarModelCache(0), currSpeed, currDirect);

                if (errs != 0) {
                    StopSAS();
                    TestItem = TEST_NONE;
                }

                break;
            }
            default:
                break;
        }

        if (ErrorList.size() > 0) {
            vector<int>::iterator it = ErrorList.end();
            it--;
            error_list_t list = GetErrorList(*it);

            int scr = 0;

            for (vector<int>::iterator it1 = ErrorList.begin(); it1 != ErrorList.end(); ++it1) {
                error_list_t list = GetErrorList(*it1);

                scr += list.dec_score;
            }

            char buff[256];

            sprintf(buff, "%s, 总计扣分 %d", list.text_desc, scr);

            TextOsd(1, buff);
        }*/
}

static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum)
@@ -1033,6 +973,9 @@
{
    struct ExamFault fault;

    if (!ExamStart)
        return;

    fault.sn = examFaultIndex++;
    sprintf(fault.utc, "%04d%02d%02d%02d%02d%02d.%02d", 2000 + rtkTime->YY,
            rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
@@ -1088,14 +1031,6 @@
                projectDistance * cos(toRadians(azimuth)) * cos(toRadians(projectAngle)) +
                main_ant.Y;
    }
}

static bool FrontTireEnterArea(const Polygon *car, const Polygon *map)
{
    if (IntersectionOf(car->point[0], map) == GM_Containment) {
        return true;
    }
    return false;
}

void SystemShutdown(int event, int timeout)

 lib/src/main/cpp/driver_test.h

@@ -88,11 +88,7 @@
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
                        int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
                        int *body, int bodyNum, double (*point)[2], int pointNum, double antPitch);
void SetSensorCfg(int (*sensor)[3], int sensorNum);
int GetSensorValidLevel(void);
void GetSensorCfg(int gpio, int &func, bool &lvl);
void FindSensorCfg(int func, int &gpio, bool &lvl);

void SensorChanged(int *sensorId, int *sensorValue, int num);
void StartDriverExam(int start, int type);
void StartMapExam(int map_id, int exam);
void GetRtkClock(struct RtkTime *s);

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

@@ -15,6 +15,7 @@
#include "../common/apptimer.h"
#include "../jni_log.h"
#include "../mcu/mcu_if.h"
#include "../test_common/car_sensor.h"

#include <string>
#include <iostream>

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

@@ -21,7 +21,7 @@
#include "../utils/xconvert.h"
#include "../utils/num.h"
#include "../driver_test.h"
#include "../test_items/comm_test.h"
#include "../test_common/car_sensor.h"

#define PARSE_BUFF_SIZE         4096


 lib/src/main/cpp/test_items/comm_test.cpp

File was deleted

 lib/src/main/cpp/test_items/comm_test.h

File was deleted

 lib/src/main/cpp/test_items2/common_check.cpp

@@ -3,7 +3,7 @@
//

#include "common_check.h"
#include "../test_items/comm_test.h"
#include "../test_common/car_sensor.h"
#include "../driver_test.h"

// 由人工确认
@@ -20,12 +20,12 @@

        if (prevMoveDirect == 0) {
            // 起步挂错挡，不合格
            if (CheckSensorX(SHIFT) != '1') {
            if (ReadSensor(SENSOR_SHIFT_1) != 1) {
                AddExamFault(19, rtkTime);
            }

            // 起步没有关闭车门，不合格
            if (CheckSensorX(DOOR) != 1) {
            if (ReadSensor(SENSOR_DOOR) != 1) {
                AddExamFault(23, rtkTime);
            }