坐标

yy1717

2020-05-25 a3c194e003b0cfb272e153ff11c510e02c5f72cd

 lib/src/main/cpp/test_items2/road_exam.cpp

@@ -34,6 +34,12 @@
    START_CAR_DONE
};

enum {
    STOP_CAR_NOT_DO,
    STOP_CAR_DOING,
    STOP_CAR_DONE
};

static const int INVALID_ROAD = -1;

static const int TURN_THRESHOLD = 1;
@@ -43,6 +49,7 @@
const double CHANGE_LANE_RANGE = 100.0;
const double OVERTAKE_RANGE = 150.0;
const double OVERTAKE_HOLD_RANGE = 30.0;                // 在超车道行驶的一段距离
const double EXAM_RANGE = 3000.0;                       // 至少驾驶距离

static const double LASTEST_BREAK_POINT = 30.0;
static const double NEXT_ROAD_TIP = 100.0;                  // 到达路口前提示下一个怎么走
@@ -81,7 +88,7 @@
static int gearErrorTime;
static int gearNSlideTime;

static int startCar;
static int startCar, stopCar;
static int currExamMapIndex;
static trigger_line_t *currRoadItem;
static int nextRoadId;
@@ -104,6 +111,10 @@
    int type;           // 实线，虚线
} CurrentLane;
static bool laneChanging;
static double odoGraph;
static struct drive_timer odoTimer;
static double odoPrevSpeed;
static int odoCnt;

static const int MAX_ENGINE_RPM = 2500;
static const double START_CAR_MOVE_DISTANCE = 10.0;
@@ -140,6 +151,7 @@
static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList);
static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
static void ClearAll(road_exam_map &map);
static bool AllCmp(road_exam_map &map);
static void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList,
                             double speed, int moveDirect, const struct RtkTime *rtkTime);

@@ -172,6 +184,8 @@
    currExamMapIndex = FIND_POSITION;

    startCar = START_CAR_NOT_DO;
    stopCar = STOP_CAR_NOT_DO;

    currRoadItem = NULL;

    checkDoor = false;
@@ -187,6 +201,11 @@

    nextRoadId = -1;
    ClearAll(RoadMap);

    odoGraph = 0.0;
    odoCnt = 0;

    // 初始化考项
}

void TerminateRoadExam(void)
@@ -194,6 +213,7 @@
    TerminateDummyLightExam();
    TerminateStopCarExam();
    TerminateOperateGearExam();
    TerminateDriveStraightExam();

    AppTimer_delete(TurnSignalError13ColdTimer);
    AppTimer_delete(TurnSignalError14ColdTimer);
@@ -306,6 +326,25 @@

