起步和直线

yy1717

2020-03-23 682b17ff66dff23e03c6a57de276ea0c3e670c0e

起步和直线

 lib/src/main/cpp/CMakeLists.txt

@@ -40,6 +40,7 @@
        test_items2/dummy_light.cpp
        test_items2/road_exam.cpp
        test_items2/through_something.cpp
        test_items2/drive_straight.cpp

        rtk_module/rtk.cpp
        rtk_module/virtual_rtk.cpp

 lib/src/main/cpp/driver_test.cpp

@@ -257,6 +257,15 @@
        if (map.area.point != NULL) {
            free(map.area.point);
        }

        if (map.roadEdgeLine != NULL) {
            for (int j = 0; j < map.roadEdgeLineNum; ++j) {
                if (map.roadEdgeLine[j].point != NULL)
                    free(map.roadEdgeLine[j].point);
            }

            free(map.roadEdgeLine);
        }
    }

    RoadMapList.clear();
@@ -282,6 +291,7 @@
                      vector<vector<int>> &redAreas,
                      vector<vector<int>> &greenLines,
                      vector<vector<int>> &triggerLines,
                      vector<vector<int>> &roadEdgeLines,
                      vector<int> area,
                      vector<int> stopLine)
{
@@ -371,7 +381,24 @@
        }
    } else {
        newMap.triggerLine = NULL;
    };
    }

    if ((newMap.roadEdgeLineNum = roadEdgeLines.size()) > 0) {
        newMap.roadEdgeLine = (Polygon *) malloc(sizeof(Polygon) * newMap.roadEdgeLineNum);

        DEBUG("道路边线 %d 根", newMap.roadEdgeLineNum);
        for (int i = 0; i < newMap.roadEdgeLineNum; ++i) {
            newMap.roadEdgeLine[i].num = roadEdgeLines[i].size();
            newMap.roadEdgeLine[i].point = (PointF *) malloc(sizeof(PointF) * newMap.roadEdgeLine[i].num);

            DEBUG("    结点 %d 个", newMap.roadEdgeLine[i].num);
            for (int j = 0; j < newMap.roadEdgeLine[i].num; ++j) {
                newMap.roadEdgeLine[i].point[j] = RoadMapPoints.point[roadEdgeLines[i][j]];
            }
        }
    } else {
        newMap.roadEdgeLine = NULL;
    }

    if (area.size() > 0) {
        newMap.area.num = area.size();

 lib/src/main/cpp/driver_test.h

@@ -107,6 +107,9 @@
    int triggerLineNum;
    trigger_line_t *triggerLine;

    int roadEdgeLineNum;    // 道路边线，只有直线驾驶、靠边停车才有
    Polygon *roadEdgeLine;

    Polygon area;           // 子项目的区域
    Line stopLine;         // 诸如人行道、路口等考点的停止线
    int flagStop;           // 到达开始线前，是否需要停车
@@ -125,7 +128,8 @@
void CleanRoadMap(void);
void SetRoadMapPoints(vector<double> &mapPoints);
void AddRoadMapParent(int id, int type, string tts, int stopFlag, vector<vector<int>> &redLines,
                      vector<vector<int>> &redAreas, vector<vector<int>> &greenLines, vector<vector<int>> &triggerLines,
                      vector<vector<int>> &redAreas, vector<vector<int>> &greenLines,
                      vector<vector<int>> &triggerLines, vector<vector<int>> &roadEdgeLines,
                      vector<int> area, vector<int> stopLine);

void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,

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

@@ -526,9 +526,12 @@
                            vector<vector<int>> greenLines;
                            vector<vector<int>> triggerLines;
                            vector<vector<int>> redAreas;
                            vector<vector<int>> roadEdgeLines;

                            vector<int> area;
                            vector<int> stopLine;

                            roadEdgeLines.clear();
                            stopLine.clear();
                            area.clear();
                            tts.clear();
@@ -595,6 +598,20 @@
                                }
                            }

                            if (itr->HasMember("road_edge_line")) {
                                const Value &s = (*itr)["road_edge_line"];

                                for (Value::ConstValueIterator itrLine = s.Begin();
                                     itrLine != s.End(); ++itrLine) {
                                    points.clear();
                                    for (Value::ConstValueIterator itrPoint = (*itrLine).Begin();
                                         itrPoint != (*itrLine).End(); ++itrPoint) {
                                        points.push_back((*itrPoint).GetInt());
                                    }
                                    roadEdgeLines.push_back(points);
                                }
                            }

                            if (itr->HasMember("area")) {
                                const Value &s = (*itr)["area"];

@@ -633,7 +650,7 @@
                                tts = s.GetString();
                            }

                            AddRoadMapParent(id, type, tts, stop_flag, redLines, redAreas, greenLines, triggerLines, area, stopLine);
                            AddRoadMapParent(id, type, tts, stop_flag, redLines, redAreas, greenLines, triggerLines, roadEdgeLines, area, stopLine);
                        }
                    }
                }

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

