坐标

yy1717

2020-05-25 a3c194e003b0cfb272e153ff11c510e02c5f72cd

坐标

 lib/src/main/cpp/driver_test.h

@@ -115,12 +115,11 @@
#define ROAD_ITEM_OPERATE_GEAR      4
#define ROAD_ITEM_START_CAR         5

enum {
    ROAD_ACTIVE_FORWARD,
    ROAD_ACTIVE_TURN_LEFT,
    ROAD_ACTIVE_TURN_RIGHT,
    ROAD_ACTIVE_TURN_BACKWARD
};

#define ROAD_ACTIVE_FORWARD     LANE_FORWARD
#define ROAD_ACTIVE_TURN_LEFT   LANE_LEFT
#define ROAD_ACTIVE_TURN_RIGHT  LANE_RIGHT
#define ROAD_ACTIVE_TURN_BACKWARD   LANE_BACKWARD

typedef struct {
    int character;                  // 属性（实线、虚线，有些可以掉头的路段）
@@ -174,6 +173,7 @@
    int time;                   // 项目最大完成时间
    int distance;               // 项目最大完成距离
    string tts;
    bool cmp;
    std::vector<PointF> points;
    std::vector<PointF> leftPoints; // 对应到道路左侧的点
} trigger_line_t;

 lib/src/main/cpp/native-lib.cpp

@@ -264,7 +264,7 @@
    ConfigRTKModule();
    MA_Init();
    InitPlatform(phone, RTK_PLATFORM_IP, RTK_PLATFORM_PORT);
    AppTimer_add(SendBootIndicate, 500);
    AppTimer_add(SendBootIndicate, D_SEC(1));

    InitVirtualDevice(VIRTUAL_RTK_IP, VIRTUAL_RTK_PORT);


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

@@ -24,6 +24,7 @@
#include "../test_common/car_sensor.h"
#include "../test_items2/stop_car.h"
#include "../test_items2/operate_gear.h"
#include "../test_items2/drive_straight.h"

#define PARSE_BUFF_SIZE         4096

@@ -440,6 +441,7 @@
        DummyLightTTSDone(*((int *)data));
        StopCarTTSDone(*((int *)data));
        OperateGearTTSDone(*((int *)data));
        DriveStraightTTSDone(*((int *)data));
    }
}


 lib/src/main/cpp/test_items2/drive_straight.cpp

@@ -8,6 +8,7 @@
#include "../jni_log.h"
#include "road_exam.h"
#include "../master/comm_if.h"
#include "../common/apptimer.h"
#include <cmath>

#define DEBUG(fmt, args...)     LOGD("<drive_straight> <%s>: " fmt, __func__, ##args)
@@ -20,24 +21,30 @@
static bool reportOffsetOver;
static double edgeDistance;
static double distanceToStartSum;
static int examTtsSeq;

static double CalcDistance2Edge(road_t &road,  const car_model *car);
static void PlayTTSTimeout(union sigval sig);

void StartDriveStraightExam(std::string tts) {
    DEBUG("开始直线行驶");

    ttsPlayEnd = 0;
    if (!tts.empty()) {
        PlayTTS(tts.c_str());
        examTtsSeq = PlayTTS(tts.c_str());
    } else {
        DEBUG("没有TTS");
        examTtsSeq = PlayTTS("请保持直线行驶");
    }

    distanceToStartSum = 0;
    reportOffsetOver = false;

    AppTimer_delete(PlayTTSTimeout);
    AppTimer_add(PlayTTSTimeout, D_SEC(5));
}

int ExecuteDriveStraightExam(road_t &road,  const car_model *car,
                             LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) {
                             const trigger_line_t *item, const struct RtkTime *rtkTime) {
    static PointF startPoint;

    double dis2roadEdge = 0;
@@ -86,6 +93,27 @@
    return 1;
}

void DriveStraightTTSDone(int id)
{
    // 等语音播报完毕后计时
    if (id == examTtsSeq) {
        DEBUG("StopCarTTSDone %d", id);
        ttsPlayEnd = 1;
    }
}

void TerminateDriveStraightExam(void)
{
    AppTimer_delete(PlayTTSTimeout);
}

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

    ttsPlayEnd = 1;
}

static double CalcDistance2Edge(road_t &road,  const car_model *car)
{
    PointF vp;

 lib/src/main/cpp/test_items2/drive_straight.h

@@ -9,6 +9,8 @@

void StartDriveStraightExam(std::string tts);
int ExecuteDriveStraightExam(road_t &road,  const car_model *car,
                             LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);
                             const trigger_line_t *item, const struct RtkTime *rtkTime);
void DriveStraightTTSDone(int id);
void TerminateDriveStraightExam(void);

#endif //MYAPPLICATION2_DRIVE_STRAIGHT_H

 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,

 lib/src/main/cpp/test_items2/stop_car.cpp

@@ -69,7 +69,7 @@
    AppTimer_delete(PlayTTSTimeout);
}

int ExecuteStopCarExam(int index, road_t &road, const car_model *car,
int ExecuteStopCarExam(road_t &road, const car_model *car,
                             LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) {
    if (ttsPlayEnd == 1) {
        ttsPlayEnd = 2;
@@ -78,7 +78,7 @@
    }

    if (ttsPlayEnd != 2)
        return index;
        return 1;

    uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
                                rtkTime->mss * 10,
@@ -220,7 +220,7 @@
        }
    }

    return index;
    return 1;
}

static void PlayTTSTimeout(union sigval sig)

 lib/src/main/cpp/test_items2/stop_car.h

@@ -8,7 +8,7 @@
#include "../driver_test.h"

void StartStopCarExam(std::string tts);
int ExecuteStopCarExam(int index, road_t &road, const car_model *car,
int ExecuteStopCarExam(road_t &road, const car_model *car,
                       LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);
void TerminateStopCarExam(void);
void StopCarTTSDone(int id);