commit 2a983fb0ace9efb1b8f225b1ad99be3f8d29e337 Author: cfif Date: Mon Jun 2 13:26:41 2025 +0300 Init diff --git a/Inc/EraGlonassAsi15.h b/Inc/EraGlonassAsi15.h new file mode 100644 index 0000000..10fa4d5 --- /dev/null +++ b/Inc/EraGlonassAsi15.h @@ -0,0 +1,38 @@ +// +// Created by xemon on 22.11.22. +// + +#ifndef UVEOS_ON_NATION_ERAGLONASSASI15_H +#define UVEOS_ON_NATION_ERAGLONASSASI15_H + +#include "vector.h" +#include "stdbool.h" +#include "LoggerInterface.h" + +typedef struct { + int count; + vector3 elements[150]; +} tAsi15AccelEventType; + + +typedef struct { + vector3 aT[150]; + float asi[150]; + float asi15[150]; + tAsi15AccelEventType tLIS331HHFirstEventArray; + tAsi15AccelEventType tLIS331HHSecondEventArray; + + uint32_t counter; + + float *threshold; + bool impactDetected; + float isDetectValue; +} tEraGlonassAsi15; + +void EraGlonassAsi15_Init(tEraGlonassAsi15 *env, float *threshold); + +float EraGlonassAsi15_GetCurrentValue(tEraGlonassAsi15 *env); + +void EraGlonassAsi15_ApplyCurrentAccel(tEraGlonassAsi15 *env, vector3 currentAcceleration); + +#endif //UVEOS_ON_NATION_ERAGLONASSASI15_H diff --git a/Src/EraGlonassAsi15.c b/Src/EraGlonassAsi15.c new file mode 100644 index 0000000..603b0fd --- /dev/null +++ b/Src/EraGlonassAsi15.c @@ -0,0 +1,111 @@ +// +// Created by xemon on 22.11.22. +// +#include "EraGlonassAsi15.h" +#include "math.h" + +#define ASI_15_DEFAULT_THRESHOLD 0.9 + +#define AX 12 +#define AY 9 +#define AZ 10 +#define DELTA 50 + + + +static float fA_t(float fIntegralValue) { + return fIntegralValue / DELTA; +} + +static float fASI(float fAX_tValue, float fAY_tValue, float fAZ_tValue) { + float _x = fA_t(fAX_tValue) / AX; + float _y = fA_t(fAY_tValue) / AY; + float _z = fA_t(fAZ_tValue) / AZ; + return (float) sqrt((double) ((_x * _x) + (_y * _y) + (_z * _z))); +} + +void EraGlonassAsi15_Init(tEraGlonassAsi15 *env, float *threshold) { + *env = (tEraGlonassAsi15) { + .asi = {0}, + .asi15 = {0}, + .counter = 0, + }; + + env->tLIS331HHFirstEventArray.count = 0; + env->tLIS331HHSecondEventArray.count = 75; + env->threshold = threshold; +} + +float EraGlonassAsi15_GetCurrentValue(tEraGlonassAsi15 *env) { + return env->asi15[env->counter]; +} + +void EraGlonassAsi15_ApplyCurrentAccel(tEraGlonassAsi15 *env, vector3 currentAcceleration) { + env->tLIS331HHFirstEventArray.elements[env->tLIS331HHFirstEventArray.count] = currentAcceleration; + env->tLIS331HHFirstEventArray.count++; + if (env->tLIS331HHFirstEventArray.count > 149) { + env->tLIS331HHFirstEventArray.count = 0; + } + env->tLIS331HHSecondEventArray.elements[env->tLIS331HHSecondEventArray.count] = currentAcceleration; + env->tLIS331HHSecondEventArray.count++; + if (env->tLIS331HHSecondEventArray.count > 149) { + env->tLIS331HHSecondEventArray.count = 0; + } + + if ((env->counter + 49) < 150) { + env->aT[env->counter].x = (env->tLIS331HHFirstEventArray.elements[env->counter].x + + env->tLIS331HHFirstEventArray.elements[env->counter + 49].x) / 2; + env->aT[env->counter].y = (env->tLIS331HHFirstEventArray.elements[env->counter].y + + env->tLIS331HHFirstEventArray.elements[env->counter + 49].y) / 2; + env->aT[env->counter].z = (env->tLIS331HHFirstEventArray.elements[env->counter].z + + env->tLIS331HHFirstEventArray.elements[env->counter + 49].z) / 2; + } else { + env->aT[env->counter].x = (env->tLIS331HHFirstEventArray.elements[env->counter].x + + env->tLIS331HHSecondEventArray.elements[env->counter - 75 + 49].x) / 2; + env->aT[env->counter].y = (env->tLIS331HHFirstEventArray.elements[env->counter].y + + env->tLIS331HHSecondEventArray.elements[env->counter - 75 + 49].y) / 2; + env->aT[env->counter].z = (env->tLIS331HHFirstEventArray.elements[env->counter].z + + env->tLIS331HHSecondEventArray.elements[env->counter - 75 + 49].z) / 2; + } + + for (int i = 1; i < 49; i++) { + if ((env->counter + i) < 150) { + env->aT[env->counter].x += env->tLIS331HHFirstEventArray.elements[env->counter + i].x; + env->aT[env->counter].y += env->tLIS331HHFirstEventArray.elements[env->counter + i].y; + env->aT[env->counter].z += env->tLIS331HHFirstEventArray.elements[env->counter + i].z; + } else { + env->aT[env->counter].x += env->tLIS331HHSecondEventArray.elements[env->counter - 75 + i].x; + env->aT[env->counter].y += env->tLIS331HHSecondEventArray.elements[env->counter - 75 + i].y; + env->aT[env->counter].z += env->tLIS331HHSecondEventArray.elements[env->counter - 75 + i].z; + } + } + + env->asi[env->counter] = fASI(env->aT[env->counter].x, env->aT[env->counter].y, env->aT[env->counter].z); + + if ((env->counter + 14) < 150) { + env->asi15[env->counter] = (env->asi[env->counter] + + env->asi[env->counter + 14]) / 2; + } else { + env->asi15[env->counter] = (env->asi[env->counter] + + env->asi[env->counter + 14 - 150]) / 2; + } + for (int i = 1; i < 14; i++) { + if ((env->counter + i) < 150) { + env->asi15[env->counter] += env->asi[env->counter + i]; + } else { + env->asi15[env->counter] += env->asi[i]; + } + } + env->asi15[env->counter] = env->asi15[env->counter] / 15; + + env->counter++; + if (env->counter >= 150) { + env->counter = 0; + } + + float asi15 = env->asi15[env->counter]; + if (asi15 > *env->threshold) { + env->isDetectValue = asi15; + env->impactDetected = true; + } +} diff --git a/modular.json b/modular.json new file mode 100644 index 0000000..b953627 --- /dev/null +++ b/modular.json @@ -0,0 +1,17 @@ +{ + "dep": [ + { + "type": "git", + "provider": "Smart_Components_Aurus", + "repo": "VectorMath" + } + ], + "cmake": { + "inc_dirs": [ + "Inc" + ], + "srcs": [ + "Src/**.c" + ] + } +} \ No newline at end of file