@@ -29,6 +29,10 @@
const char *VIRTUAL_RTK_IP = "192.168.16.100";
const int VIRTUAL_RTK_PORT = 9001;

static pthread_mutex_t tts_mutex = PTHREAD_MUTEX_INITIALIZER;

static int ttsSeq = 0;

static void SendBootIndicate(union sigval sig);

int DESEncrypt(const uint8_t *key, int key_length,
@@ -195,7 +199,18 @@
    }
}

void PlayTTS(const char *string, int id)
static int GetTtsSeq(void)
{
    int seq = 0;

    pthread_mutex_lock(&tts_mutex);
    seq = ttsSeq++;
    pthread_mutex_unlock(&tts_mutex);

    return seq;
}

int PlayTTS(const char *string)
{
    DEBUG("PlayTTS: %s", string);

@@ -206,7 +221,7 @@
        // Attach主线程
        if (sg_jvm->AttachCurrentThread(&env, NULL) != JNI_OK) {
            LOGE("%s: AttachCurrentThread() failed", __FUNCTION__);
            return;
            return -1;
        }
    } else {
        ready_in_java_env = true;
@@ -214,6 +229,8 @@

    jclass cls = env->GetObjectClass(sg_obj);
    jmethodID fun = env->GetMethodID(cls, "TextSpeak", "(Ljava/lang/String;I)V");

    int id = GetTtsSeq();

    env->CallVoidMethod(sg_obj, fun, env->NewStringUTF(string), id);

@@ -225,6 +242,8 @@
            LOGE("%s: DetachCurrentThread() failed", __FUNCTION__);
        }
    }

    return id;
}

extern "C"
@@ -248,6 +267,8 @@
    AppTimer_add(SendBootIndicate, 500);

    InitVirtualDevice(VIRTUAL_RTK_IP, VIRTUAL_RTK_PORT);

    pthread_mutex_init(&tts_mutex, NULL);
}

