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

yy1717

2021-02-07 cea2a94fc97e79897cdfd217be8250c075974a1a

 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_delete(ConfirmTurnSigalLater);

            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_delete(ConfirmTurnSigalLater);

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