修改车辆传感器，未完成。

fctom1215

2020-03-14 6d6f675d0bf6bcfd6afec95be24c8b982ae3ee1d

修改车辆传感器，未完成。

 lib/src/main/cpp/driver_test.cpp

@@ -101,9 +101,6 @@
#define RTK_BUFFER_SIZE            100
#define CAR_MODEL_CACHE_SIZE      10

static pthread_mutex_t rtk_clock_mutex = PTHREAD_MUTEX_INITIALIZER;
struct RtkTime rtkClock;

static rtk_info *RtkBuffer = NULL;
static int RtkBufferNum = 0, RtkBufferIn = 0;

@@ -121,9 +118,6 @@

void DriverTestInit(void)
{
    pthread_mutex_init(&rtk_clock_mutex, NULL);
    memset(&rtkClock, 0, sizeof(rtkClock));

    ExamStart = false;
    memset(&MapList, 0, sizeof(MapList));
    MapNum = 0;
@@ -397,16 +391,10 @@
    }
}

void GetRtkClock(struct RtkTime *s)
{
    pthread_mutex_lock(&rtk_clock_mutex);
    *s = rtkClock;
    pthread_mutex_unlock(&rtk_clock_mutex);
}

void UpdateRTKInfo(const rtk_info *s)
{
    pthread_mutex_lock(&rtk_clock_mutex);
    struct RtkTime rtkClock;

    rtkClock.YY = s->YY;
    rtkClock.MM = s->MM;
    rtkClock.DD = s->DD;
@@ -414,7 +402,10 @@
    rtkClock.mm = s->mm;
    rtkClock.ss = s->ss;
    rtkClock.mss = s->dss;
    pthread_mutex_unlock(&rtk_clock_mutex);

    if (ExamStart) {
        ExecuteExam(&rtkClock);     // 执行无需车辆定位的项目
    }

    if (s->qf == 3) {
        RtkBuffer[RtkBufferIn] = *s;
@@ -517,10 +508,15 @@
    }
}

static void ExecuteExam(const struct RtkTime* rtkTime)
{

}

static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime)
{
    if (move != 0) {
        if (ReadSensor(SENSOR_SEATBELT) == 0 && !reportSeatbeltEject) {
        if (ReadCarStatus(SEATBELT) == EJECT_SEATBELT && !reportSeatbeltEject) {
            DEBUG("不系安全带");
            reportSeatbeltEject = true;
            AddExamFault(1, rtkTime);

 lib/src/main/cpp/driver_test.h

@@ -88,10 +88,10 @@
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
                        int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
                        int *body, int bodyNum, double (*point)[2], int pointNum, double antPitch);
void SensorChanged(int *sensorId, int *sensorValue, int num);

void StartDriverExam(int start, int type);
void StartMapExam(int map_id, int exam);
void GetRtkClock(struct RtkTime *s);

void UpdateRTKInfo(const rtk_info *s);
void AddExamFault(int wrong, const struct RtkTime *rtkTime);
car_model_cache_t *GetCarModelCache(int node);

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

@@ -13,18 +13,53 @@

#define MAX_SENSOR_NUM          32

static uint16_t gpioStore = 0;
static int engine_rpm = 0;
static uint16_t gpioStore;
static int left_turn_signal, right_turn_signal;
static int flashMainBeamCnt;

enum {
    SENSOR_SEATBELT,
    SENSOR_LEFT_TURN_SIGNAL,
    SENSOR_RIGHT_TURN_SIGNAL,
    SENSOR_HANDBREAK,
    SENSOR_BREAK,
    SENSOR_CLEARANCE_LIGHT,
    SENSOR_DIPPED_BEAM_LIGHT,
    SENSOR_MAIN_BEAM_LIGHT,
    SENSOR_DOOR,
    SENSOR_SHIFT_N,
    SENSOR_SHIFT_1,
    SENSOR_SHIFT_2,
    SENSOR_SHIFT_3,
    SENSOR_SHIFT_4,
    SENSOR_SHIFT_5,
    SENSOR_SHIFT_R,
    SENSOR_ENGINE_START,
    SENSOR_ENGINE_RPM,
    SENSOR_SPEED
};



static int CarStatus[CAR_STATUS_END];

static struct sensor_cfg {
    int gpioId;
    int funId;
    int validLvl;
    int currValue;
} SensorConfig[MAX_SENSOR_NUM];
static int SensorNum = 0;

static pthread_mutex_t sonser_mutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t status_rw_mutex = PTHREAD_MUTEX_INITIALIZER;

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);
static void confirmFlashBeamLightLater(union sigval sig);
static void SensorChanged(int id, int value);

void CarSensorInit(void)
{
@@ -32,7 +67,13 @@
    SensorNum = 0;
    memset(SensorConfig, 0, sizeof(SensorConfig));

    memset(CarStatus, 0, sizeof(CarStatus));

    left_turn_signal = right_turn_signal = 0;
    flashMainBeamCnt = 0;

    pthread_mutex_init(&sonser_mutex, NULL);
    pthread_mutex_init(&status_rw_mutex, NULL);
}

void SetSensorCfg(int (*sensor)[3], int sensorNum)
@@ -50,68 +91,185 @@
        } else {
            SensorConfig[i].validLvl = 0;
        }

        int level = 0;

        if (gpioStore & BV(SensorConfig[i].gpioId)) {
            SensorConfig[i].currValue = 1;
            level = 1;
        }

        if (level == SensorConfig[i].validLvl) {
            SensorChanged(SensorConfig[i].funId, 1);
        } else {
            SensorConfig[i].currValue = 0;
            SensorChanged(SensorConfig[i].funId, 0);
        }
    }

    pthread_mutex_unlock(&sonser_mutex);
}

