110 lines
4.3 KiB
C
110 lines
4.3 KiB
C
//
|
|
// 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;
|
|
}
|
|
}
|