坐标

yy1717

2020-08-20 2d6a9d02c77d7e08d4f18ee87d6e3d337b949f47

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

@@ -16,6 +16,7 @@
#include "stop_car.h"
#include "operate_gear.h"
#include "../test_common/odo_graph.h"
#include "car_start.h"

#include <cmath>
#include <vector>
@@ -62,13 +63,6 @@
    PointF point;
} projection_t;

typedef struct
{
    int name;
    int value;
    struct RtkTime time;
} car_sensor_value_t;

typedef struct {
    int gain;
    struct RtkTime time;
@@ -83,6 +77,7 @@

static const int INVALID_ROAD = -1;

static const int CROSSING_TURN_THRESHOLD = 45;
static const int TURN_THRESHOLD = 3;
static const int TURN_CHECK_INTERVAL = 500;
const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
@@ -107,14 +102,13 @@
static int prevMoveDirect;
static uint32_t stopTimepoint = 0;
static bool reportStopCarOnRedArea;
static PointF stopPoint, startPoint;
static PointF stopPoint;
static bool prevGearError = false;
static bool prevGearNSlide = false;

static bool slideLongDistance;
static bool slideNormalDistance;
static bool occurSlide;
static bool startCarLeftTurnSignal, checkStartCarSignal;

static struct RtkTime crashGreenRunTime, crashGreenStartTime;

@@ -126,16 +120,13 @@
static int gearErrorTime;
static int gearNSlideTime;

static int startCar, stopCar;
static int stopCar;
static int currExamMapIndex;
static trigger_line_t *currRoadItem;
static PointF roadItemStartPoint;
static struct drive_timer roadItemStartTime;
static bool overtake = false;
static bool checkTurn = false;
static bool checkDoor = false;
static bool handBreakActive = false;
static bool reportRPMOver = false;

static lane_t Lane;
static change_lane_t ChangeLane;
@@ -146,14 +137,32 @@
static map<int, int> CrossingHint;
static map<int, bool> ErrorLaneReport;

#define ROAD_EXAM_READY_NEXT            0
#define ROAD_EXAM_FREE_RUN              1
#define ROAD_EXAM_ITEM_CAR_START            2
#define ROAD_EXAM_ITEM_STRAIGHT            3
#define ROAD_EXAM_ITEM_OP_GEAR            4
#define ROAD_EXAM_ITEM_CHANGE_LANE           5
#define ROAD_EXAM_ITEM_OVER_TAKE            6
#define ROAD_EXAM_ITEM_CAR_STOP            7

#define ROAD_EXAM_ITEM_NOT_EXEC             0
#define ROAD_EXAM_ITEM_EXECING             1
#define ROAD_EXAM_ITEM_EXECED             2


typedef struct {
    int name;
    int status;
} RoadExamItem_t;
static map<int, int> RoadExamItem;
static int RoadExamStatus;

static struct RtkTime beginTurnTime, prevDetectTurnTime;
static int startTurnYaw, prevYaw;
static int turnCnt, turnTimeCnt;
static int prevTurnWise;

static const int MAX_ENGINE_RPM = 2500;
static const double START_CAR_MOVE_DISTANCE = 10.0;
static const double START_CAR_CHECK_DOOR_DISTANCE = 1.0;
static const uint32_t GEAR_N_SLIDE_TIMEOUT = D_SEC(5);
static const uint32_t GEAR_ERROR_TIMEOUT = D_SEC(15);
static const uint32_t STOP_CAR_TIME = D_SEC(2);
@@ -170,8 +179,7 @@
static const double MAX_SPEED = 60.0 * 1000.0 / 3600.0;
static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};

static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime);

static void ItemExam(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end);
static int isTurn(int currYaw, int prevYaw, int thres);
static void ResetTurnDetect(const car_model *car);
static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime);
@@ -212,16 +220,9 @@

    currExamMapIndex = -1;

    startCar = START_CAR_NOT_DO;
    stopCar = STOP_CAR_NOT_DO;

    currRoadItem = NULL;

    checkDoor = false;
    handBreakActive = false;
    reportRPMOver = false;

    checkStartCarSignal = startCarLeftTurnSignal = false;

    Lane.road = Lane.sep = Lane.no = -1;
    Lane.guide = 0;
