Init

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

Src/CarFlipDetection.c

@@ -0,0 +1,55 @@
+//
+// 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);
+
+        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 = false;
+            env->flipDetected = false;
+        }
+    }
+}

modular.json

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