坐标

yy1717

2024-02-28 27fc91fbe8f88b6885356e68828cfe1ce1db7601

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

@@ -10,6 +10,8 @@
#include "../jni_log.h"
#include "../rtk_platform/platform.h"

using namespace std;

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

#define MAX_SENSOR_NUM          32
@@ -45,8 +47,6 @@
    SENSOR_SPEED
};



static int CarStatus[CAR_STATUS_END];

static struct sensor_cfg {
@@ -74,9 +74,9 @@
    SensorNum = 0;
    memset(SensorConfig, 0, sizeof(SensorConfig));

    memset(CarStatus, 0, sizeof(CarStatus));
    CarStatus[DOOR] = DOOR_CLOSE;
    CarStatus[GEAR] = GEAR_N;
    for (int i = 0; i < CAR_STATUS_END; ++i) {
        CarStatus[i] = -1;
    }

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

@@ -118,36 +118,12 @@
//    pthread_mutex_unlock(&sonser_mutex);
}

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

    sonser_mutex.lock();
    chg = gpioStore^gpio;
    gpioStore = gpio;
    sonser_mutex.unlock();

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

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

void UpdateSensor(const car_sensor_t *s)
/******************************************
 * 初步得到灯光、挡位等IO信号，后续要根据延迟判断
 * 得到转向灯的正确情况
 * @param s
 */