static void SendBootIndicate(union sigval sig) {

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

@@ -18,6 +18,6 @@
void TextOsd(int type, const char *text);
void DrawScreen(const Polygon *map, const Polygon *car);
void SendMsgToMainProc(int cmd, const char *value);
void PlayTTS(const char *string, int id);
int PlayTTS(const char *string);

#endif //RTKBASESTATION_NATIVE_LIB_H

 lib/src/main/cpp/test_common/car_sensor.cpp

@@ -84,6 +84,8 @@

    SensorNum = sensorNum;
    for (int i = 0; i < sensorNum; ++i) {
        DEBUG("    配置<%d>: id %d - fun %d - lvl %d", i, sensor[i][0], sensor[i][1], sensor[i][2]);

        SensorConfig[i].gpioId = sensor[i][0];
        SensorConfig[i].funId = sensor[i][1];
        if (sensor[i][2] > 0) {

 lib/src/main/cpp/test_common/car_sensor.h

@@ -22,6 +22,7 @@
    BREAK,
    HAND_BREAK,
    SECOND_BREAK,
    DOOR,
    CAR_STATUS_END              //////////////
};

@@ -30,6 +31,7 @@
    EJECT_SEATBELT = 0,
    ENGINE_START_INACTIVE = 0,
    BREAK_INACTIVE = 0,
    DOOR_OPEN = 0,
    HAZARD_LIGHTS,
    LEFT_TURN_LIGHT,
    RIGHT_TURN_LIGHT,
@@ -47,7 +49,8 @@
    GEAR_3,
    GEAR_4,
    GEAR_5,
    BREAK_ACTIVE
    BREAK_ACTIVE,
    DOOR_CLOSE
};

void CarSensorInit(void);

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

New file
@@ -0,0 +1,80 @@
//
// Created by YY on 2020/3/23.
//

#include "drive_straight.h"
#include "../driver_test.h"
#include "../native-lib.h"
#include "../jni_log.h"
#include "road_exam.h"
#include <cmath>

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

static bool crossStartLine;
static bool reportOffsetOver;
static double edgeDistance;

void StartDriveStraightExam(int index, LIST_ROAD_MAP &RoadMapList) {
    if (index == -1)
        return;

    DEBUG("进入路考直线行驶地图 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type);

    if (!RoadMapList[index].tts.empty()) {
        DEBUG("播放TTS");
        PlayTTS(RoadMapList[index].tts.c_str());
    } else {
        DEBUG("没有TTS");
    }

    crossStartLine = false;
    reportOffsetOver = false;
}

int ExecuteDriveStraightExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
                             LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) {
    Line road_edge;

    static PointF startPoint;

    MakeLine(&road_edge, &RoadMapList[index].roadEdgeLine[0].point[0], &RoadMapList[index].roadEdgeLine[0].point[1]);

    if (!crossStartLine) {
        PointF p1, p2;

        p1.X = RoadMapList[index].stopLine.X1;
        p1.Y = RoadMapList[index].stopLine.Y1;
        p2.X = RoadMapList[index].stopLine.X2;
        p2.Y = RoadMapList[index].stopLine.Y2;

        if (IntersectionOfLine(p1, p2, car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1) {
            crossStartLine = true;
            startPoint = car->basePoint;
            edgeDistance = DistanceOf(car->basePoint, road_edge);
        }
    } else {
        double runDistance;
        runDistance = DistanceOf(car->basePoint, startPoint);

        if (runDistance < 100.0) {
            if (!reportOffsetOver && fabs(DistanceOf(car->basePoint, road_edge) - edgeDistance) > 0.3) {
                DEBUG("直线偏移大于30厘米");
                // 偏移大于30厘米，不合格
                AddExamFault(30, rtkTime);
                reportOffsetOver = true;
            }
        } else {
            startPoint = car->basePoint;
            edgeDistance = DistanceOf(car->basePoint, road_edge);
            reportOffsetOver = false;
        }
    }

    if (ExitSonArea(index, RoadMapList, car)) {
        DEBUG("离开直线行驶区域");
        return -1;
    }

    return index;
}

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

New file
@@ -0,0 +1,14 @@
//
// Created by YY on 2020/3/23.
//

#ifndef MYAPPLICATION2_DRIVE_STRAIGHT_H
#define MYAPPLICATION2_DRIVE_STRAIGHT_H

#include "../driver_test.h"

void StartDriveStraightExam(int index, LIST_ROAD_MAP &RoadMapList);
int ExecuteDriveStraightExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
                             LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);

#endif //MYAPPLICATION2_DRIVE_STRAIGHT_H

 lib/src/main/cpp/test_items2/dummy_light.cpp

@@ -11,12 +11,22 @@

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

enum {
    TTS_NOT_START,
    TTS_DOING,
    TTS_DONE,
    WAIT_OPERATE,
    CHECK_OPERATE
};

static struct RtkTime currRtkTime;
static struct dummy_light_exam *content;
static int contentNum;
static int currContent;

static int checkCnt;
static bool turn_left_active, flash_beam_active;
static int examTtsSeq = 0;
static bool testing;

static void DummyLightCheckActive(union sigval sig);
static void ExamDummyLight(union sigval sig);
@@ -26,10 +36,13 @@
    DEBUG("StartDummyLightExam");
    content = ptr;
    contentNum = num;
    currContent = 0;

    currRtkTime = *rtkTime;

    AppTimer_delete(DummyLightCheckActive);
    for (int i = 0; i < contentNum; ++i) {
        content[i].itemStatus = TTS_NOT_START;
    }
    testing = true;
    AppTimer_delete(ExamDummyLight);
    AppTimer_add(ExamDummyLight, D_SEC(2));
}
@@ -37,31 +50,28 @@
int ExecuteDummyLightExam(const struct RtkTime* rtkTime)
{
    currRtkTime = *rtkTime;
    return (currContent == contentNum)?2:1;
    return (testing)?1:2;
}

