Audio working

main/system.c

@@ -1,104 +1,169 @@
-#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);
-    }
+#include "system.h"
+#include "esp_log.h"
+
+static SystemState_t _systemState;
+
+static EventGroupHandle_t _systemEvent;
+static EventManager_t _eventManager;
+
+void system_init(void)
+{
+    _systemState.zeroAngle = 0.0f;
+    _systemState.primaryAxis = PRIMARY_AXIS;
+
+	EventGroupHandle_t evt = xEventGroupCreate();
+
+	_eventManager.count = 0;
+    _eventManager.mutex = xSemaphoreCreateMutex();
+}
+
+int system_getPrimaryAxis(void)
+{
+    int axis;
+
+    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
+    axis = _systemState.primaryAxis;
+    xSemaphoreGive(_eventManager.mutex);
+
+    return axis;
+}
+
+float system_getAngle(void)
+{
+    float angle;
+
+    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
+
+    
+    angle = _systemState.imu.raw[_systemState.primaryAxis] - _systemState.zeroAngle;
+
+
+	xSemaphoreGive(_eventManager.mutex);
+
+    return angle;    
+}
+
+void system_setZeroAngle(void)
+{
+    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
+
+    
+	_systemState.zeroAngle = _systemState.imu.raw[_systemState.primaryAxis];
+
+    ESP_LOGI("system", "Zero: %.1f", _systemState.zeroAngle);
+
+	xSemaphoreGive(_eventManager.mutex);
+}
+
+void system_clearZeroAngle(void)
+{
+    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
+
+	_systemState.zeroAngle = 0.0f;
+
+    ESP_LOGI("system", "Zero: %.1f", _systemState.zeroAngle);
+
+	xSemaphoreGive(_eventManager.mutex);
+}
+
+float system_getZeroAngle(void)
+{
+    float angle;
+
+    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
+
+	angle = _systemState.zeroAngle;
+
+	xSemaphoreGive(_eventManager.mutex);
+
+    return angle;
+}
+
+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);
+    }
 }