From 6d6f675d0bf6bcfd6afec95be24c8b982ae3ee1d Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期六, 14 三月 2020 23:44:01 +0800
Subject: [PATCH] 修改车辆传感器，未完成。
---
lib/src/main/cpp/driver_test.cpp | 28 ++--
lib/src/main/cpp/test_common/car_sensor.h | 46 ++++---
lib/src/main/cpp/test_common/car_sensor.cpp | 234 ++++++++++++++++++++++++++++++++------
lib/src/main/cpp/driver_test.h | 4
4 files changed, 237 insertions(+), 75 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 1429252..91672cc 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/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);
diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h
index 4ffde45..0fe5431 100644
--- a/lib/src/main/cpp/driver_test.h
+++ b/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);
diff --git a/lib/src/main/cpp/test_common/car_sensor.cpp b/lib/src/main/cpp/test_common/car_sensor.cpp
index 73afdc4..2a8e020 100644
--- a/lib/src/main/cpp/test_common/car_sensor.cpp
+++ b/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;
- } else {
- SensorConfig[i].currValue = 0;
+ level = 1;
}
- reportSensor[reportNum] = SensorConfig[i].funId;
- reportValue[reportNum] = (SensorConfig[i].validLvl == SensorConfig[i].currValue)?1:0;
- reportNum++;
+ if (level == SensorConfig[i].validLvl) {
+ SensorChanged(SensorConfig[i].funId, 1);
+ } else {
+ SensorChanged(SensorConfig[i].funId, 0);
+ }
}
}
- 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);
- if (sensor_id == SENSOR_ENGINE_RPM) {
- ret = engine_rpm;
- }
- pthread_mutex_unlock(&sonser_mutex);
-
- return ret;
+ 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;
+ }
+}
+
+
diff --git a/lib/src/main/cpp/test_common/car_sensor.h b/lib/src/main/cpp/test_common/car_sensor.h
index ef94afd..5d85ea7 100644
--- a/lib/src/main/cpp/test_common/car_sensor.h
+++ b/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
--
Gitblit v1.8.0