角度修正。

fctom1215

2020-02-27 ce568d65d1738e0dfe64e0f903c9aa8d05cd365b

 lib/src/main/cpp/driver_test.cpp

@@ -79,7 +79,7 @@
static int CurrExamMapIndex = -1;
static int CurrEnterMapIndex = -1;

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

static car_model *CarModel = NULL;
static car_model *CarModelPrev = NULL;
@@ -132,6 +132,8 @@

    RtkBuffer = (rtk_info *) malloc(RTK_BUFFER_SIZE * sizeof(rtk_info));
    RtkBufferNum = RtkBufferIn = 0;

    CurrExamStatus = EXAM_AREA_NONE;
}

static void ReadDriverExamPrimerTimeout(union sigval sig)
@@ -397,7 +399,7 @@

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

@@ -497,7 +499,7 @@
                        MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), 1);

                        CurrExamMapIndex = CurrEnterMapIndex;
                        CurrExamStatus = 2;
                        CurrExamStatus = EXAM_AREA_START;
                    }
                } else {
                    if (ExitMap(CarModel, CurrEnterMapIndex, MapList, MapNum)) {
@@ -513,7 +515,7 @@
        if (CurrExamMapIndex >= 0) {
            int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);

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

                switch (mtype) {
@@ -522,20 +524,20 @@
                        MA_SendDebugInfo("进入倒车入库场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));

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

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

                if (testing > 0) {
                    CurrExamStatus = EXAM_AREA_RUN;
                } else {
                    CurrExamStatus = EXAM_AREA_END;
                }
            }

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

                CurrExamStatus = EXAM_AREA_NONE;
                CurrExamMapIndex = -1;
            }
        }