void UpdateSensor(uint16_t gpio, uint16_t speed, uint16_t engine)
void UpdateSensor(uint16_t gpio, uint16_t speed, uint16_t rpm)
{
    uint16_t chg = 0;

    int reportSensor[MAX_SENSOR_NUM];
    int reportValue[MAX_SENSOR_NUM];
    int reportNum = 0;

    pthread_mutex_lock(&sonser_mutex);

    chg = gpioStore^gpio;
    gpioStore = gpio;
    engine_rpm = engine;
    pthread_mutex_unlock(&sonser_mutex);

    WriteCarStatus(OBD_SPEED, speed);
    WriteCarStatus(ENGINE_RPM, rpm);

    for (int i = 0; i < SensorNum; ++i) {
        if (chg & BV(SensorConfig[i].gpioId)) {
            int level = 0;

            if (gpio & BV(SensorConfig[i].gpioId)) {
                SensorConfig[i].currValue = 1;
                level = 1;
            }

            if (level == SensorConfig[i].validLvl) {
                SensorChanged(SensorConfig[i].funId, 1);
            } else {
                SensorConfig[i].currValue = 0;
                SensorChanged(SensorConfig[i].funId, 0);
            }

            reportSensor[reportNum] =  SensorConfig[i].funId;
            reportValue[reportNum] =  (SensorConfig[i].validLvl == SensorConfig[i].currValue)?1:0;
            reportNum++;
        }
    }

    reportSensor[reportNum] = SENSOR_ENGINE_RPM;
    reportValue[reportNum] = engine;
    reportNum++;

    pthread_mutex_unlock(&sonser_mutex);

    SensorChanged(reportSensor, reportValue, reportNum);
}