@@ -234,14 +235,23 @@
    ResetOdo();
    ResetTarget(RoadMap);
    // 初始化考项
    RoadExamItem.clear();
    RoadExamItem[ROAD_EXAM_ITEM_CAR_START] = ROAD_EXAM_ITEM_NOT_EXEC;
    RoadExamItem[ROAD_EXAM_ITEM_STRAIGHT] = ROAD_EXAM_ITEM_NOT_EXEC;
    RoadExamItem[ROAD_EXAM_ITEM_OP_GEAR] = ROAD_EXAM_ITEM_NOT_EXEC;
    RoadExamItem[ROAD_EXAM_ITEM_CHANGE_LANE] = ROAD_EXAM_ITEM_NOT_EXEC;
    RoadExamItem[ROAD_EXAM_ITEM_OVER_TAKE] = ROAD_EXAM_ITEM_NOT_EXEC;
    RoadExamItem[ROAD_EXAM_ITEM_CAR_STOP] = ROAD_EXAM_ITEM_NOT_EXEC;

    RoadExamStatus = ROAD_EXAM_READY_NEXT;
}

void TerminateRoadExam(void)
{
    TerminateDummyLightExam();
    TerminateStopCarExam();
    TerminateOperateGearExam();
    TerminateDriveStraightExam();
//    TerminateDummyLightExam();
//    TerminateStopCarExam();
//    TerminateOperateGearExam();
//    TerminateDriveStraightExam();
}

