Init

Inc/CarCrashDetection.h

@@ -0,0 +1,47 @@
+/*
+ * asi15.h
+ *
+ *  Created on: Aug 25, 2020
+ *      Author: FICOM-IT LTD
+ *
+ */
+
+#ifndef INC_ASI15_H_
+#define INC_ASI15_H_
+
+#include <stdbool.h>
+#include <cmsis_os.h>
+#include "stdint.h"
+#include "vector.h"
+#include "UveosEmergencyEventInterface.h"
+#include "AccelDataFlowInterface.h"
+#include "EraGlonassAsi15.h"
+#include "CarFlipDetection.h"
+
+typedef struct {
+    tAccelDataFlowInterface *accelData;
+    osMessageQueueId_t outEvents;
+
+    tEraGlonassAsi15 asi15;
+    tCarFlipDetection flip;
+
+    vector3 currentAcceleration;
+    bool crashEventGenerated;
+} tCarCrashDetection;
+
+void CarCrashDetection_Init(
+        tCarCrashDetection *env,
+        tAccelDataFlowInterface *accelData,
+        float *asi15threshold,
+        tCarFlipDetectionSettings *settings
+);
+
+void CarCrashDetection_Processing(tCarCrashDetection *env, uint32_t timeout);
+
+bool CarCrashDetection_GetNext(tCarCrashDetection *env, tUveosEmergencyEvent *event, uint32_t timeout);
+
+tUveosEmergencyEventInterface CarCrashDetection_GetInterface(tCarCrashDetection *env);
+
+tUveosEmergencySimulationInterface CarCrashDetection_GetSimInterface(tCarCrashDetection *env);
+
+#endif /* INC_ASI15_H_ */

Src/CarCrashDetection.c

@@ -0,0 +1,121 @@
+#include <sched.h>
+/*
+ * asi15.c
+ *
+ *  Created on: Aug 25, 2020
+ *      Author: FICOM-IT LTD
+ */
+
+#include "CarCrashDetection.h"
+#include "SystemDelayInterface.h"
+
+#define COS_30_DEG 0.86602540
+#define FLIP_DEFAULT_THRESHOLD_ANGLE_COS COS_30_DEG
+
+//Use only from CCD task cause input accel concurrency
+void CarCrashDetection_AutoFlipThreshold(tCarCrashDetection *env) {
+    int initialSteps = 10;
+    int initialStepsLeft = initialSteps;
+
+    vector3 initialGravity = {0, 0, 0};
+
+    for (;;) {
+        if (AccelDataFlow_GetNext(env->accelData, &env->currentAcceleration, SystemWaitForever)) {
+            initialGravity = vector3Add(
+                    initialGravity,
+                    env->currentAcceleration
+            );
+            --initialStepsLeft;
+            if (initialStepsLeft <= 0)break;
+        }
+    }
+
+    initialGravity = vector3DivNum(initialGravity, (float) initialSteps);
+
+//    CarFlipDetection_SetThreshold(&env->flip, initialGravity, FLIP_DEFAULT_THRESHOLD_ANGLE_COS);
+}
+
+
+void CarCrashDetection_Init(
+        tCarCrashDetection *env,
+        tAccelDataFlowInterface *accelData,
+        float *asi15threshold,
+        tCarFlipDetectionSettings *settings
+) {
+    env->outEvents = osMessageQueueNew(128, sizeof(tUveosEmergencyEvent), NULL);
+    env->accelData = accelData;
+    EraGlonassAsi15_Init(&env->asi15, asi15threshold);
+    CarFlipDetection_Init(&env->flip, settings);
+    env->crashEventGenerated = false;
+}
+
+//void CarCrashDetection_AutoCalibration(tCarCrashDetection *env) {
+//    CarCrashDetectionAutoFlipThreshold(env);
+//}
+
+void CarCrashDetection_Processing(tCarCrashDetection *env, uint32_t timeout) {
+    if (AccelDataFlow_GetNext(env->accelData, &env->currentAcceleration, timeout)) {
+        EraGlonassAsi15_ApplyCurrentAccel(&env->asi15, env->currentAcceleration);
+        CarFlipDetection_ApplyCurrentAccel(&env->flip, env->currentAcceleration);
+
+        if (!env->crashEventGenerated && (env->flip.flipDetected || env->asi15.impactDetected)) {
+            tUveosEmergencyEvent crash = {
+                    .impactValue = EraGlonassAsi15_GetCurrentValue(&env->asi15),
+                    .flipAngleCos = env->flip.current.angleCos,
+                    .isFlip = env->flip.flipDetected,
+                    .isImpact = env->asi15.impactDetected,
+            };
+
+            if (osMessageQueuePut(env->outEvents, &crash, 0, 1) == osOK) {
+                env->crashEventGenerated = true;
+            }
+        }
+    }
+}
+
+bool CarCrashDetection_GetNext(tCarCrashDetection *env, tUveosEmergencyEvent *event, uint32_t timeout) {
+    return osMessageQueueGet(env->outEvents, event, 0, timeout) == osOK;
+}
+
+
+tUveosEmergencyEventInterface CarCrashDetection_GetInterface(tCarCrashDetection *env) {
+    tUveosEmergencyEventInterface result = {
+            .env = env,
+            .getNext = (UveosEmergencyGetNextCall) CarCrashDetection_GetNext,
+    };
+    return result;
+}
+
+static void CarCrashDetection_SimulateImpact(tCarCrashDetection *env, float asi15) {
+    tUveosEmergencyEvent crash = {
+            .impactValue = asi15,
+            .flipAngleCos = 0,
+            .isFlip = false,
+            .isImpact = true,
+    };
+
+    if (osMessageQueuePut(env->outEvents, &crash, 0, 1) == osOK) {
+
+    }
+}
+
+static void CarCrashDetection_SimulateFlip(tCarCrashDetection *env, float angle) {
+    tUveosEmergencyEvent crash = {
+            .impactValue = 0,
+            .flipAngleCos = angle,
+            .isFlip = true,
+            .isImpact = false,
+    };
+
+    if (osMessageQueuePut(env->outEvents, &crash, 0, 1) == osOK) {
+    }
+}
+
+tUveosEmergencySimulationInterface CarCrashDetection_GetSimInterface(tCarCrashDetection *env) {
+    tUveosEmergencySimulationInterface result = {
+            .env = env,
+            .simulateImpact = (UveosEmergencySimulate) CarCrashDetection_SimulateImpact,
+            .simulateFlip= (UveosEmergencySimulate) CarCrashDetection_SimulateFlip,
+    };
+    return result;
+}

modular.json

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