From 04a2ca4715769485ef180e81fd388a5db0562eee Mon Sep 17 00:00:00 2001
From: cfif <cham-zod@mail.ru>
Date: Wed, 4 Dec 2024 13:10:49 +0300
Subject: [PATCH] Init

---
 Inc/CarFlipDetection.h | 41 ++++++++++++++++++++++++++++++
 Src/CarFlipDetection.c | 57 ++++++++++++++++++++++++++++++++++++++++++
 modular.json           | 17 +++++++++++++
 3 files changed, 115 insertions(+)
 create mode 100644 Inc/CarFlipDetection.h
 create mode 100644 Src/CarFlipDetection.c
 create mode 100644 modular.json

diff --git a/Inc/CarFlipDetection.h b/Inc/CarFlipDetection.h
new file mode 100644
index 0000000..a21dd3f
--- /dev/null
+++ b/Inc/CarFlipDetection.h
@@ -0,0 +1,41 @@
+//
+// Created by xemon on 22.11.22.
+//
+
+#ifndef UVEOS_ON_NATION_CARFLIPDETECTION_H
+#define UVEOS_ON_NATION_CARFLIPDETECTION_H
+
+#include "vector.h"
+#include "stdbool.h"
+
+typedef struct {
+    ///пока этот флаг в нуле, событие с авари
+    bool enabled;
+    vector3 gravity;
+    struct {
+        float angleCos;
+        uint32_t antiFlightMs;
+    } thresholds;
+} tCarFlipDetectionSettings;
+
+typedef struct {
+    //watch value
+    struct {
+        float angleCos;
+        vector3 value;
+        uint32_t durationMs;
+    } current;
+
+    tCarFlipDetectionSettings *settings;
+
+    //detection result
+    bool flipDetected;
+} tCarFlipDetection;
+
+void CarFlipDetection_InitSettings(tCarFlipDetectionSettings *settings);
+
+void CarFlipDetection_Init(tCarFlipDetection *env, tCarFlipDetectionSettings *settings);
+
+void CarFlipDetection_ApplyCurrentAccel(tCarFlipDetection *env, vector3 currentValue);
+
+#endif //UVEOS_ON_NATION_CARFLIPDETECTION_H
diff --git a/Src/CarFlipDetection.c b/Src/CarFlipDetection.c
new file mode 100644
index 0000000..cb51288
--- /dev/null
+++ b/Src/CarFlipDetection.c
@@ -0,0 +1,57 @@
+//
+// Created by xemon on 22.11.22.
+//
+#include <SystemDelayInterface.h>
+#include "CarFlipDetection.h"
+
+#define NUM_0_8_SQR 0.64
+#define NUM_1_2_SQR 1.44
+#define FLIP_ANTI_FLAY_DELAY_TIME_MS 3000
+#define FLIP_MINIMAL_ACCEL_SQR_LENGTH NUM_0_8_SQR
+#define FLIP_MAXIMAL_ACCEL_SQR_LENGTH NUM_1_2_SQR
+
+
+void CarFlipDetection_InitSettings(tCarFlipDetectionSettings *settings) {
+    settings->enabled = false;
+    settings->gravity = (vector3) {0.f, 0.f, 0.f};
+    settings->thresholds.antiFlightMs = 3000;
+    settings->thresholds.angleCos = -1.f;
+
+}
+
+void CarFlipDetection_Init(tCarFlipDetection *env, tCarFlipDetectionSettings *settings) {
+    env->current.durationMs = 0;
+    env->flipDetected = false;
+    env->settings = settings;
+}
+
+bool CarFlipDetection_IsAccelLenSqrInBounds(float accelSqr) {
+    return (accelSqr > FLIP_MINIMAL_ACCEL_SQR_LENGTH) && (accelSqr < FLIP_MAXIMAL_ACCEL_SQR_LENGTH);
+}
+
+void CarFlipDetection_ApplyCurrentAccel(tCarFlipDetection *env, vector3 currentValue) {
+    env->current.value = currentValue;
+
+    if (CarFlipDetection_IsAccelLenSqrInBounds(vector3LenSquare(currentValue))) {
+
+        env->current.angleCos = vector3AngleCos(env->settings->gravity, currentValue);
+
+//		LOGF("ANGLE COS %i \n",(int)(angleCos*100))
+
+        if (!env->settings->enabled) {
+            env->flipDetected = false;
+            return;
+        }
+
+        if (env->current.angleCos < env->settings->thresholds.angleCos) {
+            if (env->current.durationMs == 0) {
+                env->current.durationMs = SystemGetMs() + env->settings->thresholds.antiFlightMs;
+            } else if (env->current.durationMs < SystemGetMs()) {
+                env->flipDetected = true;
+            }
+        } else {
+            env->current.durationMs = 0;
+            env->flipDetected = 0;
+        }
+    }
+}
\ No newline at end of file
diff --git a/modular.json b/modular.json
new file mode 100644
index 0000000..fe10ee6
--- /dev/null
+++ b/modular.json
@@ -0,0 +1,17 @@
+{
+  "dep": [
+    {
+      "type": "git",
+      "provider": "NAVIGATOR_UVEOS_NATION_TELIT",
+      "repo": "VectorMath"
+    }
+  ],
+  "cmake": {
+    "inc_dirs": [
+      "Inc"
+    ],
+    "srcs": [
+      "Src/**.c"
+    ]
+  }
+}
\ No newline at end of file