Init

Inc/EraGlonassAsi15.h

@@ -0,0 +1,37 @@
+//
+// 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"
+
+
+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;
+} 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

Src/EraGlonassAsi15.c

@@ -0,0 +1,109 @@
+//
+// 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;
+    }
+
+    if (env->asi15[env->counter] > *env->threshold) {
+        env->impactDetected = true;
+    }
+}

modular.json

@@ -0,0 +1,17 @@
+{
+  "dep": [
+    {
+      "type": "git",
+      "provider": "NAVIGATOR_UVEOS_NATION_TELIT",
+      "repo": "VectorMath"
+    }
+  ],
+  "cmake": {
+    "inc_dirs": [
+      "Inc"
+    ],
+    "srcs": [
+      "Src/**.c"
+    ]
+  }
+}