//
|
// Created by fctom on 2020/2/13.
|
//
|
|
#include <pthread.h>
|
#include "car_sensor.h"
|
#include "../driver_test.h"
|
#include "../defs.h"
|
#include "../common/apptimer.h"
|
#include "../jni_log.h"
|
|
#define DEBUG(fmt, args...) LOGD("<car_sensor> <%s>: " fmt, __func__, ##args)
|
|
#define MAX_SENSOR_NUM 32
|
|
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_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_BREAK2,
|
SENSOR_SURROUND_CAR_1,
|
SENSOR_SURROUND_CAR_2,
|
SENSOR_SURROUND_CAR_3,
|
SENSOR_SURROUND_CAR_4,
|
SENSOR_FOG_LIGHT,
|
SENSOR_CLEARANCE_LIGHT,
|
SENSOR_ENGINE_RPM,
|
SENSOR_SPEED
|
};
|
|
|
|
static int CarStatus[CAR_STATUS_END];
|
|
static struct sensor_cfg {
|
int gpioId;
|
int funId;
|
int validLvl;
|
} SensorConfig[MAX_SENSOR_NUM];
|
static int SensorNum = 0;
|
|
static car_sensor_t Sensor;
|
static pthread_mutex_t sonser_mutex = PTHREAD_MUTEX_INITIALIZER;
|
static pthread_mutex_t status_rw_mutex = PTHREAD_MUTEX_INITIALIZER;
|
|
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);
|
static void confirmFlashBeamLightLater(union sigval sig);
|
static void SensorChanged(int id, int value);
|
|
void CarSensorInit(void)
|
{
|
gpioStore = 0;
|
SensorNum = 0;
|
memset(SensorConfig, 0, sizeof(SensorConfig));
|
|
memset(CarStatus, 0, sizeof(CarStatus));
|
|
memset(&Sensor, 0, sizeof(Sensor));
|
|
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)
|
{
|
DEBUG("加入传感器配置 sensorNum %d", sensorNum);
|
|
pthread_mutex_lock(&sonser_mutex);
|
|
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) {
|
SensorConfig[i].validLvl = 1;
|
} else {
|
SensorConfig[i].validLvl = 0;
|
}
|
|
int level = 0;
|
|
if (gpioStore & BV(SensorConfig[i].gpioId)) {
|
level = 1;
|
}
|
|
if (level == SensorConfig[i].validLvl) {
|
SensorChanged(SensorConfig[i].funId, 1);
|
} else {
|
SensorChanged(SensorConfig[i].funId, 0);
|
}
|
}
|
|
pthread_mutex_unlock(&sonser_mutex);
|
}
|
|
void UpdateSensor(uint16_t gpio, uint16_t speed, uint16_t rpm)
|
{
|
uint16_t chg = 0;
|
|
pthread_mutex_lock(&sonser_mutex);
|
chg = gpioStore^gpio;
|
gpioStore = gpio;
|
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)) {
|
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)
|
{
|
WriteCarStatus(OBD_SPEED, s->speed);
|
WriteCarStatus(ENGINE_RPM, s->engine);
|
|
if (s->leftTurnLamp != Sensor.leftTurnLamp && s->leftTurnLamp != '#') {
|
SensorChanged(SENSOR_LEFT_TURN_SIGNAL, BX(s->leftTurnLamp));
|
}
|
if (s->rightTurnLamp != Sensor.leftTurnLamp && s->rightTurnLamp != '#') {
|
SensorChanged(SENSOR_RIGHT_TURN_SIGNAL, BX(s->rightTurnLamp));
|
}
|
if (s->fogLamp != Sensor.fogLamp && s->fogLamp != '#') {
|
SensorChanged(SENSOR_FOG_LIGHT, BX(s->fogLamp));
|
}
|
if (s->clearanceLamp != Sensor.clearanceLamp && s->clearanceLamp != '#') {
|
SensorChanged(SENSOR_CLEARANCE_LIGHT, BX(s->clearanceLamp));
|
}
|
if (s->dippedBeamLamp != Sensor.dippedBeamLamp && s->dippedBeamLamp != '#') {
|
SensorChanged(SENSOR_DIPPED_BEAM_LIGHT, BX(s->dippedBeamLamp));
|
}
|
if (s->mainBeamLamp != Sensor.leftTurnLamp && s->mainBeamLamp != '#') {
|
SensorChanged(SENSOR_MAIN_BEAM_LIGHT, BX(s->mainBeamLamp));
|
}
|
if (s->seatBelt != Sensor.seatBelt && s->seatBelt != '#') {
|
SensorChanged(SENSOR_SEATBELT, BX(s->seatBelt));
|
}
|
if (s->key != Sensor.key && s->key != '#') {
|
SensorChanged(SENSOR_ENGINE_START, s->key == 3? 1 : 0);
|
}
|
if (s->mainBreak != Sensor.mainBreak && s->mainBreak != '#') {
|
SensorChanged(SENSOR_BREAK, BX(s->mainBreak));
|
}
|
if (s->assBreak != Sensor.assBreak && s->assBreak != '#') {
|
SensorChanged(SENSOR_BREAK2, BX(s->assBreak));
|
}
|
if (s->handBreak != Sensor.handBreak && s->handBreak != '#') {
|
SensorChanged(SENSOR_HANDBREAK, BX(s->handBreak));
|
}
|
if (s->lock != Sensor.lock && s->lock != '#') {
|
SensorChanged(SENSOR_DOOR, BX(s->lock));
|
}
|
if (s->surround1 != Sensor.surround1 && s->surround1 != '#') {
|
SensorChanged(SENSOR_SURROUND_CAR_1, BX(s->surround1));
|
}
|
if (s->surround2 != Sensor.surround2 && s->surround2 != '#') {
|
SensorChanged(SENSOR_SURROUND_CAR_2, BX(s->surround2));
|
}
|
if (s->surround3 != Sensor.surround3 && s->surround3 != '#') {
|
SensorChanged(SENSOR_SURROUND_CAR_3, BX(s->surround3));
|
}
|
if (s->surround4 != Sensor.surround4 && s->surround4 != '#') {
|
SensorChanged(SENSOR_SURROUND_CAR_4, BX(s->surround4));
|
}
|
if (s->gear != Sensor.gear && s->gear != '#') {
|
SensorChanged(GEAR, GEAR_N + s->gear);
|
}
|
|
Sensor = *s;
|
}
|
|
int ReadCarStatus(uint16_t id)
|
{
|
int value;
|
|
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)
|
{
|
DEBUG("状态改变 %d = %d", id, 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_FOG_LIGHT: {
|
if (value == 0) {
|
WriteCarStatus(FOG_LAMP, OFF_LIGHT);
|
} else {
|
WriteCarStatus(FOG_LAMP, FOG_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(2));
|
}
|
|
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: {
|
if (value == 0) {
|
WriteCarStatus(ENGINE_START, ENGINE_START_INACTIVE);
|
} else {
|
WriteCarStatus(ENGINE_START, ENGINE_START_ACTIVE);
|
}
|
break;
|
}
|
case SENSOR_BREAK: {
|
if (value == 0) {
|
WriteCarStatus(BREAK, BREAK_INACTIVE);
|
} else {
|
WriteCarStatus(BREAK, BREAK_ACTIVE);
|
}
|
break;
|
}
|
case SENSOR_BREAK2: {
|
if (value == 0) {
|
WriteCarStatus(SECOND_BREAK, BREAK_INACTIVE);
|
} else {
|
WriteCarStatus(SECOND_BREAK, BREAK_ACTIVE);
|
}
|
break;
|
}
|
case SENSOR_HANDBREAK: {
|
if (value == 0) {
|
WriteCarStatus(HAND_BREAK, BREAK_INACTIVE);
|
} else {
|
WriteCarStatus(HAND_BREAK, BREAK_ACTIVE);
|
}
|
break;
|
}
|
case SENSOR_DOOR: {
|
if (value == 0) {
|
WriteCarStatus(DOOR, DOOR_CLOSE);
|
} else {
|
WriteCarStatus(DOOR, DOOR_OPEN);
|
}
|
break;
|
}
|
case SENSOR_SURROUND_CAR_1: {
|
if (value == 0) {
|
WriteCarStatus(SURROUND_CAR_1, SURROUND_CAR_INACTIVE);
|
} else {
|
WriteCarStatus(SURROUND_CAR_1, SURROUND_CAR_ACTIVE);
|
}
|
break;
|
}
|
case SENSOR_SURROUND_CAR_2: {
|
if (value == 0) {
|
WriteCarStatus(SURROUND_CAR_2, SURROUND_CAR_INACTIVE);
|
} else {
|
WriteCarStatus(SURROUND_CAR_2, SURROUND_CAR_ACTIVE);
|
}
|
break;
|
}
|
case SENSOR_SURROUND_CAR_3: {
|
if (value == 0) {
|
WriteCarStatus(SURROUND_CAR_3, SURROUND_CAR_INACTIVE);
|
} else {
|
WriteCarStatus(SURROUND_CAR_3, SURROUND_CAR_ACTIVE);
|
}
|
break;
|
}
|
case SENSOR_SURROUND_CAR_4: {
|
if (value == 0) {
|
WriteCarStatus(SURROUND_CAR_4, SURROUND_CAR_INACTIVE);
|
} else {
|
WriteCarStatus(SURROUND_CAR_4, SURROUND_CAR_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:
|
break;
|
}
|
}
|
|
inline static int BX(int value)
|
{
|
return (value == 1) ? 1 : 0;
|
}
|