void DummyLightTTSDone(int id)
{
    DEBUG("DummyLightTTSDone %d", id);
    // 等语音播报完毕后计时
    if (id == 100) {
        AppTimer_add(DummyLightCheckActive, D_SEC(3), id);
    } else if (id == 101) {
        AppTimer_add(DummyLightCheckActive, D_SEC(1), id);
    } else if (id == OVERTAKE) {
        checkCnt = 0;
        turn_left_active = flash_beam_active = false;
        AppTimer_add(DummyLightCheckActive, D_SEC(1), id);
    } else {
        AppTimer_add(DummyLightCheckActive, D_SEC(5), id);
    if (id == examTtsSeq) {
        for (int i = 0; i < contentNum; ++i) {
            if (content[i].itemStatus == TTS_DOING) {
                content[i].itemStatus = TTS_DONE;
                break;
            }
        }

        AppTimer_add(ExamDummyLight, 100);
    }
}

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

    DEBUG("DummyLightCheckActive %d", sig.sival_int);
    DEBUG("DummyLightCheckActive item = %d", sig.sival_int);

    switch (sig.sival_int) {
        case DRIVE_AT_NIGHT:
@@ -119,17 +129,53 @@
            break;
    }

    AppTimer_add(ExamDummyLight, D_SEC(1));
    for (int i = 0; i < contentNum; ++i) {
        if (content[i].item == sig.sival_int) {
            content[i].itemStatus = CHECK_OPERATE;
            break;
        }
    }

    AppTimer_add(ExamDummyLight, 500);
}

