Initial commit

IMU and bubble level working

main/system.c

@@ -0,0 +1,104 @@
+#include "system.h"
+#include "esp_log.h"
+
+static SystemState_t _systemState;
+
+static EventGroupHandle_t _systemEvent;
+static EventManager_t _eventManager;
+
+void system_init(void)
+{
+	EventGroupHandle_t evt = xEventGroupCreate();
+
+	_eventManager.count = 0;
+    _eventManager.mutex = xSemaphoreCreateMutex();
+}
+
+void system_setImuData(ImuData_t imu)
+{
+	xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
+
+	_systemState.imu = imu;
+
+	xSemaphoreGive(_eventManager.mutex);
+
+    //ESP_LOGI("g", "New IMU Data: %.2f", xy);
+
+	system_notifyAll(EM_EVENT_NEW_DATA);
+}
+
+ImuData_t system_getImuData(void)
+{
+	ImuData_t imu;
+	xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
+
+    imu = _systemState.imu;
+
+	xSemaphoreGive(_eventManager.mutex);
+
+	return imu;
+}
+
+void system_waitForEvent(void)
+{
+}
+
+SystemState_t *system_getState(void)
+{
+	return &_systemState;
+}
+
+
+BaseType_t system_subscribe(TaskHandle_t task) {
+
+	EventManager_t *em = &_eventManager;
+
+    ESP_LOGI("g", "Subscribe: %p", task);
+    if (xSemaphoreTake(em->mutex, portMAX_DELAY) == pdFALSE) {
+        return pdFAIL;
+    }
+    if (em->count < EM_MAX_SUBSCRIBERS) {
+        // avoid duplicates?
+        ESP_LOGI("g", "PASS");
+        em->subscribers[em->count++] = task;
+        xSemaphoreGive(em->mutex);
+        return pdPASS;
+    }
+    xSemaphoreGive(em->mutex);
+    return pdFAIL; // full
+}
+
+BaseType_t system_unsubscribe(TaskHandle_t task) {
+
+	EventManager_t *em = &_eventManager;
+
+    BaseType_t removed = pdFAIL;
+    if (xSemaphoreTake(em->mutex, portMAX_DELAY) == pdTRUE) {
+        for (size_t i = 0; i < em->count; ++i) {
+            if (em->subscribers[i] == task) {
+                // shift down
+                for (size_t j = i; j + 1 < em->count; ++j)
+                    em->subscribers[j] = em->subscribers[j+1];
+                em->count--;
+                removed = pdPASS;
+                break;
+            }
+        }
+        xSemaphoreGive(em->mutex);
+    }
+    return removed;
+}
+
+void system_notifyAll(uint32_t eventBits) {
+	EventManager_t *em = &_eventManager;
+    if (xSemaphoreTake(em->mutex, portMAX_DELAY) == pdTRUE) {
+        for (size_t i = 0; i < em->count; ++i) {
+            // set the bits in each task's notification value
+            ESP_LOGI("g", "Notify: %p", em->subscribers[i]);
+            xTaskNotify(em->subscribers[i],
+                        eventBits,
+                        eSetBits);
+        }
+        xSemaphoreGive(em->mutex);
+    }
+}