482 lines
14 KiB
C
482 lines
14 KiB
C
#include "system.h"
|
|
#include "esp_log.h"
|
|
#include "nvs_flash.h"
|
|
#include "nvs.h"
|
|
#include "esp_timer.h"
|
|
#include <string.h>
|
|
|
|
static SystemState_t _systemState;
|
|
|
|
static EventGroupHandle_t _systemEvent;
|
|
static EventManager_t _eventManager;
|
|
|
|
static QueueHandle_t _nvsRequestQueue;
|
|
static const char* NVS_NAMESPACE = "bt_devices";
|
|
static const char* NVS_KEY_COUNT = "count";
|
|
|
|
static esp_err_t nvs_load_devices_internal(paired_device_t *devices, size_t *count);
|
|
static esp_err_t nvs_save_devices_internal(const paired_device_t *devices, size_t count);
|
|
|
|
void system_init(void)
|
|
{
|
|
_systemState.zeroAngle = 0.0f;
|
|
_systemState.primaryAxis = PRIMARY_AXIS;
|
|
_systemState.pairedDeviceCount = 0;
|
|
|
|
_systemEvent = xEventGroupCreate();
|
|
|
|
_eventManager.count = 0;
|
|
_eventManager.mutex = xSemaphoreCreateMutex();
|
|
|
|
system_initNvsService();
|
|
}
|
|
|
|
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);
|
|
}
|
|
}
|
|
|
|
void system_requestBtRefresh(void) {
|
|
ESP_LOGI("system", "BT Refresh requested");
|
|
system_notifyAll(EM_EVENT_BT_REFRESH);
|
|
}
|
|
|
|
void system_requestBtConnect(int device_index) {
|
|
xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
|
|
_systemState.btDeviceIndex = device_index;
|
|
xSemaphoreGive(_eventManager.mutex);
|
|
|
|
ESP_LOGI("system", "BT Connect requested for device %d", device_index);
|
|
system_notifyAll(EM_EVENT_BT_CONNECT);
|
|
}
|
|
|
|
int system_getBtDeviceIndex(void) {
|
|
int index;
|
|
xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
|
|
index = _systemState.btDeviceIndex;
|
|
xSemaphoreGive(_eventManager.mutex);
|
|
return index;
|
|
}
|
|
|
|
void system_requestVolumeUp(void) {
|
|
ESP_LOGI("system", "Volume Up requested");
|
|
system_notifyAll(EM_EVENT_VOLUME_UP);
|
|
}
|
|
|
|
void system_requestVolumeDown(void) {
|
|
ESP_LOGI("system", "Volume Down requested");
|
|
system_notifyAll(EM_EVENT_VOLUME_DOWN);
|
|
}
|
|
|
|
void system_initNvsService(void) {
|
|
_nvsRequestQueue = xQueueCreate(10, sizeof(nvs_request_t));
|
|
if (_nvsRequestQueue == NULL) {
|
|
ESP_LOGE("system", "Failed to create NVS request queue");
|
|
}
|
|
|
|
memset(_systemState.pairedDevices, 0, sizeof(_systemState.pairedDevices));
|
|
_systemState.pairedDeviceCount = 0;
|
|
|
|
paired_device_t devices[MAX_PAIRED_DEVICES];
|
|
size_t count = 0;
|
|
if (nvs_load_devices_internal(devices, &count) == ESP_OK) {
|
|
xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
|
|
memcpy(_systemState.pairedDevices, devices, count * sizeof(paired_device_t));
|
|
_systemState.pairedDeviceCount = count;
|
|
xSemaphoreGive(_eventManager.mutex);
|
|
ESP_LOGI("system", "Loaded %d paired devices from NVS", count);
|
|
}
|
|
}
|
|
|
|
static esp_err_t nvs_load_devices_internal(paired_device_t *devices, size_t *count) {
|
|
nvs_handle_t nvs_handle;
|
|
esp_err_t ret;
|
|
|
|
ret = nvs_open(NVS_NAMESPACE, NVS_READONLY, &nvs_handle);
|
|
if (ret != ESP_OK) {
|
|
ESP_LOGE("system", "Failed to open NVS namespace: %s", esp_err_to_name(ret));
|
|
*count = 0;
|
|
return ret;
|
|
}
|
|
|
|
size_t device_count = 0;
|
|
size_t required_size = sizeof(size_t);
|
|
ret = nvs_get_blob(nvs_handle, NVS_KEY_COUNT, &device_count, &required_size);
|
|
if (ret != ESP_OK) {
|
|
nvs_close(nvs_handle);
|
|
*count = 0;
|
|
return ESP_OK;
|
|
}
|
|
|
|
device_count = (device_count > MAX_PAIRED_DEVICES) ? MAX_PAIRED_DEVICES : device_count;
|
|
|
|
for (size_t i = 0; i < device_count; i++) {
|
|
char key[16];
|
|
snprintf(key, sizeof(key), "device_%zu", i);
|
|
required_size = sizeof(paired_device_t);
|
|
ret = nvs_get_blob(nvs_handle, key, &devices[i], &required_size);
|
|
if (ret != ESP_OK) {
|
|
ESP_LOGW("system", "Failed to load device %zu", i);
|
|
device_count = i;
|
|
break;
|
|
}
|
|
}
|
|
|
|
nvs_close(nvs_handle);
|
|
*count = device_count;
|
|
return ESP_OK;
|
|
}
|
|
|
|
static esp_err_t nvs_save_devices_internal(const paired_device_t *devices, size_t count) {
|
|
nvs_handle_t nvs_handle;
|
|
esp_err_t ret;
|
|
|
|
ret = nvs_open(NVS_NAMESPACE, NVS_READWRITE, &nvs_handle);
|
|
if (ret != ESP_OK) {
|
|
ESP_LOGE("system", "Failed to open NVS namespace for write: %s", esp_err_to_name(ret));
|
|
return ret;
|
|
}
|
|
|
|
ret = nvs_set_blob(nvs_handle, NVS_KEY_COUNT, &count, sizeof(size_t));
|
|
if (ret != ESP_OK) {
|
|
nvs_close(nvs_handle);
|
|
return ret;
|
|
}
|
|
|
|
for (size_t i = 0; i < count; i++) {
|
|
char key[16];
|
|
snprintf(key, sizeof(key), "device_%zu", i);
|
|
ret = nvs_set_blob(nvs_handle, key, &devices[i], sizeof(paired_device_t));
|
|
if (ret != ESP_OK) {
|
|
nvs_close(nvs_handle);
|
|
return ret;
|
|
}
|
|
}
|
|
|
|
ret = nvs_commit(nvs_handle);
|
|
nvs_close(nvs_handle);
|
|
return ret;
|
|
}
|
|
|
|
void system_processNvsRequests(void) {
|
|
nvs_request_t request;
|
|
|
|
while (xQueueReceive(_nvsRequestQueue, &request, 0) == pdTRUE) {
|
|
xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
|
|
switch (request.operation) {
|
|
case NVS_OP_SAVE_DEVICE:
|
|
request.result = nvs_save_devices_internal(_systemState.pairedDevices, _systemState.pairedDeviceCount);
|
|
break;
|
|
|
|
case NVS_OP_LOAD_DEVICES:
|
|
request.result = nvs_load_devices_internal(_systemState.pairedDevices, &_systemState.pairedDeviceCount);
|
|
break;
|
|
|
|
case NVS_OP_REMOVE_DEVICE: {
|
|
size_t removed_index = SIZE_MAX;
|
|
for (size_t i = 0; i < _systemState.pairedDeviceCount; i++) {
|
|
if (memcmp(_systemState.pairedDevices[i].bda, request.bda, ESP_BD_ADDR_LEN) == 0) {
|
|
removed_index = i;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (removed_index != SIZE_MAX) {
|
|
for (size_t i = removed_index; i < _systemState.pairedDeviceCount - 1; i++) {
|
|
_systemState.pairedDevices[i] = _systemState.pairedDevices[i + 1];
|
|
}
|
|
_systemState.pairedDeviceCount--;
|
|
request.result = nvs_save_devices_internal(_systemState.pairedDevices, _systemState.pairedDeviceCount);
|
|
} else {
|
|
request.result = ESP_ERR_NOT_FOUND;
|
|
}
|
|
break;
|
|
}
|
|
|
|
case NVS_OP_UPDATE_TIMESTAMP: {
|
|
for (size_t i = 0; i < _systemState.pairedDeviceCount; i++) {
|
|
if (memcmp(_systemState.pairedDevices[i].bda, request.bda, ESP_BD_ADDR_LEN) == 0) {
|
|
_systemState.pairedDevices[i].last_connected = (uint32_t)(esp_timer_get_time() / 1000000);
|
|
request.result = nvs_save_devices_internal(_systemState.pairedDevices, _systemState.pairedDeviceCount);
|
|
break;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
default:
|
|
request.result = ESP_ERR_INVALID_ARG;
|
|
break;
|
|
}
|
|
xSemaphoreGive(_eventManager.mutex);
|
|
|
|
request.response_ready = true;
|
|
if (request.requestor) {
|
|
xTaskNotify(request.requestor, (uint32_t)&request, eSetValueWithOverwrite);
|
|
}
|
|
}
|
|
}
|
|
|
|
esp_err_t system_savePairedDevice(const paired_device_t *device) {
|
|
if (!device) return ESP_ERR_INVALID_ARG;
|
|
|
|
xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
|
|
|
|
size_t existing_index = SIZE_MAX;
|
|
for (size_t i = 0; i < _systemState.pairedDeviceCount; i++) {
|
|
if (memcmp(_systemState.pairedDevices[i].bda, device->bda, ESP_BD_ADDR_LEN) == 0) {
|
|
existing_index = i;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (existing_index != SIZE_MAX) {
|
|
_systemState.pairedDevices[existing_index] = *device;
|
|
} else if (_systemState.pairedDeviceCount < MAX_PAIRED_DEVICES) {
|
|
_systemState.pairedDevices[_systemState.pairedDeviceCount++] = *device;
|
|
} else {
|
|
xSemaphoreGive(_eventManager.mutex);
|
|
return ESP_ERR_NO_MEM;
|
|
}
|
|
|
|
xSemaphoreGive(_eventManager.mutex);
|
|
|
|
nvs_request_t request = {
|
|
.operation = NVS_OP_SAVE_DEVICE,
|
|
.device = *device,
|
|
.requestor = xTaskGetCurrentTaskHandle()
|
|
};
|
|
|
|
if (xQueueSend(_nvsRequestQueue, &request, pdMS_TO_TICKS(NVS_TIMEOUT_MS)) == pdTRUE) {
|
|
uint32_t notification;
|
|
if (xTaskNotifyWait(0, UINT32_MAX, ¬ification, pdMS_TO_TICKS(NVS_TIMEOUT_MS)) == pdTRUE) {
|
|
nvs_request_t *response = (nvs_request_t*)notification;
|
|
return response->result;
|
|
}
|
|
}
|
|
return ESP_ERR_TIMEOUT;
|
|
}
|
|
|
|
esp_err_t system_loadPairedDevices(paired_device_t *devices, size_t *count) {
|
|
if (!devices || !count) return ESP_ERR_INVALID_ARG;
|
|
|
|
xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
|
|
size_t copy_count = (*count < _systemState.pairedDeviceCount) ? *count : _systemState.pairedDeviceCount;
|
|
memcpy(devices, _systemState.pairedDevices, copy_count * sizeof(paired_device_t));
|
|
*count = copy_count;
|
|
xSemaphoreGive(_eventManager.mutex);
|
|
|
|
return ESP_OK;
|
|
}
|
|
|
|
esp_err_t system_removePairedDevice(esp_bd_addr_t bda) {
|
|
nvs_request_t request = {
|
|
.operation = NVS_OP_REMOVE_DEVICE,
|
|
.requestor = xTaskGetCurrentTaskHandle()
|
|
};
|
|
memcpy(request.bda, bda, ESP_BD_ADDR_LEN);
|
|
|
|
if (xQueueSend(_nvsRequestQueue, &request, pdMS_TO_TICKS(NVS_TIMEOUT_MS)) == pdTRUE) {
|
|
uint32_t notification;
|
|
if (xTaskNotifyWait(0, UINT32_MAX, ¬ification, pdMS_TO_TICKS(NVS_TIMEOUT_MS)) == pdTRUE) {
|
|
nvs_request_t *response = (nvs_request_t*)notification;
|
|
return response->result;
|
|
}
|
|
}
|
|
return ESP_ERR_TIMEOUT;
|
|
}
|
|
|
|
bool system_isDeviceKnown(esp_bd_addr_t bda) {
|
|
bool known = false;
|
|
xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
|
|
for (size_t i = 0; i < _systemState.pairedDeviceCount; i++) {
|
|
if (memcmp(_systemState.pairedDevices[i].bda, bda, ESP_BD_ADDR_LEN) == 0) {
|
|
known = true;
|
|
break;
|
|
}
|
|
}
|
|
xSemaphoreGive(_eventManager.mutex);
|
|
return known;
|
|
}
|
|
|
|
esp_err_t system_getKnownDeviceCount(size_t *count) {
|
|
if (!count) return ESP_ERR_INVALID_ARG;
|
|
|
|
xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
|
|
*count = _systemState.pairedDeviceCount;
|
|
xSemaphoreGive(_eventManager.mutex);
|
|
return ESP_OK;
|
|
}
|
|
|
|
esp_err_t system_updateConnectionTimestamp(esp_bd_addr_t bda) {
|
|
nvs_request_t request = {
|
|
.operation = NVS_OP_UPDATE_TIMESTAMP,
|
|
.requestor = xTaskGetCurrentTaskHandle()
|
|
};
|
|
memcpy(request.bda, bda, ESP_BD_ADDR_LEN);
|
|
|
|
if (xQueueSend(_nvsRequestQueue, &request, pdMS_TO_TICKS(NVS_TIMEOUT_MS)) == pdTRUE) {
|
|
uint32_t notification;
|
|
if (xTaskNotifyWait(0, UINT32_MAX, ¬ification, pdMS_TO_TICKS(NVS_TIMEOUT_MS)) == pdTRUE) {
|
|
nvs_request_t *response = (nvs_request_t*)notification;
|
|
return response->result;
|
|
}
|
|
}
|
|
return ESP_ERR_TIMEOUT;
|
|
}
|
|
|
|
const paired_device_t* system_getPairedDevices(size_t *count) {
|
|
if (!count) return NULL;
|
|
|
|
xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
|
|
*count = _systemState.pairedDeviceCount;
|
|
xSemaphoreGive(_eventManager.mutex);
|
|
|
|
return (const paired_device_t*)_systemState.pairedDevices;
|
|
} |