/*********************************************************************
@@ -821,7 +831,7 @@
    }
}

static car_sensor_value_t ReadCarSensorValue(int name)
car_sensor_value_t ReadCarSensorValue(int name)
{
    car_sensor_value_t nv = {.name = -1};

@@ -843,78 +853,6 @@
    WriteCarSensorValue(SECOND_BREAK, ReadCarStatus(SECOND_BREAK), rtkTime);
    WriteCarSensorValue(GEAR, ReadCarStatus(GEAR), rtkTime);
    WriteCarSensorValue(BREAK, ReadCarStatus(BREAK), rtkTime);
}

static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    double moveDistance;

    if (startCar == START_CAR_NOT_DO) {
        startPoint = car->basePoint;
        reportRPMOver = false;
        startCar = START_CAR_DOING;

        PlayTTS("请起步");
    } else if (startCar == START_CAR_DOING) {
        moveDistance = DistanceOf(startPoint, car->basePoint);
        DEBUG("起步行驶距离 %f", moveDistance);

        if (!startCarLeftTurnSignal && ReadCarStatus(TURN_SIGNAL_LAMP) == LEFT_TURN_LIGHT) {
            startCarLeftTurnSignal = true;
            Rtk2DriveTimer(startCarLeftTurnSignalTime, rtkTime);
        }

        if (!checkStartCarSignal && moveDirect == 1) {
            checkStartCarSignal = true;
            if (!startCarLeftTurnSignal) {
                AddExamFault(13, rtkTime);
            } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                                   startCarLeftTurnSignalTime.hour, startCarLeftTurnSignalTime.min, startCarLeftTurnSignalTime.sec, startCarLeftTurnSignalTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) {
                AddExamFault(14, rtkTime);
            }
        }

        if (moveDistance > START_CAR_MOVE_DISTANCE) {
            if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
                DEBUG("Handbreak active move over 10m");
                // 手刹拉起状态下，行驶了10米以上，不合格
                AddExamFault(25, rtkTime);
            } else if (handBreakActive) {
                // 手刹拉起状态下，行驶了1米以上，扣10分
                DEBUG("Handbreak active move over 1M");
                AddExamFault(26, rtkTime);
            }

            PlayTTS("完成起步");
            DEBUG("############# 完成起步 ############");
            startCar = START_CAR_DONE;
        } else if (moveDistance >= START_CAR_CHECK_DOOR_DISTANCE) {
            if (!checkDoor) {
                checkDoor = true;

                if (ReadCarStatus(DOOR) == DOOR_OPEN) {
                    // 车门未完全关闭，不合格
                    DEBUG("车门未关闭");
                    AddExamFault(23, rtkTime);
                }

                if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
                    handBreakActive = true;
                }
            }
        }

        if (ReadCarStatus(ENGINE_RPM) > MAX_ENGINE_RPM && !reportRPMOver) {
            // 转速超标，不合格
            DEBUG("转速超标");
            AddExamFault(29, rtkTime);
            reportRPMOver = true;
        }
    } else {

    }

    return startCar;
}

static void DetectLine(int roadIndex, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, int moveDirect, const struct RtkTime *rtkTime)
@@ -1209,7 +1147,7 @@
        if (GetCrossingStatus(roadIndex, stopIndex) == CROSSING_NOT_HINT) {
            if (!RoadMap.roads[roadIndex].stopLine[stopIndex].tts.empty()) {
                DEBUG("路口提示 %s", RoadMap.roads[roadIndex].stopLine[stopIndex].tts.c_str());
                PlayTTS(RoadMap.roads[roadIndex].stopLine[stopIndex].tts.c_str());
                PlayTTS(RoadMap.roads[roadIndex].stopLine[stopIndex].tts.c_str(), NULL);
            }
            ChangeCrossingStatus(roadIndex, stopIndex, CROSSING_HAS_HINT);
        }
@@ -1273,31 +1211,7 @@

void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
//    uint32_t cts = AppTimer_GetTickCount();
//    int ri = CalcRoadIndex(-1, RoadMap, car);
//    bool crash = CrashRedLine(CrashLineType, 0, RoadMap, car, CarModelList);
//    lane_t laneInfo;
//    double redist = -1;
//
//    laneInfo.road = -1;
//    laneInfo.sep = -1;
//    laneInfo.no = -1;
//
//    if (ri >= 0) {
//        GetLane(laneInfo, car->carXY[car->axial[AXIAL_FRONT]], RoadMap, ri);
//
//        int m = RoadMap.roads[ri].rightEdge.size();
//        int n = RoadMap.roads[ri].rightEdge[m-1].points.size();
//
//        PointF base;
//
//        base.X = 428922.2985; base.Y = 3292119.5457;
//
//        redist = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], base,
//                              RoadMap.roads[ri].rightEdge);
//    }
//
//    DEBUG("当前道路索引 %d, 触发红线 %d lane %d 距离 %f %ld", ri, crash, laneInfo.no, redist, AppTimer_GetTickCount() - cts);
    double BigStraightRoadFree = 0, RoadCrossingFree = 0;

    UpdateCarSensor(rtkTime);

@@ -1504,7 +1418,7 @@
    }

    if (currExamMapIndex >= 0 && Lane.guide == 0) {
        double BigStraightRoad = AnalysisRoad(RoadMap, currExamMapIndex, Lane, car);
        BigStraightRoadFree = AnalysisRoad(RoadMap, currExamMapIndex, Lane, car);

        road_end_point_t ep = NearbyRoadEndPoint(currExamMapIndex, RoadMap, car);

@@ -1517,88 +1431,17 @@
            freeSepDis = ep.distance;
        }

        DEBUG("直道剩余距离 %f, 车道剩余距离 %f", BigStraightRoad, freeSepDis);
        RoadCrossingFree = freeSepDis;

        if (startCar == START_CAR_DONE) {
            if (itemExec[0] == 1 || itemExec[1] == 1 || itemExec[2] == 1 || itemExec[3] == 1) {

                DEBUG("项目执行距离<%d %d %d %d> %f", itemExec[0], itemExec[1], itemExec[2], itemExec[3], ReadOdo() - odo);

                if (ReadOdo() - odo > 120) {
                    odo = ReadOdo();
                    if (itemExec[0] == 1) {
                        itemExec[0] = 2;
                    }
                    if (itemExec[1] == 1) {
                        itemExec[1] = 2;
                    }
                    if (itemExec[2] == 1) {
                        itemExec[2] = 2;
                    }
                    if (itemExec[3] == 1) {
                        itemExec[3] = 2;
                    }
                }
                goto BIG_DOG;
            }

            if (itemExec[0] == 2 || itemExec[1] == 2 || itemExec[2] == 2 || itemExec[3] == 2) {
                DEBUG("项目休息距离<%d %d %d %d> %f", itemExec[0], itemExec[1], itemExec[2], itemExec[3], ReadOdo() - odo);

                if (ReadOdo() - odo > 100) {
                    if (itemExec[0] == 2) {
                        itemExec[0] = 3;
                    }
                    if (itemExec[1] == 2) {
                        itemExec[1] = 3;
                    }
                    if (itemExec[2] == 2) {
                        itemExec[2] = 3;
                    }
                    if (itemExec[3] == 2) {
                        itemExec[3] = 3;
                    }
                }
                goto BIG_DOG;
            }


            if (BigStraightRoad > 170 && ep.distance > 170) {
                if (itemExec[0] == 0) {
                    PlayTTS("二狗直线行驶");
                    itemExec[0] = 1;
                    odo = ReadOdo();
                }
            } else if (BigStraightRoad > 150 && ep.distance > 150) {
                if (itemExec[3] == 0) {
                    PlayTTS("二狗加减档");
                    itemExec[3] = 1;
                    odo = ReadOdo();
                }
            } else if (freeSepDis > 150) {
                if (itemExec[2] == 0) {
                    PlayTTS("二狗变道");
                    itemExec[2] = 1;
                    odo = ReadOdo();
                } else if (itemExec[1] == 0) {
                    PlayTTS("二狗超车");
                    itemExec[1] = 1;
                    odo = ReadOdo();
                }
            }

            BIG_DOG:;
        }
        DEBUG("直道剩余距离 %f, 车道剩余距离 %f", BigStraightRoadFree, RoadCrossingFree);
    }

    // 检测压线状态
    TestRoadStartCar(car, speed, moveDirect, rtkTime);

    // 额外的转向检测
    DetectTurn(car, moveDirect, rtkTime);

    if (startCar != START_CAR_DONE)
        return;
    ItemExam(RoadMap, car, CarModelList, speed, moveDirect, rtkTime, BigStraightRoadFree, RoadCrossingFree);

    if (ReadOdo() > EXAM_RANGE && currRoadItem == NULL && AllCmp(RoadMap) && stopCar == STOP_CAR_NOT_DO) {
        // 在合适条件下停车结束考试
@@ -1661,6 +1504,63 @@
            }
        }
    }*/
}