void UpdateSensorHw(const car_sensor_t *s)
{
    WriteCarStatus(OBD_SPEED, s->speed);
    WriteCarStatus(ENGINE_RPM, s->engine);
@@ -226,10 +202,11 @@
static void WriteCarStatus(uint16_t id, int value)
{
    int old_value;
    lock_guard<std::mutex> lock(status_rw_mutex);
    old_value = CarStatus[id];
    CarStatus[id] = value;

    {
        lock_guard<std::mutex> lock(status_rw_mutex);
        old_value = CarStatus[id];
        CarStatus[id] = value;
    }
    if (id != OBD_SPEED && id != ENGINE_RPM && old_value != value) {
        uint8_t buffer[6];

@@ -241,6 +218,10 @@
        buffer[5] = BREAK_UINT32(value, 0);

        PlatformStatusChanged(SENSOR_CHANGE_EVT, buffer, 6);
    } else if (id == OBD_SPEED) {
        UpdateSensor(OBD_SPEED, value);
    } else if (id == ENGINE_RPM) {
        UpdateSensor(ENGINE_RPM, value);
    }
}

@@ -249,7 +230,7 @@
    DEBUG("确认转向灯 左 %d 右 %d", left_turn_signal, right_turn_signal);

    if (!left_turn_signal && !right_turn_signal) {
        WriteCarStatus(TURN_SIGNAL_LAMP, OFF_LIGHT);
        WriteCarStatus(TURN_SIGNAL_LAMP, LIGHT_OFF);
    } else if (left_turn_signal && right_turn_signal) {
        WriteCarStatus(TURN_SIGNAL_LAMP, HAZARD_LIGHTS);
    } else if (left_turn_signal) {
@@ -261,12 +242,13 @@

static void flashBeamLightClose(apptimer_var_t val)
{
    WriteCarStatus(FLASH_BEAM_LAMP, OFF_LIGHT);
    WriteCarStatus(FLASH_BEAM_LAMP, LIGHT_OFF);
}

static void SensorChanged(int id, int value)
{
    DEBUG("状态改变 %d = %d", id, value);

    switch (id) {
        case SENSOR_LEFT_TURN_SIGNAL: {
            left_turn_signal = (bool) (value > 0);
@@ -286,25 +268,25 @@
        }
        case SENSOR_FOG_LIGHT: {
            if (value == 0) {
                WriteCarStatus(FOG_LAMP, OFF_LIGHT);
                WriteCarStatus(FOG_LAMP, LIGHT_OFF);
            } else {
                WriteCarStatus(FOG_LAMP, FOG_LIGHT);
                WriteCarStatus(FOG_LAMP, LIGHT_ON);
            }
            break;
        }
        case SENSOR_CLEARANCE_LIGHT: {
            if (value == 0) {
                WriteCarStatus(CLEARANCE_LAMP, OFF_LIGHT);
                WriteCarStatus(CLEARANCE_LAMP, LIGHT_OFF);
            } else {
                WriteCarStatus(CLEARANCE_LAMP, CLEARANCE_LIGHT);
                WriteCarStatus(CLEARANCE_LAMP, LIGHT_ON);
            }
            break;
        }
        case SENSOR_DIPPED_BEAM_LIGHT: {
            if (value == 0) {
                WriteCarStatus(DIPPED_BEAM_LAMP, OFF_LIGHT);
                WriteCarStatus(DIPPED_BEAM_LAMP, LIGHT_OFF);
            } else {
                WriteCarStatus(DIPPED_BEAM_LAMP, DIPPED_BEAM_LIGHT);
                WriteCarStatus(DIPPED_BEAM_LAMP, LIGHT_ON);
            }
            break;
        }
@@ -312,17 +294,17 @@
            static uint32_t t1 = 0;

            if (value == 0) {
                WriteCarStatus(MAIN_BEAM_LAMP, OFF_LIGHT);
                WriteCarStatus(MAIN_BEAM_LAMP, LIGHT_OFF);
            } else {
                WriteCarStatus(MAIN_BEAM_LAMP, MAIN_BEAM_LIGHT);
                WriteCarStatus(MAIN_BEAM_LAMP, LIGHT_ON);
            }

            if (value != 0) {
                t1 = AppTimer_GetTickCount();
            } else if (AppTimer_GetTickCount() - t1 < D_SEC(3)) {
                // 3秒内完成亮灭，闪灯
                if (ReadCarStatus(FLASH_BEAM_LAMP) != FLASH_BEAM_LIGHT) {
                    WriteCarStatus(FLASH_BEAM_LAMP, FLASH_BEAM_LIGHT);
                if (ReadCarStatus(FLASH_BEAM_LAMP) != LIGHT_ON) {
                    WriteCarStatus(FLASH_BEAM_LAMP, LIGHT_ON);
                }
                AppTimer_delete(flashBeamLightClose);
                AppTimer_add(flashBeamLightClose, D_SEC(2));
@@ -331,41 +313,41 @@
        }
        case SENSOR_SEATBELT: {
            if (value == 0) {
                WriteCarStatus(SEATBELT, EJECT_SEATBELT);
                WriteCarStatus(SEATBELT, EJECT);
            } else {
                WriteCarStatus(SEATBELT, INSERT_SEATBELT);
                WriteCarStatus(SEATBELT, INSERT);
            }
            break;
        }
        case SENSOR_ENGINE_START: {
            if (value == 0) {
                WriteCarStatus(ENGINE_START, ENGINE_START_INACTIVE);
                WriteCarStatus(ENGINE_START, INACTIVE);
            } else {
                WriteCarStatus(ENGINE_START, ENGINE_START_ACTIVE);
                WriteCarStatus(ENGINE_START, ACTIVE);
            }
            break;
        }
        case SENSOR_BREAK: {
            if (value == 0) {
                WriteCarStatus(BREAK, BREAK_INACTIVE);
                WriteCarStatus(BRAKE, INACTIVE);
            } else {
                WriteCarStatus(BREAK, BREAK_ACTIVE);
                WriteCarStatus(BRAKE, ACTIVE);
            }
            break;
        }
        case SENSOR_BREAK2: {
            if (value == 0) {
                WriteCarStatus(SECOND_BREAK, BREAK_INACTIVE);
                WriteCarStatus(SECOND_BRAKE, INACTIVE);
            } else {
                WriteCarStatus(SECOND_BREAK, BREAK_ACTIVE);
                WriteCarStatus(SECOND_BRAKE, ACTIVE);
            }
            break;
        }
        case SENSOR_HANDBREAK: {
            if (value == 0) {
                WriteCarStatus(HAND_BREAK, BREAK_INACTIVE);
                WriteCarStatus(HAND_BRAKE, INACTIVE);
            } else {
                WriteCarStatus(HAND_BREAK, BREAK_ACTIVE);
                WriteCarStatus(HAND_BRAKE, ACTIVE);
            }
            break;
        }
@@ -379,48 +361,36 @@
        }
        case SENSOR_SURROUND_CAR_1: {
            if (value == 0) {
                WriteCarStatus(SURROUND_CAR_1, SURROUND_CAR_INACTIVE);
                WriteCarStatus(SURROUND_CAR_1, INACTIVE);
            } else {
                WriteCarStatus(SURROUND_CAR_1, SURROUND_CAR_ACTIVE);
                WriteCarStatus(SURROUND_CAR_1, ACTIVE);
            }
            break;
        }
        case SENSOR_SURROUND_CAR_2: {
            if (value == 0) {
                WriteCarStatus(SURROUND_CAR_2, SURROUND_CAR_INACTIVE);
                WriteCarStatus(SURROUND_CAR_2, INACTIVE);
            } else {
                WriteCarStatus(SURROUND_CAR_2, SURROUND_CAR_ACTIVE);
                WriteCarStatus(SURROUND_CAR_2, ACTIVE);
            }
            break;
        }
        case SENSOR_SURROUND_CAR_3: {
            if (value == 0) {
                WriteCarStatus(SURROUND_CAR_3, SURROUND_CAR_INACTIVE);
                WriteCarStatus(SURROUND_CAR_3, INACTIVE);
            } else {
                WriteCarStatus(SURROUND_CAR_3, SURROUND_CAR_ACTIVE);
                WriteCarStatus(SURROUND_CAR_3, ACTIVE);
            }
            break;
        }
        case SENSOR_SURROUND_CAR_4: {
            if (value == 0) {
                WriteCarStatus(SURROUND_CAR_4, SURROUND_CAR_INACTIVE);
                WriteCarStatus(SURROUND_CAR_4, INACTIVE);
            } else {
                WriteCarStatus(SURROUND_CAR_4, SURROUND_CAR_ACTIVE);
                WriteCarStatus(SURROUND_CAR_4, ACTIVE);
            }
            break;
        }
//        case SENSOR_SHIFT_N:
//        case SENSOR_SHIFT_1:
//        case SENSOR_SHIFT_2:
//        case SENSOR_SHIFT_3:
//        case SENSOR_SHIFT_4:
//        case SENSOR_SHIFT_5:
//        case SENSOR_SHIFT_R: {
//            if (value != 0) {
//                WriteCarStatus(GEAR, id - SENSOR_SHIFT_N + GEAR_N);
//            }
//            break;
//        }
        default:
            return;
    }