From cee29f807e43c0920ee0ad5b23f3ace238f3f245 Mon Sep 17 00:00:00 2001 From: cfif Date: Mon, 2 Jun 2025 13:26:41 +0300 Subject: [PATCH] Init --- Inc/CarCrashDetection.h | 49 ++++++++++++++++ Src/CarCrashDetection.c | 122 ++++++++++++++++++++++++++++++++++++++++ modular.json | 32 +++++++++++ 3 files changed, 203 insertions(+) create mode 100644 Inc/CarCrashDetection.h create mode 100644 Src/CarCrashDetection.c create mode 100644 modular.json diff --git a/Inc/CarCrashDetection.h b/Inc/CarCrashDetection.h new file mode 100644 index 0000000..8b10b3a --- /dev/null +++ b/Inc/CarCrashDetection.h @@ -0,0 +1,49 @@ +/* + * asi15.h + * + * Created on: Aug 25, 2020 + * Author: FICOM-IT LTD + * + */ + +#ifndef INC_ASI15_H_ +#define INC_ASI15_H_ + +#include +#include +#include "stdint.h" +#include "vector.h" +#include "UveosEmergencyEventInterface.h" +#include "AccelDataFlowInterface.h" +#include "EraGlonassAsi15.h" +#include "CarFlipDetection.h" + +typedef struct { + tAccelDataFlowInterface *accelData; + osMessageQueueId_t outEvents; + + tEraGlonassAsi15 asi15; + tCarFlipDetection flip; + + vector3 currentAcceleration; + bool crashEventGenerated; + float asi15This; + bool isStarted; +} tCarCrashDetection; + +void CarCrashDetection_Init( + tCarCrashDetection *env, + tAccelDataFlowInterface *accelData, + float *asi15threshold, + tCarFlipDetectionSettings *settings +); + +void CarCrashDetection_Processing(tCarCrashDetection *env, uint32_t timeout); + +bool CarCrashDetection_GetNext(tCarCrashDetection *env, tUveosEmergencyEvent *event, uint32_t timeout); + +tUveosEmergencyEventInterface CarCrashDetection_GetInterface(tCarCrashDetection *env); + +tUveosEmergencySimulationInterface CarCrashDetection_GetSimInterface(tCarCrashDetection *env); + +#endif /* INC_ASI15_H_ */ diff --git a/Src/CarCrashDetection.c b/Src/CarCrashDetection.c new file mode 100644 index 0000000..5e29196 --- /dev/null +++ b/Src/CarCrashDetection.c @@ -0,0 +1,122 @@ +#include +/* + * asi15.c + * + * Created on: Aug 25, 2020 + * Author: FICOM-IT LTD + */ + +#include "CarCrashDetection.h" +#include "SystemDelayInterface.h" + +#define COS_30_DEG 0.86602540 +#define FLIP_DEFAULT_THRESHOLD_ANGLE_COS COS_30_DEG + +//Use only from CCD task cause input accel concurrency +void CarCrashDetection_AutoFlipThreshold(tCarCrashDetection *env) { + int initialSteps = 10; + int initialStepsLeft = initialSteps; + + vector3 initialGravity = {0, 0, 0}; + + for (;;) { + if (AccelDataFlow_GetNext(env->accelData, &env->currentAcceleration, SystemWaitForever)) { + initialGravity = vector3Add( + initialGravity, + env->currentAcceleration + ); + --initialStepsLeft; + if (initialStepsLeft <= 0)break; + } + } + + initialGravity = vector3DivNum(initialGravity, (float) initialSteps); + +// CarFlipDetection_SetThreshold(&env->flip, initialGravity, FLIP_DEFAULT_THRESHOLD_ANGLE_COS); +} + + +void CarCrashDetection_Init( + tCarCrashDetection *env, + tAccelDataFlowInterface *accelData, + float *asi15threshold, + tCarFlipDetectionSettings *settings +) { + env->outEvents = osMessageQueueNew(128, sizeof(tUveosEmergencyEvent), NULL); + env->accelData = accelData; + EraGlonassAsi15_Init(&env->asi15, asi15threshold); + CarFlipDetection_Init(&env->flip, settings); + env->crashEventGenerated = false; + env->isStarted = false; +} + +void CarCrashDetection_Processing(tCarCrashDetection *env, uint32_t timeout) { + if(env->isStarted == false){ + env->isStarted = true; + } + if (AccelDataFlow_GetNext(env->accelData, &env->currentAcceleration, timeout)) { + EraGlonassAsi15_ApplyCurrentAccel(&env->asi15, env->currentAcceleration); + CarFlipDetection_ApplyCurrentAccel(&env->flip, env->currentAcceleration); + env->asi15This = env->asi15.asi15[env->asi15.counter]; + + if (!env->crashEventGenerated && (env->flip.flipDetected || env->asi15.impactDetected)) { + tUveosEmergencyEvent crash = { + .impactValue = EraGlonassAsi15_GetCurrentValue(&env->asi15), + .flipAngleCos = env->flip.current.angleCos, + .isFlip = env->flip.flipDetected, + .isImpact = env->asi15.impactDetected, + }; + + if (osMessageQueuePut(env->outEvents, &crash, 0, 1) == osOK) { + env->crashEventGenerated = true; + } + } + } +} + +bool CarCrashDetection_GetNext(tCarCrashDetection *env, tUveosEmergencyEvent *event, uint32_t timeout) { + return osMessageQueueGet(env->outEvents, event, 0, timeout) == osOK; +} + + +tUveosEmergencyEventInterface CarCrashDetection_GetInterface(tCarCrashDetection *env) { + tUveosEmergencyEventInterface result = { + .env = env, + .getNext = (UveosEmergencyGetNextCall) CarCrashDetection_GetNext, + }; + return result; +} + +static void CarCrashDetection_SimulateImpact(tCarCrashDetection *env, float asi15) { + tUveosEmergencyEvent crash = { + .impactValue = asi15, + .flipAngleCos = 0, + .isFlip = false, + .isImpact = true, + }; + + if (osMessageQueuePut(env->outEvents, &crash, 0, 1) == osOK) { + + } +} + +static void CarCrashDetection_SimulateFlip(tCarCrashDetection *env, float angle) { + tUveosEmergencyEvent crash = { + .impactValue = 0, + .flipAngleCos = angle, + .isFlip = true, + .isImpact = false, + }; + + if (osMessageQueuePut(env->outEvents, &crash, 0, 1) == osOK) { + } +} + +tUveosEmergencySimulationInterface CarCrashDetection_GetSimInterface(tCarCrashDetection *env) { + tUveosEmergencySimulationInterface result = { + .env = env, + .simulateImpact = (UveosEmergencySimulate) CarCrashDetection_SimulateImpact, + .simulateFlip= (UveosEmergencySimulate) CarCrashDetection_SimulateFlip, + }; + return result; +} diff --git a/modular.json b/modular.json new file mode 100644 index 0000000..94f7476 --- /dev/null +++ b/modular.json @@ -0,0 +1,32 @@ +{ + "dep": [ + { + "type": "git", + "provider": "Smart_Components_Aurus", + "repo": "AccelDataFlowInterface" + }, + { + "type": "git", + "provider": "Smart_Components_Aurus", + "repo": "EraGlonassAsi15" + }, + { + "type": "git", + "provider": "Smart_Components_Aurus", + "repo": "CarFlipDetection" + }, + { + "type": "git", + "provider": "Smart_Components_Aurus", + "repo": "UveosEmergencyEventInterface" + } + ], + "cmake": { + "inc_dirs": [ + "Inc" + ], + "srcs": [ + "Src/**.c" + ] + } +} \ No newline at end of file