static void ItemExam(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end)
{
    static double freeRunDistance;
    static double totalRunDistance;

    if (RoadExamStatus == ROAD_EXAM_READY_NEXT) {
        if (RoadExamItem[ROAD_EXAM_ITEM_CAR_START] == ROAD_EXAM_ITEM_NOT_EXEC) {
            CarStartInit();
            totalRunDistance = ReadOdo();
            RoadExamStatus = ROAD_EXAM_ITEM_CAR_START;
        } else {
            if (RoadExamItem[ROAD_EXAM_ITEM_STRAIGHT] == ROAD_EXAM_ITEM_NOT_EXEC) {
                if (straight > 170 && road_end > 170) {

                }
            }
            if (RoadExamItem[ROAD_EXAM_ITEM_OP_GEAR] == ROAD_EXAM_ITEM_NOT_EXEC) {
                if (straight > 150 && road_end > 150) {
                    StartOperateGearExam();
                    RoadExamStatus = ROAD_EXAM_ITEM_OP_GEAR;
                }
            }
            if (RoadExamItem[ROAD_EXAM_ITEM_CHANGE_LANE] == ROAD_EXAM_ITEM_NOT_EXEC) {

            }
            if (RoadExamItem[ROAD_EXAM_ITEM_OVER_TAKE] == ROAD_EXAM_ITEM_NOT_EXEC) {

            }

        }
    } else if (RoadExamStatus == ROAD_EXAM_FREE_RUN) {
        if (ReadOdo() - freeRunDistance > 300) {
            RoadExamStatus = ROAD_EXAM_READY_NEXT;
        }
    } else {
        bool testing = false;

        switch (RoadExamStatus) {
            case ROAD_EXAM_ITEM_CAR_START:
                testing = TestCarStart(car, speed, moveDirect, rtkTime);
                break;
            case ROAD_EXAM_ITEM_OP_GEAR:
                testing = TestOperateGear(rtkTime);
                break;
            default:break;
        }

        if (!testing) {
            RoadExamItem[RoadExamStatus] = ROAD_EXAM_ITEM_EXECED;

            RoadExamStatus = ROAD_EXAM_FREE_RUN;
            freeRunDistance = ReadOdo();
        }
    }

}

void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
@@ -1755,7 +1655,7 @@
            turnTimeCnt += TimeGetDiff(rtkTime, &prevDetectTurnTime);
            int wise = isTurn((int) car->yaw, startTurnYaw, TURN_THRESHOLD);
            DEBUG("转动角度 %d", wise);
            if (ABS(wise) > 60) {
            if (ABS(wise) > CROSSING_TURN_THRESHOLD) {
                // 确认转弯行为，检测开始刚转弯时转向灯情况
                turnCnt = -1;