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

---
 Inc/Accel.h  | 38 +++++++++++++++++++++++++++++
 Src/Accel.c  | 68 ++++++++++++++++++++++++++++++++++++++++++++++++++++
 modular.json | 23 ++++++++++++++++++
 3 files changed, 129 insertions(+)
 create mode 100644 Inc/Accel.h
 create mode 100644 Src/Accel.c
 create mode 100644 modular.json

diff --git a/Inc/Accel.h b/Inc/Accel.h
new file mode 100644
index 0000000..9b074d1
--- /dev/null
+++ b/Inc/Accel.h
@@ -0,0 +1,38 @@
+//
+// Created by xemon on 23.11.22.
+//
+
+#ifndef UVEOS_ON_NATION_ACCEL_H
+#define UVEOS_ON_NATION_ACCEL_H
+
+#include <cmsis_os.h>
+#include "Accel_QMA6100P.h"
+#include "AccelDataFlowInterface.h"
+
+typedef struct {
+    tAccelQma6100P accel;
+    tAccelDataFlowInterface accelDataFlow;
+
+    osMessageQueueId_t dataQueue;
+    bool errorDataFlag;
+    vector3 current;
+
+
+    struct {
+        osThreadId_t id;
+        uint32_t stack[256];
+        StaticTask_t controlBlock;
+        osThreadAttr_t attr;
+    } thread;
+
+} tAccel;
+
+void Accel_Init(tAccel *env, tSpiPortIO *accelIO);
+
+void Accel_StartDevice(tAccel *env);
+
+void Accel_StartThread(tAccel *env);
+
+bool Accel_WaitReady(tAccel *env, uint32_t timeout);
+
+#endif //UVEOS_ON_NATION_ACCEL_H
diff --git a/Src/Accel.c b/Src/Accel.c
new file mode 100644
index 0000000..dbe680f
--- /dev/null
+++ b/Src/Accel.c
@@ -0,0 +1,68 @@
+#include <alloca.h>
+//
+// Created by xemon on 23.11.22.
+//
+
+#include <SystemDelayInterface.h>
+#include "Accel.h"
+#include "CmsisRtosThreadUtils.h"
+
+bool Accel_GetNext(tAccel *env, vector3 *value, uint32_t timeout) {
+    return osMessageQueueGet(env->dataQueue, value, 0, timeout) == osOK;
+}
+
+void Accel_Clear(tAccel *env) {
+    osMessageQueueReset(env->dataQueue);
+}
+
+void Accel_Init(tAccel *env, tSpiPortIO *accelIO) {
+
+    AccelQma6100P_Init(&env->accel, accelIO);
+    InitThreadAtrStatic(&env->thread.attr, "Accel", env->thread.controlBlock, env->thread.stack, osPriorityNormal);
+    env->thread.id = 0;
+    env->dataQueue = osMessageQueueNew(128, sizeof(vector3), NULL);
+
+    env->accelDataFlow = (tAccelDataFlowInterface) {
+            .env = env,
+            .getNext = (accelDataGetNextCall) Accel_GetNext,
+            .clear = (accelDataClearCall) Accel_Clear,
+    };
+}
+
+
+void Accel_StartDevice(tAccel *env) {
+    AccelQma6100P_ConfigureDefaults(&env->accel);
+}
+
+bool Accel_WaitReady(tAccel *env, uint32_t timeout) {
+
+    uint32_t time_end = SystemGetMs() + timeout;
+
+    float len;
+    do {
+        len = vector3Len(env->current);
+    } while ((time_end > SystemGetMs()) && len < 0.1f);
+
+    return len > 0.1f;
+}
+
+
+static _Noreturn void Accel_Thread(tAccel *env) {
+    //todo нужно переделать под таймер и семафор
+    for (;;) {
+        if (AccelQma6100P_Read(&env->accel, &env->current)) {
+            osMessageQueuePut(env->dataQueue, &env->current, 0, 0);
+            env->errorDataFlag = true;
+        } else {
+            env->errorDataFlag = false;
+        }
+        SystemDelayMs(1);
+    }
+}
+
+void Accel_StartThread(tAccel *env) {
+    if (!env->thread.id) {
+        env->thread.id = osThreadNew((osThreadFunc_t) (Accel_Thread), (void *) (env), &env->thread.attr);
+    }
+}
+
diff --git a/modular.json b/modular.json
new file mode 100644
index 0000000..096cbc7
--- /dev/null
+++ b/modular.json
@@ -0,0 +1,23 @@
+{
+  "dep": [
+    {
+      "type": "git",
+      "provider": "NAVIGATOR_UVEOS_NATION_TELIT",
+      "repo": "Accel_QMA6100P"
+    },
+    {
+      "type": "git",
+      "provider": "NAVIGATOR_UVEOS_NATION_TELIT",
+      "repo": "AccelDataFlowInterface"
+    }
+  ],
+  "cmake": {
+    "inc_dirs": [
+      "Inc/"
+    ],
+    "srcs": [
+      "Src/**.c",
+      "Src_InitDefaults/**.c"
+    ]
+  }
+}
\ No newline at end of file