坡起限制条件和手刹起步全阶段生效.

yy1717

2021-02-07 cea2a94fc97e79897cdfd217be8250c075974a1a

坡起限制条件和手刹起步全阶段生效.

 lib/src/main/cpp/driver_test.cpp

@@ -38,6 +38,7 @@
#include "test_items2/road_exam.h"
#include "test_items/area_exam.h"
#include "test_items2/prepare.h"
#include "test_common/odo_graph.h"

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

@@ -891,13 +892,48 @@

static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime)
{
    static bool rec = false;
    static bool handBreakActive = false, handBreakActive2 = false;
    static double startCarMoveDistance;
    static int prevMove = 0;

    if (move != 0) {
        if (ReadCarStatus(SEATBELT) == EJECT_SEATBELT && !reportSeatbeltEject) {
            DEBUG("不系安全带");
            reportSeatbeltEject = true;
            AddExamFault(ExamType == TEST_TYPE_AREA? 10101: 30101, rtkTime);
        }
        if (rec) {
            if (!handBreakActive2 && ReadOdo() - startCarMoveDistance >= examParam.start_car_limit_distance) {
                handBreakActive2 = true;

                if (ExamType == TEST_TYPE_ROAD_DUMMY_LIGHT || ExamType == TEST_TYPE_ROAD_TRUE_LIGHT) {
                    if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
                        DEBUG("Handbreak active move over 10m");
                        // 手刹拉起状态下，行驶了10米以上，不合格
                        AddExamFault(40205, rtkTime);
                    } else if (handBreakActive) {
                        // 手刹拉起状态下，行驶了1米以上，扣10分
                        DEBUG("Handbreak active move over 1M");
                        AddExamFault(40206, rtkTime);
    }
                }
            } else if (!handBreakActive && ReadOdo() - startCarMoveDistance >= examParam.open_door_drive_allow_distance && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
                handBreakActive = true;

                if (ExamType == TEST_TYPE_AREA) {
                    DEBUG("Handbreak active move over 1M");
                    AddExamFault(10107, rtkTime);
                }
            }
        }
    } else if (!rec || prevMove != 0) {          // 记录停车点
        rec = true;
        handBreakActive = handBreakActive2 = false;
        startCarMoveDistance = ReadOdo();
    }

    prevMove = move;

    if (ExamType != TEST_TYPE_AREA) {
        if (exam_dummy_light == 4 || ExamType == TEST_TYPE_ROAD_TRUE_LIGHT || ExamType == TEST_TYPE_ROAD_CALIBRATE) {

 lib/src/main/cpp/rtk_module/rtk.cpp

@@ -225,7 +225,6 @@
#if 1
            if (VirtualIsConnected()) {     //PC模拟用时
                static bool first = false;
                DEBUG("XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX");
                if (!first) {
                    first = true;


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

@@ -469,7 +469,7 @@
            break;
        }
        case RTK_STATUS_EVT: {
            DEBUG("模块信息 %02X %02X %02X %02X %02X %02X", data[0], data[1], data[2], data[3], data[4], data[5]);
//            DEBUG("模块信息 %02X %02X %02X %02X %02X %02X", data[0], data[1], data[2], data[3], data[4], data[5]);
            if (length == 33)
                MA_SendRtkStatus((char *)data, data[32]);
            break;
@@ -808,7 +808,7 @@

void ReceivedRtk(const uint8_t *data, int length)
{
    DEBUG("ReceivedRtk length %d", length);
//    DEBUG("ReceivedRtk length %d", length);
    AppTimer_delete(RequestRtkNoResp);
    rtcmLength = length;
    // 汇报给单片机

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

@@ -15,7 +15,7 @@
#define MAX_SENSOR_NUM          32

static uint16_t gpioStore;
static int left_turn_signal, right_turn_signal;
static bool left_turn_signal, right_turn_signal;

enum {
    SENSOR_SEATBELT,
@@ -62,8 +62,7 @@

inline static int BX(int value);
static void WriteCarStatus(uint16_t id, int value);
static void LRLightTimeout(union sigval sig);
static void ChangeLRLight(int light);

static void ConfirmTurnSigalLater(union sigval sig);
static void flashBeamLightClose(union sigval sig);

@@ -81,7 +80,7 @@

    memset(&Sensor, 0, sizeof(Sensor));

    left_turn_signal = right_turn_signal = 0;
    left_turn_signal = right_turn_signal = false;

    pthread_mutex_init(&sonser_mutex, NULL);
    pthread_mutex_init(&status_rw_mutex, NULL);
@@ -245,42 +244,21 @@
    }
}

static void LRLightTimeout(union sigval sig)
{
    AppTimer_delete(LRLightTimeout);
    if (!left_turn_signal && !right_turn_signal) {
        WriteCarStatus(TURN_SIGNAL_LAMP, OFF_LIGHT);
    } else if (left_turn_signal && right_turn_signal) {
        WriteCarStatus(TURN_SIGNAL_LAMP, HAZARD_LIGHTS);
    } else if (left_turn_signal) {
        WriteCarStatus(TURN_SIGNAL_LAMP, LEFT_TURN_LIGHT);
    } else {
        WriteCarStatus(TURN_SIGNAL_LAMP, RIGHT_TURN_LIGHT);
    }
}

static void ChangeLRLight(int light)
{
    DEBUG("ChangeLRLight %d %d", left_turn_signal, right_turn_signal);
    if (!left_turn_signal && !right_turn_signal) {
        WriteCarStatus(TURN_SIGNAL_LAMP, OFF_LIGHT);
    } else if (left_turn_signal && right_turn_signal) {
        WriteCarStatus(TURN_SIGNAL_LAMP, HAZARD_LIGHTS);
    } else if (left_turn_signal) {
        WriteCarStatus(TURN_SIGNAL_LAMP, LEFT_TURN_LIGHT);
    } else {
        WriteCarStatus(TURN_SIGNAL_LAMP, RIGHT_TURN_LIGHT);
    }

    AppTimer_delete(LRLightTimeout);
    AppTimer_add(LRLightTimeout, 1500);
}

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

    ChangeLRLight(sig.sival_int);
    DEBUG("确认转向灯 左 %d 右 %d", left_turn_signal, right_turn_signal);

    if (!left_turn_signal && !right_turn_signal) {
        WriteCarStatus(TURN_SIGNAL_LAMP, OFF_LIGHT);
    } else if (left_turn_signal && right_turn_signal) {
        WriteCarStatus(TURN_SIGNAL_LAMP, HAZARD_LIGHTS);
    } else if (left_turn_signal) {
        WriteCarStatus(TURN_SIGNAL_LAMP, LEFT_TURN_LIGHT);
    } else {
        WriteCarStatus(TURN_SIGNAL_LAMP, RIGHT_TURN_LIGHT);
    }
}

static void flashBeamLightClose(union sigval sig)
@@ -293,43 +271,19 @@
    DEBUG("状态改变 %d = %d", id, value);
    switch (id) {
        case SENSOR_LEFT_TURN_SIGNAL: {
            left_turn_signal = value;
            left_turn_signal = (bool) (value > 0);

            if (left_turn_signal) {
                AppTimer_delete(ConfirmTurnSigalLater);
                if (right_turn_signal) {
                    // 判定为双闪
                    AppTimer_add(ConfirmTurnSigalLater, 500, HAZARD_LIGHTS);
                } else {
                    AppTimer_add(ConfirmTurnSigalLater, 200, LEFT_TURN_LIGHT);
                }
            } else {
                AppTimer_delete(ConfirmTurnSigalLater);
                if (right_turn_signal)
                    AppTimer_add(ConfirmTurnSigalLater, 1500, RIGHT_TURN_LIGHT);
                else
                    AppTimer_add(ConfirmTurnSigalLater, 1500, OFF_LIGHT);
            }

            AppTimer_add(ConfirmTurnSigalLater, left_turn_signal? 100 : 1100);
            break;
        }
        case SENSOR_RIGHT_TURN_SIGNAL: {
            right_turn_signal = value;
            right_turn_signal = (bool) (value > 0);

            if (right_turn_signal) {
                AppTimer_delete(ConfirmTurnSigalLater);
                if (left_turn_signal) {
                    // 判定为双闪
                    AppTimer_add(ConfirmTurnSigalLater, 500, HAZARD_LIGHTS);
                } else {
                    AppTimer_add(ConfirmTurnSigalLater, 200, RIGHT_TURN_LIGHT);
                }
            } else {
                AppTimer_delete(ConfirmTurnSigalLater);
                if (left_turn_signal)
                    AppTimer_add(ConfirmTurnSigalLater, 1500, LEFT_TURN_LIGHT);
                else
                    AppTimer_add(ConfirmTurnSigalLater, 1500, OFF_LIGHT);
            }

            AppTimer_add(ConfirmTurnSigalLater, right_turn_signal? 100 : 1100);
            break;
        }
        case SENSOR_FOG_LIGHT: {

 lib/src/main/cpp/test_items/area_exam.cpp

@@ -14,6 +14,7 @@
#include "turn_a90.h"
#include "../utils/xconvert.h"
#include "../common/apptimer.h"
#include "../test_common/odo_graph.h"

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

@@ -35,10 +36,13 @@
void InitAreaExam(void)
{
    CurrExamMapIndex = -1;
    ResetOdo();
}

void TestAreaGeneral(LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, double azimuth, const struct RtkTime *rtkTime)
{
    UpdataOdo(speed, moveDirect, rtkTime);

    DetectEnterOrExitMap(car, CarModelList, AreaMapList);

    ExecuteExam(CurrExamMapIndex, AreaMapList, car, CarModelList, speed, moveDirect, azimuth, rtkTime);
@@ -134,7 +138,7 @@
                case MAP_TYPE_STOP_START:
                    DEBUG("进入上坡起步场地 %d", AreaMapList[index].id);

                    StartSAS(AreaMapList[index].id, move, rtkTime);
                    StartSAS(AreaMapList[index].id, &AreaMapList[index].map, car, move, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_PART_EDGE:

 lib/src/main/cpp/test_items/stop_and_start.cpp

@@ -44,9 +44,17 @@
static double DistanceOfTire2Edge(const Polygon *map, const car_model *car);
static bool ExitTestArea(const Polygon *map, const car_model *car);

void StartSAS(int index, int moveDirect, const struct RtkTime *rtkTime)
void StartSAS(int index, const Polygon *map, const car_model *car, int moveDirect, const struct RtkTime *rtkTime)
{
    double yawEdge = YawOf(map->point[8], map->point[0]);

    if (moveDirect < 0 || DeltaYaw(car->yaw, yawEdge) >= 90.0) {
        testing = false;
        return;
    }

    DEBUG("进入坡起项目");

    testing = true;
    mapIndex = index;
    prevMoveDirect = moveDirect;
@@ -68,6 +76,9 @@

int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    if (!testing)
        return 0;

    if (CrashRedLine(map, car)) {
        // 车轮压线，不合格
        if (!occurCrashRedLine) {
@@ -81,6 +92,10 @@

    if (ExitTestArea(map, car)) {
        // 驶离测试区
        if (!stopCar) {
            // 不停车直接离开
            AddExamFault(10103, rtkTime);
        }
        testing = false;
    }


 lib/src/main/cpp/test_items/stop_and_start.h

@@ -9,7 +9,7 @@

using namespace std;

void StartSAS(int index, int moveDirect, const struct RtkTime *rtkTime);
void StartSAS(int index, const Polygon *map, const car_model *car, int moveDirect, const struct RtkTime *rtkTime);
int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime);

#endif //RTKDRIVERTEST_STOP_AND_START_H

 lib/src/main/cpp/test_items2/car_start.cpp

@@ -58,8 +58,7 @@
    }

    if (moveDistance > examParam.start_car_limit_distance) {
        sensor = ReadCarSensorValue(HAND_BREAK);

        /*sensor = ReadCarSensorValue(HAND_BREAK);
        if (sensor.name == HAND_BREAK && sensor.value == BREAK_ACTIVE) {
            DEBUG("Handbreak active move over 10m");
            // 手刹拉起状态下，行驶了10米以上，不合格
@@ -68,7 +67,7 @@
            // 手刹拉起状态下，行驶了1米以上，扣10分
            DEBUG("Handbreak active move over 1M");
            AddExamFault(40206, rtkTime);
        }
        }*/

        PlayTTS(examParam.start_car_end_tts, NULL);
        DEBUG("############# 完成起步 ############");