int ReadSensor(uint16_t sensor_id)
int ReadCarStatus(uint16_t id)
{
    int ret = -1;
    int value;

    pthread_mutex_lock(&sonser_mutex);
    for (int i = 0; i < SensorNum; ++i) {
        if (sensor_id == SensorConfig[i].funId) {
            ret = (SensorConfig[i].validLvl == SensorConfig[i].currValue)?1:0;
    pthread_mutex_lock(&status_rw_mutex);
    value = CarStatus[id];
    pthread_mutex_unlock(&status_rw_mutex);

    return value;
}

static void WriteCarStatus(uint16_t id, int value)
{
    pthread_mutex_lock(&status_rw_mutex);
    CarStatus[id] = value;
    pthread_mutex_unlock(&status_rw_mutex);
}

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

    WriteCarStatus(TURN_SIGNAL_LAMP, OFF_LIGHT);
}

static void ChangeLRLight(int light)
{
    WriteCarStatus(TURN_SIGNAL_LAMP, light);

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

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

    ChangeLRLight(sig.sival_int);
}

static void flashBeamLightClose(union sigval sig)
{
    WriteCarStatus(FLASH_BEAM_LAMP, OFF_LIGHT);
}

static void confirmFlashBeamLightLater(union sigval sig)
{
    AppTimer_delete(confirmFlashBeamLightLater);
    flashMainBeamCnt = 0;
}

static void SensorChanged(int id, int value)
{
    switch (id) {
        case SENSOR_LEFT_TURN_SIGNAL: {
            left_turn_signal = value;

            if (left_turn_signal) {
                AppTimer_delete(ConfirmTurnSigalLater);
                if (right_turn_signal) {
                    // 判定为双闪
                    ChangeLRLight(HAZARD_LIGHTS);
                } else {
                    AppTimer_add(ConfirmTurnSigalLater, 200, LEFT_TURN_LIGHT);
                }
            }
            break;
        }
        case SENSOR_RIGHT_TURN_SIGNAL: {
            right_turn_signal = value;

            if (right_turn_signal) {
                AppTimer_delete(ConfirmTurnSigalLater);
                if (left_turn_signal) {
                    // 判定为双闪
                    ChangeLRLight(HAZARD_LIGHTS);
                } else {
                    AppTimer_add(ConfirmTurnSigalLater, 200, RIGHT_TURN_LIGHT);
                }
            }
            break;
        }
        case SENSOR_CLEARANCE_LIGHT: {
            if (value == 0) {
                WriteCarStatus(CLEARANCE_LAMP, OFF_LIGHT);
            } else {
                WriteCarStatus(CLEARANCE_LAMP, CLEARANCE_LIGHT);
            }
            break;
        }
        case SENSOR_DIPPED_BEAM_LIGHT: {
            if (value == 0) {
                WriteCarStatus(DIPPED_BEAM_LAMP, OFF_LIGHT);
            } else {
                WriteCarStatus(DIPPED_BEAM_LAMP, DIPPED_BEAM_LIGHT);
            }
            break;
        }
        case SENSOR_MAIN_BEAM_LIGHT: {
            if (value == 0) {
                WriteCarStatus(MAIN_BEAM_LAMP, OFF_LIGHT);
            } else {
                WriteCarStatus(MAIN_BEAM_LAMP, MAIN_BEAM_LIGHT);
            }

            if (flashMainBeamCnt > 3) {         // 亮-灭-亮-灭
                WriteCarStatus(FLASH_BEAM_LAMP, FLASH_BEAM_LIGHT);

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

            flashMainBeamCnt++;
            AppTimer_delete(confirmFlashBeamLightLater);
            AppTimer_add(confirmFlashBeamLightLater, D_SEC(3));
            break;
        }
        case SENSOR_SEATBELT: {
            if (value == 0) {
                WriteCarStatus(SEATBELT, EJECT_SEATBELT);
            } else {
                WriteCarStatus(SEATBELT, INSERT_SEATBELT);
            }
            break;
        }
        case SENSOR_ENGINE_START: {
            break;
        }
        default:
            break;
        }
    }

    if (sensor_id == SENSOR_ENGINE_RPM) {
        ret = engine_rpm;
    }
    pthread_mutex_unlock(&sonser_mutex);

    return ret;
}

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

@@ -8,28 +8,36 @@
#include <cstdint>

enum {
    SENSOR_SEATBELT,
    SENSOR_LEFT_TURN_SIGNAL,
    SENSOR_RIGHT_TURN_SIGNAL,
    SENSOR_HANDBREAK,
    SENSOR_BREAK,
    SENSOR_LIGHT,
    SENSOR_FAR_LIGHT,
    SENSOR_DOOR,
    SENSOR_SHIFT_N,
    SENSOR_SHIFT_1,
    SENSOR_SHIFT_2,
    SENSOR_SHIFT_3,
    SENSOR_SHIFT_4,
    SENSOR_SHIFT_5,
    SENSOR_SHIFT_R,
    SENSOR_ENGINE_START,
    SENSOR_ENGINE_RPM
    OBD_SPEED,
    ENGINE_RPM,
    SHIFT,
    TURN_SIGNAL_LAMP,
    DIPPED_BEAM_LAMP,
    FOG_LAMP,
    CLEARANCE_LAMP,
    FLASH_BEAM_LAMP,
    MAIN_BEAM_LAMP,
    SEATBELT,
    CAR_STATUS_END
};

enum {
    OFF_LIGHT = 0,
    EJECT_SEATBELT = 0,
    HAZARD_LIGHTS,
    LEFT_TURN_LIGHT,
    RIGHT_TURN_LIGHT,
    CLEARANCE_LIGHT,
    DIPPED_BEAM_LIGHT,
    MAIN_BEAM_LIGHT,
    FLASH_BEAM_LIGHT,
    FOG_LIGHT,
    INSERT_SEATBELT
};

void CarSensorInit(void);
int ReadSensor(uint16_t sensor_id);
void UpdateSensor(uint16_t gpio, uint16_t speed, uint16_t engine);
int ReadCarStatus(uint16_t id);
void UpdateSensor(uint16_t gpio, uint16_t speed, uint16_t rpm);
void SetSensorCfg(int (*sensor)[3], int sensorNum);

#endif //MYAPPLICATION2_COMM_TEST_H