static void ExamDummyLight(union sigval sig)
{
    int i = 0;
    AppTimer_delete(ExamDummyLight);

    if (currContent < contentNum) {
        DEBUG("模拟灯光测试 %s", content[currContent].tts);
    for (; i < contentNum; ++i) {
        switch (content[i].itemStatus) {
            case TTS_NOT_START:
                content[i].itemStatus = TTS_DOING;
                examTtsSeq = PlayTTS(content[i].tts);
                // 等待TTS播放完毕
                return;
            case TTS_DOING:
                return;
            case TTS_DONE:
                content[i].itemStatus = WAIT_OPERATE;

        PlayTTS(content[currContent].tts, content[currContent].item);
        currContent++;
                AppTimer_delete(DummyLightCheckActive);
                if (content[i].item == OVERTAKE) {
                    checkCnt = 0;
                    turn_left_active = flash_beam_active = false;
                    AppTimer_add(DummyLightCheckActive, D_SEC(1), content[i].item);
                }
                else if (content[i].item >= 100)
                    AppTimer_add(DummyLightCheckActive, D_SEC(3), content[i].item);
                else
                    AppTimer_add(DummyLightCheckActive, D_SEC(5), content[i].item);
                return;
            case WAIT_OPERATE:
                return;
            case CHECK_OPERATE:
            default:
                break;
        }
    }

    if (i >= contentNum) {
        testing = false;
    }
}

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

@@ -24,6 +24,7 @@

struct dummy_light_exam {
    int item;
    int itemStatus;
    char tts[512];
};


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

@@ -12,6 +12,7 @@
#include "../native-lib.h"
#include "through_something.h"
#include "../master/comm_if.h"
#include "drive_straight.h"

#include <vector>
#include <list>
@@ -23,6 +24,12 @@
using namespace std;

#define TURN_CHECK_CNT          4

enum {
    START_CAR_NOT_DO,
    START_CAR_DOING,
    START_CAR_DONE
};

static const int TURN_THRESHOLD = 5;
static const int TURN_CHECK_INTERVAL = 500;
@@ -40,7 +47,7 @@
static int prevMoveDirect;
static uint32_t stopTimepoint = 0;
static bool reportStopCarOnRedArea;
static PointF stopPoint;
static PointF stopPoint, startPoint;
static bool prevGearError = false;
static bool prevGearNSlide = false;

@@ -61,8 +68,12 @@
static int gearErrorTime;
static int gearNSlideTime;

static int startCar;
static int currExamMapIndex;

static const int MAX_ENGINE_RPM = 2500;
static const double START_CAR_MOVE_DISTANCE = 0.5;//10.0;
static const double START_CAR_CHECK_DOOR_DISTANCE = 0.1;//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);
@@ -73,8 +84,9 @@
static const double MAX_SPEED = 40.0 * 1000.0 / 3600.0;
static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};

static void TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime);
static void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime);
static char isTurn(int currYaw, int prevYaw);
static char isTurn(int currYaw, int prevYaw, int &ang);
static char CheckCarTurn(LIST_CAR_MODEL &CarModelList);
static bool CrashRedLine(LIST_ROAD_MAP &RoadMapList, const car_model *car);
static bool CrashRedArea(LIST_ROAD_MAP &RoadMapList, const car_model *car);
@@ -107,10 +119,69 @@
    gearNSlideTime = 0;

    currExamMapIndex = -1;

    startCar = START_CAR_NOT_DO;
}

static void TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    double moveDistance;
    static bool checkDoor = false;
    static bool handBreakActive = false;
    static bool reportRPMOver = false;

    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);
        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);
            }
            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 {

    }
}

void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    // 起步检测
    TestRoadStartCar(car, speed, moveDirect, rtkTime);

    // 超速检测
    if (moveDirect != 0 && speed > MAX_SPEED) {
        if (!occurOverSpeed) {
@@ -266,7 +337,7 @@
    // 检查是否持续转向
    char turnDirect = CheckCarTurn(CarModelList);
    if (turnDirect == 'L') {
        PlayTTS("左1", 5692);
//        PlayTTS("左1");
        if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
            if (!reportTurnSignalError) {
                DEBUG("没打左转灯");
@@ -284,7 +355,7 @@
            }
        }
    } else if (turnDirect == 'R') {
        PlayTTS("右1", 5692);
//        PlayTTS("右1");
        if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
            if (!reportTurnSignalError) {
                DEBUG("没打右转灯");
@@ -386,7 +457,7 @@

                // 检查变道前，是否提前转向灯
                if (inter == 1) {
                    PlayTTS("左2", 5698);
//                    PlayTTS("左2");
                    // 向左侧变道
                    DEBUG("向左侧变道");
                    if (turnSignalStatusWhenCrashGreenLine != LEFT_TURN_LIGHT) {
@@ -395,7 +466,7 @@
                        AddExamFault(13, rtkTime);
                    }
                } else {
                    PlayTTS("右2", 5698);
//                    PlayTTS("右2");
                    // 向右侧变道
                    DEBUG("向右侧变道");
                    if (turnSignalStatusWhenCrashGreenLine != RIGHT_TURN_LIGHT) {
@@ -410,20 +481,32 @@
        checkCrashGreenTimeout = 0;
    }

    // 触发线检测
    if (currExamMapIndex == -1) {
    // 完成起步后，触发线检测
    if (currExamMapIndex == -1 && startCar == START_CAR_DONE) {
        currExamMapIndex = CrashTriggerLine(RoadMapList, car, CarModelList);
        if (currExamMapIndex != -1) {
            DEBUG("碰撞触发线");

            MA_EnterMap(RoadMapList[currExamMapIndex].id, RoadMapList[currExamMapIndex].type, 1);

            if (RoadMapList[currExamMapIndex].type >= THROUGH_INTERSECTION_MAP &&
                    RoadMapList[currExamMapIndex].type <= TURN_AROUND_MAP) {
            StartThroughExam(currExamMapIndex, RoadMapList);
            } else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) {
                StartDriveStraightExam(currExamMapIndex, RoadMapList);
        }
    } else {
        }
    } else if (startCar == START_CAR_DONE) {
        int prevIdx = currExamMapIndex;

        if (currExamMapIndex >= THROUGH_INTERSECTION_MAP && currExamMapIndex <= TURN_AROUND_MAP) {
        currExamMapIndex = ExecuteThroughExam(currExamMapIndex, RoadMapList, car,
        CarModelList, speed, moveDirect, rtkTime);
        }
        else if (currExamMapIndex == DRIVE_STRAIGHT_MAP) {
            currExamMapIndex = ExecuteDriveStraightExam(currExamMapIndex, RoadMapList, car,
                                     CarModelList, speed, moveDirect, rtkTime);
        }

        if (currExamMapIndex == -1) {
            MA_EnterMap(RoadMapList[prevIdx].id, RoadMapList[prevIdx].type, 1);
@@ -521,10 +604,9 @@
    tm.msec = rtkTime->mss;
}

static char isTurn(int currYaw, int prevYaw)
static char isTurn(int currYaw, int prevYaw, int &ang)
{
//    DEBUG("currYaw %d prevYaw %d", currYaw, prevYaw);

    int deltaAng = 0;

    if (ABS(currYaw - prevYaw) > 180) {
@@ -532,6 +614,8 @@
    } else {
        deltaAng = ABS(currYaw - prevYaw);
    }

    ang = deltaAng;

//    DEBUG("角度差值 %d", deltaAng);

@@ -545,12 +629,12 @@
        }
    }

    return 0;
    return 'N';
}

static char CheckCarTurn(LIST_CAR_MODEL &CarModelList)
{
    // 最近3秒内，每秒的角度差大于10度，且方向相同，连续3秒，认为转向
    // 最近2秒内，每0.5秒的角度差大于5度，且方向相同，连续4次；或突现超30度的转向；认为转向。
    if (CarModelList.size() < 1)
        return false;

@@ -571,10 +655,15 @@
        uint32_t tdiff = TimeGetDiff(c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);

        if (tdiff >= TURN_CHECK_INTERVAL) {
            turn[checkCnt] = isTurn((int)c1->yaw, (int)c2->yaw);
            int ang = 0;
            turn[checkCnt] = isTurn((int)c1->yaw, (int)c2->yaw, ang);
//            DEBUG("%c  角度比较 %02d:%02d:%02d.%03d  %02d:%02d:%02d.%03d", turn[checkCnt], c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);
            if (turn[checkCnt] == 0) {

            if (turn[checkCnt] == 'N') {
                break;
            } else if (ang >= 30) {
                DEBUG("左右转确认 %c", turn[checkCnt]);
                return turn[checkCnt];
            }

            c1 = c2;
@@ -790,3 +879,5 @@
    }
    return -1;
}



 lib/src/main/cpp/test_items2/through_something.cpp

@@ -26,7 +26,7 @@
    DEBUG("进入路考通过something地图 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type);
    if (!RoadMapList[index].tts.empty()) {
        DEBUG("播放TTS");
        PlayTTS(RoadMapList[index].tts.c_str(), 0);
        PlayTTS(RoadMapList[index].tts.c_str());
    } else {
        DEBUG("没有TTS");
    }