void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    // 行驶距离，不包含倒车
    if (odoCnt == 0 && moveDirect == 1) {
        odoPrevSpeed = speed;
        odoCnt = 1;
        Rtk2DriveTimer(odoTimer, rtkTime);
    } else if (odoCnt == 1) {
        uint32_t tm = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                                  odoTimer.hour, odoTimer.min, odoTimer.sec, odoTimer.msec*10);
        if (tm >= D_SEC(1)) {
            odoGraph += ((double)tm)*(odoPrevSpeed + speed)/2.0/1000.0;
            if (moveDirect == 1) {
                Rtk2DriveTimer(odoTimer, rtkTime);
                odoPrevSpeed = speed;
            } else {
                odoCnt = 0;
            }
        }
    }

    // 超速检测
    if (moveDirect != 0 && speed > MAX_SPEED) {
        if (!occurOverSpeed) {
@@ -636,7 +675,9 @@
            crashRedLineNow = true;
        }

        // 检查当前车道
        struct car_on_lane lane;

        if (UpdateLane(lane, RoadMap.roads[currExamMapIndex], car)) {

            if (lane.road == CurrentLane.road && lane.separate == CurrentLane.separate &&
@@ -737,7 +778,7 @@
                }
            }

            if (lane.road != CurrentLane.road || lane.separate != CurrentLane.separate) {
            if (lane.road != CurrentLane.road || lane.separate != CurrentLane.separate || lane.direct != CurrentLane.direct) {
                // 路或段变更，撤销变道跟踪
                DEBUG("============== 路或段变更 CURR %d, %d dir %d NEW %d, %d, dir %d", CurrentLane.road,
                      CurrentLane.separate, CurrentLane.direct,
@@ -818,7 +859,17 @@
    if (startCar != START_CAR_DONE)
        return;

    // 检测由触发线控制的项目
    if (odoGraph > EXAM_RANGE && currRoadItem == NULL && AllCmp(RoadMap) && stopCar == STOP_CAR_NOT_DO) {
        // 在合适条件下停车结束考试
        StartStopCarExam("");
        stopCar = STOP_CAR_DOING;
    } else if (stopCar == STOP_CAR_DOING) {
        if (ExecuteStopCarExam(RoadMap.roads[currExamMapIndex], car, CarModelList, speed, moveDirect, rtkTime) < 0)
            stopCar = STOP_CAR_DONE;
    }


    // 执行某个项目
    if (currRoadItem != NULL) {
        if (currRoadItem->active == ROAD_ITEM_CHANGE_LANE) {
            if (DistanceOf(car->basePoint, roadItemStartPoint) > CHANGE_LANE_RANGE) {
@@ -837,11 +888,20 @@
                currRoadItem = NULL;
            }
        } else if (currRoadItem->active == ROAD_ITEM_OPERATE_GEAR) {
            ExecuteOperateGearExam(rtkTime);
            if (ExecuteOperateGearExam(rtkTime) < 0) {
                currRoadItem = NULL;
            }
        } else if (currRoadItem->active == ROAD_ITEM_STRAIGHT) {
            if (ExecuteDriveStraightExam(RoadMap.roads[currExamMapIndex], car, currRoadItem, rtkTime) < 0) {
                currRoadItem = NULL;
            }
        }
    } else if (currExamMapIndex >= 0) {
        currRoadItem = EntryItem(currExamMapIndex, RoadMap, car, CarModelList);
        if (currRoadItem != NULL) {
    } // 检测由触发线控制的项目
    else if (currExamMapIndex >= 0) {
        trigger_line_t *new_item = EntryItem(currExamMapIndex, RoadMap, car, CarModelList);

        if (new_item != NULL && !new_item->cmp) {
            currRoadItem = new_item;
            if (!currRoadItem->tts.empty())
                PlayTTS(currRoadItem->tts.c_str());

@@ -853,6 +913,8 @@
                overtake = false;
            } else if (currRoadItem->active == ROAD_ITEM_OPERATE_GEAR) {
                StartOperateGearExam(rtkTime);
            } else if (currRoadItem->active == ROAD_ITEM_STRAIGHT) {
                StartDriveStraightExam(currRoadItem->tts);
            }
        }
    }
@@ -903,7 +965,7 @@
                MakeLine(&rightExtLine, &car->carXY[car->axial[AXIAL_FRONT]], &vp);
                goto RIGHT_EXT_CMP;
            } else {
                DEBUG("右侧不垂点 %d p1(%f,%f) p2(%f,%f) (%f,%f)", j, p1.X, p1.Y, p2.X, p2.Y, vp.X, vp.Y);
//                DEBUG("右侧不垂点 %d p1(%f,%f) p2(%f,%f) (%f,%f)", j, p1.X, p1.Y, p2.X, p2.Y, vp.X, vp.Y);
            }
            p1 = p2;
        }
@@ -946,7 +1008,6 @@
                        orthogonalInSegment = true;
                        intersection = true;

//                        DEBUG("分道线 %d 左正交", j);
                        break;
                    } else if (IntersectionOf(rightExtLine, sep) == GM_Intersection) {
                        vector<int> stor(4);
@@ -960,7 +1021,6 @@
                        orthogonalInSegment = true;
                        intersection = true;

//                        DEBUG("分道线 %d 右正交", j);
                        break;
                    }
                    p1 = p2;
@@ -983,12 +1043,15 @@
                            lane.road = road.id;
                            lane.separate = i;
                            lane.lane = itx->first;

//                            DEBUG("left_char %d second %d", left_char, itx->second[1]);

                            if ((left_char == LINE_SOLID || left_char == LINE_HALF_SOLID_RIGHT) &&
                                    (itx->second[1] == LINE_SOLID || itx->second[1] == LINE_HALF_SOLID_LEFT))    // 车道左右均是实线
                                lane.direct = itx->second[2];
                            else
                                lane.direct = 0;
//                            DEBUG("路 %d 段 %d 车道 %d", lane.road, lane.separate, lane.lane);
//                            DEBUG("路 %d 段 %d 车道 %d 限定 %d", lane.road, lane.separate, lane.lane, lane.direct);
                            break;
                        } else {
                            right_direct = itx->second[3];
@@ -1000,12 +1063,15 @@
                    lane.road = road.id;
                    lane.separate = i;
                    lane.lane = orthogonal.size();

//                    DEBUG("left_char %d right_direct %d", left_char, right_direct);

                    // 最后车道的右侧限定
                    if (left_char == LINE_SOLID || left_char == LINE_HALF_SOLID_RIGHT)
                        lane.direct = right_direct;
                    else
                        lane.direct = 0;
//                    DEBUG("路 %d 段 %d 车道 %d", lane.road, lane.separate, lane.lane);
//                    DEBUG("路 %d 段 %d 车道 %d 限定 %d", lane.road, lane.separate, lane.lane, lane.direct);
                }
                out = lane;
                return true;
@@ -1835,6 +1901,15 @@
    return NULL;
}

static bool AllCmp(road_exam_map &map)
{
    for (int i = 0; i < map.triggerLines.size(); ++i) {
        if (!map.triggerLines[i].cmp)
            return false;
    }
    return true;
}

/************************************************************************
 * 开始新的考试后，清除地图所有的刹车、停车记录
 * @param map
@@ -1849,6 +1924,9 @@
    for (int i = 0; i < map.specialAreas.size(); i++) {
        map.specialAreas[i].overSpeed = map.specialAreas[i].activeBreak = false;
    }
    for (int i = 0; i < map.triggerLines.size(); ++i) {
        map.triggerLines[i].cmp = false;
    }
}

static void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList,