diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..3726424 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,9 @@ +[submodule "boards/components/BME280/BME280_SensorAPI"] + path = boards/components/BME280/BME280_SensorAPI + url = https://github.com/boschsensortec/BME280_SensorAPI.git +[submodule "boards/components/LSM6DSO/lsm6dso-pid"] + path = boards/components/LSM6DSO/lsm6dso-pid + url = https://github.com/STMicroelectronics/lsm6dso-pid.git +[submodule "boards/components/ring_buffer/ultra-low-latency-ring-buffer"] + path = boards/components/ring_buffer/ultra-low-latency-ring-buffer + url = https://github.com/cale-cmd/ultra-low-latency-ring-buffer.git diff --git a/boards/CMakeLists.txt b/boards/CMakeLists.txt index fb93631..8b5aae9 100755 --- a/boards/CMakeLists.txt +++ b/boards/CMakeLists.txt @@ -1,6 +1,13 @@ # The following five lines of boilerplate have to be in your project's # CMakeLists in this exact order for cmake to work correctly -cmake_minimum_required(VERSION 3.16) +cmake_minimum_required(VERSION 3.22) + +find_package(Git QUIET) +execute_process(COMMAND ${GIT_EXECUTABLE} submodule update --init + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + RESULT_VARIABLE GIT_SUBMODULE_RESULT) + +set(CMAKE_CXX_STANDARD 23) set(IDF_TOOLCHAIN clang) diff --git a/boards/components/BME280/BME280_SensorAPI b/boards/components/BME280/BME280_SensorAPI new file mode 160000 index 0000000..c90d419 --- /dev/null +++ b/boards/components/BME280/BME280_SensorAPI @@ -0,0 +1 @@ +Subproject commit c90d419492e26dd95586598a794e65eb2760753a diff --git a/boards/components/BME280/CMakeLists.txt b/boards/components/BME280/CMakeLists.txt new file mode 100644 index 0000000..8dd9f08 --- /dev/null +++ b/boards/components/BME280/CMakeLists.txt @@ -0,0 +1,2 @@ +idf_component_register(SRCS "BME280_SensorAPI/bme280.c" + INCLUDE_DIRS ".") diff --git a/boards/components/LSM6DSO/CMakeLists.txt b/boards/components/LSM6DSO/CMakeLists.txt new file mode 100644 index 0000000..8d6dcee --- /dev/null +++ b/boards/components/LSM6DSO/CMakeLists.txt @@ -0,0 +1,2 @@ +idf_component_register(SRCS "lsm6dso-pid/lsm6dso_reg.c" + INCLUDE_DIRS ".") diff --git a/boards/components/LSM6DSO/lsm6dso-pid b/boards/components/LSM6DSO/lsm6dso-pid new file mode 160000 index 0000000..f3720e9 --- /dev/null +++ b/boards/components/LSM6DSO/lsm6dso-pid @@ -0,0 +1 @@ +Subproject commit f3720e98e7e02b0cec5685c1bee56dae67934abe diff --git a/boards/components/ring_buffer/CMakeLists.txt b/boards/components/ring_buffer/CMakeLists.txt new file mode 100644 index 0000000..311a0dc --- /dev/null +++ b/boards/components/ring_buffer/CMakeLists.txt @@ -0,0 +1 @@ +idf_component_register(INCLUDE_DIRS ".") diff --git a/boards/components/ring_buffer/ring_buffer.h b/boards/components/ring_buffer/ring_buffer.h new file mode 100644 index 0000000..8d2c29c --- /dev/null +++ b/boards/components/ring_buffer/ring_buffer.h @@ -0,0 +1 @@ +#include "ultra-low-latency-ring-buffer/src/ring_buffer.cpp" diff --git a/boards/components/ring_buffer/ultra-low-latency-ring-buffer b/boards/components/ring_buffer/ultra-low-latency-ring-buffer new file mode 160000 index 0000000..2655909 --- /dev/null +++ b/boards/components/ring_buffer/ultra-low-latency-ring-buffer @@ -0,0 +1 @@ +Subproject commit 2655909056bf244ef58be4c5662b78d03dd8f7ae diff --git a/boards/main/CMakeLists.txt b/boards/main/CMakeLists.txt index 67233cd..a366ec2 100755 --- a/boards/main/CMakeLists.txt +++ b/boards/main/CMakeLists.txt @@ -1,4 +1,5 @@ file(GLOB_RECURSE MAIN_SOURCES "*.cpp") idf_component_register(SRCS ${MAIN_SOURCES} - INCLUDE_DIRS ".") + INCLUDE_DIRS "." + REQUIRES driver esp_timer BME280 LSM6DSO ring_buffer) diff --git a/boards/main/Kconfig.projbuild b/boards/main/Kconfig.projbuild new file mode 100644 index 0000000..99c00dc --- /dev/null +++ b/boards/main/Kconfig.projbuild @@ -0,0 +1,23 @@ +menu "Rocket board configuration" + orsource "$IDF_PATH/examples/common_components/env_caps/$IDF_TARGET/Kconfig.env_caps" + + config I2C_SDA_GPIO + int "I2C SDA GPIO number" + range ENV_GPIO_RANGE_MIN ENV_GPIO_OUT_RANGE_MAX + default 7 + + config I2C_SCL_GPIO + int "I2C SCL GPIO number" + range ENV_GPIO_RANGE_MIN ENV_GPIO_OUT_RANGE_MAX + default 6 + + config BME280_I2C_ADDR + hex "BME280 I2C address" + range 0x00 0xFF + default 0x76 + + config LSM6DSO_I2C_ADDR + hex "LSM6DSO I2C address" + range 0x00 0xFF + default 0x6B +endmenu \ No newline at end of file diff --git a/boards/main/bme280.cpp b/boards/main/bme280.cpp new file mode 100644 index 0000000..7724be1 --- /dev/null +++ b/boards/main/bme280.cpp @@ -0,0 +1,131 @@ +#include "bme280.h" + +#include + +#include +#include +#include +#include +#include + +#include "BME280_SensorAPI/bme280.h" +#include "common.h" +#include "esp_log.h" +#include "i2c.h" +#include "sdkconfig.h" +#include "storage.h" + +namespace bme280 { + +constexpr inline const char *TAG = "BME280"; + +// Indicates that an error has occurred during initialization and all subsequent +// operations will fail. +bool g_init_error = false; +// Sensor driver context +struct bme280_dev g_sensor; + +// I/O functions passed to bme280 driver +BME280_INTF_RET_TYPE platform_read(uint8_t reg_addr, uint8_t *reg_data, + uint32_t length, void *intf_ptr) { + return i2c::read(CONFIG_BME280_I2C_ADDR, reg_addr, reg_data, length) + .transform([](Success) -> BME280_INTF_RET_TYPE { return 0; }) + .value_or(-1); +} + +BME280_INTF_RET_TYPE platform_write(uint8_t reg_addr, const uint8_t *reg_data, + uint32_t length, void *intf_ptr) { + return i2c::write(CONFIG_BME280_I2C_ADDR, reg_addr, reg_data, length) + .transform([](Success) -> BME280_INTF_RET_TYPE { return 0; }) + .value_or(-1); +} + +void bme280_delay_us(uint32_t period, void *intf_ptr) { usleep(period); } + +auto init() -> std::expected { + g_sensor.intf = BME280_I2C_INTF; + g_sensor.intf_ptr = nullptr; // Not used + g_sensor.read = platform_read; + g_sensor.write = platform_write; + g_sensor.delay_us = bme280_delay_us; + + int8_t res = bme280_init(&g_sensor); + if (res != BME280_OK) { + ESP_LOGE(TAG, "sensor not found. %d", res); + g_init_error = true; + return std::unexpected(Error::BME280_INIT_FAILED); + } + + struct bme280_settings settings; + + // Filter is of no use to us, because we need to know values at the moment, + // even if they reach extremes and last only a short while + settings.filter = BME280_FILTER_COEFF_OFF; + + // Only the pressure is oversampled to provide higher accuracy + settings.osr_h = BME280_OVERSAMPLING_1X; + settings.osr_p = BME280_OVERSAMPLING_16X; + settings.osr_t = BME280_OVERSAMPLING_1X; + + // Measure continuously + settings.standby_time = BME280_STANDBY_TIME_0_5_MS; + + res = bme280_set_sensor_settings(BME280_SEL_ALL_SETTINGS, &settings, + &g_sensor); + if (res != BME280_OK) { + ESP_LOGE(TAG, "Set settings failed"); + g_init_error = true; + return std::unexpected(Error::BME280_INIT_FAILED); + } + + res = bme280_set_sensor_mode(BME280_POWERMODE_NORMAL, &g_sensor); + if (res != BME280_OK) { + ESP_LOGE(TAG, "Set mode failed"); + g_init_error = true; + return std::unexpected(Error::BME280_INIT_FAILED); + } + + uint32_t period = 0; + bme280_cal_meas_delay(&period, &settings); + ESP_LOGD(TAG, "Measurement time [ms]: %f", period / 1000.0f); + + return Success{}; +} + +static auto readData() -> std::expected { + Data data{}; + if (g_init_error) { + return std::unexpected(Error::BME280_INIT_FAILED); + } + + struct bme280_data raw_data{}; + if (bme280_get_sensor_data(BME280_PRESS, &raw_data, &g_sensor) != 0) { + ESP_LOGE(TAG, "Read failed. Reinitializing..."); + init(); + g_init_error = false; + return std::unexpected(Error::BME280_READ_FAILED); + } + data.air_pressure = static_cast( + raw_data.pressure); // We don't need double precision + if (bme280_get_sensor_data(BME280_HUM, &raw_data, &g_sensor) != 0) { + return std::unexpected(Error::BME280_READ_FAILED); + } + data.humidity = static_cast(raw_data.humidity); + if (bme280_get_sensor_data(BME280_TEMP, &raw_data, &g_sensor) != 0) { + return std::unexpected(Error::BME280_READ_FAILED); + } + data.temperature = static_cast(raw_data.temperature); + + return data; +} + +auto readAndProcessData() -> std::expected { + auto ret = readData(); + + return ret.and_then([](Data const &data) -> std::expected { + storage::postBarometerData(data); + return Success{}; + }); +} + +} // namespace bme280 diff --git a/boards/main/bme280.h b/boards/main/bme280.h new file mode 100644 index 0000000..a47383c --- /dev/null +++ b/boards/main/bme280.h @@ -0,0 +1,18 @@ +#pragma once + +#include + +#include "common.h" + +namespace bme280 { + +struct Data { + float air_pressure; + float humidity; + float temperature; +}; + +auto init() -> std::expected; +auto readAndProcessData() -> std::expected; + +} // namespace bme280 \ No newline at end of file diff --git a/boards/main/common.h b/boards/main/common.h new file mode 100644 index 0000000..39daca9 --- /dev/null +++ b/boards/main/common.h @@ -0,0 +1,22 @@ +#pragma once + +#include +#include + +struct Success {}; + +enum class Error : uint8_t { + FAILURE, + + I2C_INIT_FAILED = 0x10, + I2C_WRITE_FAILED, + I2C_READ_FAILED, + + BME280_INIT_FAILED = 0x20, + BME280_READ_FAILED, + + LSM6DSO_INIT_FAILED = 0x30, + LSM6DSO_READ_FAILED, + + STORAGE_INIT_FAILED = 0x40, +}; diff --git a/boards/main/i2c.cpp b/boards/main/i2c.cpp new file mode 100644 index 0000000..cc86592 --- /dev/null +++ b/boards/main/i2c.cpp @@ -0,0 +1,101 @@ +#include "i2c.h" + +#include +#include + +#include "common.h" +#include "driver/i2c.h" +#include "esp_log.h" + +namespace i2c { + +constexpr inline const char *TAG = "I2C"; + +// The project uses only one I2C bus +constexpr inline i2c_port_t BUS_NUMBER = I2C_NUM_0; + +constexpr inline int FREQUENCY = 400000; + +constexpr inline uint32_t TIMEOUT = 50; + +auto init() -> std::expected { + i2c_config_t conf = { + .mode = I2C_MODE_MASTER, + .sda_io_num = CONFIG_I2C_SDA_GPIO, + .scl_io_num = CONFIG_I2C_SCL_GPIO, + .sda_pullup_en = true, + .scl_pullup_en = true, + .master = + { + .clk_speed = FREQUENCY, + }, + .clk_flags = I2C_SCLK_SRC_FLAG_FOR_NOMAL, + }; + + if (i2c_param_config(BUS_NUMBER, &conf) != ESP_OK) { + ESP_LOGE(TAG, "Config error"); + return std::unexpected(Error::I2C_INIT_FAILED); + } + + if (i2c_driver_install(BUS_NUMBER, conf.mode, 0, 0, 0) != ESP_OK) { + ESP_LOGE(TAG, "Init error"); + return std::unexpected(Error::I2C_INIT_FAILED); + } + + return Success{}; +} + +auto read(uint8_t addr, uint8_t reg, uint8_t *buffer, uint16_t size) + -> std::expected { + if (i2c_master_write_read_device(BUS_NUMBER, addr, ®, 1, buffer, size, + TIMEOUT / portTICK_PERIOD_MS) != ESP_OK) { + ESP_LOGE(TAG, "Failed to read"); + return std::unexpected(Error::I2C_READ_FAILED); + } + + return Success{}; +} + +static auto performWriteTransaction(uint8_t addr, uint8_t reg, + const uint8_t *buffer, uint16_t size, + i2c_cmd_handle_t command) + -> std::expected { + if (i2c_master_start(command) != ESP_OK) { + return std::unexpected(Error::I2C_WRITE_FAILED); + } + if (i2c_master_write_byte(command, addr << 1U, true) != ESP_OK) { + return std::unexpected(Error::I2C_WRITE_FAILED); + } + if (i2c_master_write_byte(command, reg, true) != ESP_OK) { + return std::unexpected(Error::I2C_WRITE_FAILED); + } + if (i2c_master_write(command, buffer, size, true) != ESP_OK) { + return std::unexpected(Error::I2C_WRITE_FAILED); + } + if (i2c_master_stop(command) != ESP_OK) { + return std::unexpected(Error::I2C_WRITE_FAILED); + } + if (i2c_master_cmd_begin(BUS_NUMBER, command, TIMEOUT) != ESP_OK) { + return std::unexpected(Error::I2C_WRITE_FAILED); + } + return Success{}; +} + +auto write(uint8_t addr, uint8_t reg, const uint8_t *buffer, uint16_t size) + -> std::expected { + uint8_t command_buffer[I2C_LINK_RECOMMENDED_SIZE(2)] = {0}; + i2c_cmd_handle_t command = i2c_cmd_link_create_static( + command_buffer, I2C_LINK_RECOMMENDED_SIZE(2)); + + auto res = performWriteTransaction(addr, reg, buffer, size, command); + + i2c_cmd_link_delete_static(command); + + return res.or_else([](Error const &error) -> std::expected { + ESP_LOGE(TAG, "Failed to write"); + + return std::unexpected(error); + }); +} + +} // namespace i2c diff --git a/boards/main/i2c.h b/boards/main/i2c.h new file mode 100644 index 0000000..13f3bf1 --- /dev/null +++ b/boards/main/i2c.h @@ -0,0 +1,15 @@ +#pragma once + +#include + +#include "common.h" + +namespace i2c { + +auto init() -> std::expected; +auto read(uint8_t addr, uint8_t reg, uint8_t *buffer, uint16_t size) + -> std::expected; +auto write(uint8_t addr, uint8_t reg, const uint8_t *buffer, uint16_t size) + -> std::expected; + +} // namespace i2c diff --git a/boards/main/lsm6dso.cpp b/boards/main/lsm6dso.cpp new file mode 100644 index 0000000..5bc8c9f --- /dev/null +++ b/boards/main/lsm6dso.cpp @@ -0,0 +1,172 @@ +#include "lsm6dso.h" + +#include +#include +#include + +#include +#include +#include + +#include "common.h" +#include "esp_log.h" +#include "i2c.h" +#include "sdkconfig.h" +#include "storage.h" + +namespace lsm6dso { + +constexpr inline const char *TAG = "LSM6DSO"; + +constexpr inline int SENSOR_BOOT_TIME = 10; // In milliseconds +constexpr inline size_t RAW_DATA_BUFFER_SIZE = 6; + +// Indicates that an error has occurred during initialization and all subsequent +// operations will fail. +bool g_init_error = false; + +// Sensor driver object +stmdev_ctx_t g_sensor_ctx; + +// I/O functions passed to the lsm6dso driver: +static int32_t platform_write(void *handle, uint8_t reg, const uint8_t *bufp, + uint16_t len) { + // We bit shift the address by one because the driver includes the + // read/write bit, which is not needed, because the I2C implementation + // already adds it automatically + return i2c::write(CONFIG_LSM6DSO_I2C_ADDR, reg, bufp, len) + .transform([](Success) -> int32_t { return 0; }) + .value_or(1); +} + +static int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp, + uint16_t len) { + // We bit shift the address by one because the driver includes the + // read/write bit, which is not needed, because the I2C implementation + // already adds it automatically + return i2c::read(CONFIG_LSM6DSO_I2C_ADDR, reg, bufp, len) + .transform([](Success) -> int32_t { return 0; }) + .value_or(1); +} + +static void platform_delay(uint32_t ms) { usleep(ms * 1000); } + +auto init() -> std::expected { + g_sensor_ctx.write_reg = platform_write; + g_sensor_ctx.read_reg = platform_read; + g_sensor_ctx.mdelay = platform_delay; + g_sensor_ctx.handle = nullptr; // Not used + g_sensor_ctx.priv_data = nullptr; // Not used + + // Wait for the sensor to boot + vTaskDelay(SENSOR_BOOT_TIME / portTICK_PERIOD_MS); + uint8_t device_id = 0; + if (lsm6dso_device_id_get(&g_sensor_ctx, &device_id) != 0) { + ESP_LOGE(TAG, "sensor not found."); + g_init_error = true; + return std::unexpected(Error::LSM6DSO_INIT_FAILED); + } + if (device_id != LSM6DSO_ID) { + ESP_LOGE(TAG, "invalid device id."); + g_init_error = true; + return std::unexpected(Error::LSM6DSO_INIT_FAILED); + } + lsm6dso_reset_set(&g_sensor_ctx, PROPERTY_ENABLE); + uint8_t reset = 0; + do { + vTaskDelay(1 / portTICK_PERIOD_MS); + if (lsm6dso_reset_get(&g_sensor_ctx, &reset) != + 0) { // Stop if an error occurs + g_init_error = true; + return std::unexpected(Error::LSM6DSO_INIT_FAILED); + } + } while (reset); + + lsm6dso_i3c_disable_set(&g_sensor_ctx, LSM6DSO_I3C_DISABLE); + lsm6dso_block_data_update_set(&g_sensor_ctx, PROPERTY_ENABLE); + + lsm6dso_xl_full_scale_set(&g_sensor_ctx, LSM6DSO_16g); + lsm6dso_gy_full_scale_set(&g_sensor_ctx, LSM6DSO_2000dps); + + // Setup internal FIFO. The sensor will collect measurements in its internal + // memory and store it until the main task reads it (multiple measurements + // are read at once). + lsm6dso_fifo_xl_batch_set(&g_sensor_ctx, LSM6DSO_XL_BATCHED_AT_417Hz); + lsm6dso_fifo_gy_batch_set(&g_sensor_ctx, LSM6DSO_GY_BATCHED_AT_417Hz); + + lsm6dso_fifo_mode_set(&g_sensor_ctx, LSM6DSO_STREAM_MODE); + + lsm6dso_xl_data_rate_set(&g_sensor_ctx, LSM6DSO_XL_ODR_417Hz); + lsm6dso_gy_data_rate_set(&g_sensor_ctx, LSM6DSO_GY_ODR_417Hz); + + return Success{}; +} + +static std::array convertToSignedShort( + const std::array &raw_data_buffer) { + std::array tmp{}; + std::memcpy(tmp.data(), raw_data_buffer.data(), + RAW_DATA_BUFFER_SIZE * sizeof(uint8_t)); + return tmp; +} + +auto readAndProcessData() -> std::expected { + lsm6dso_fifo_tag_t tag{}; + uint16_t data_count = 0; + + if (g_init_error) { + return std::unexpected(Error::LSM6DSO_INIT_FAILED); + }; + + // Check number of samples stored in FIFO + if (lsm6dso_fifo_data_level_get(&g_sensor_ctx, &data_count) != 0) { + // Failed to read data from the sensor + ESP_LOGE(TAG, "read failed. Reinitializing..."); + init(); + return std::unexpected(Error::LSM6DSO_READ_FAILED); + } + + // Serial.printf("LSM6DSO: reading %d measurements from FIFO\n", + // data_count); + + std::array raw_data_buffer{}; + + while (data_count--) { + lsm6dso_fifo_sensor_tag_get(&g_sensor_ctx, &tag); + switch (tag) { + case LSM6DSO_XL_NC_TAG: { + if (lsm6dso_fifo_out_raw_get(&g_sensor_ctx, + raw_data_buffer.data()) != 0) { + return std::unexpected(Error::LSM6DSO_READ_FAILED); + } + storage::postAccelerationData( + convertToSignedShort(raw_data_buffer)); + break; + } + case LSM6DSO_GYRO_NC_TAG: { + if (lsm6dso_fifo_out_raw_get(&g_sensor_ctx, + raw_data_buffer.data()) != 0) { + return std::unexpected(Error::LSM6DSO_READ_FAILED); + } + storage::postAngularRateData( + convertToSignedShort(raw_data_buffer)); + break; + } + default: { + // Even if we don't use the data type, it still needs to be read + // to free the internal FIFO + ESP_LOGE(TAG, "excessive data found"); + auto res = lsm6dso_fifo_out_raw_get(&g_sensor_ctx, + raw_data_buffer.data()); + if (res != 0) { + return std::unexpected(Error::LSM6DSO_READ_FAILED); + } + break; + } + } + } + + return Success{}; +} + +} // namespace lsm6dso \ No newline at end of file diff --git a/boards/main/lsm6dso.h b/boards/main/lsm6dso.h new file mode 100644 index 0000000..090d83f --- /dev/null +++ b/boards/main/lsm6dso.h @@ -0,0 +1,15 @@ +#pragma once + +#include + +#include "common.h" + +namespace lsm6dso { + +constexpr inline size_t ACCEL_BUFFER_SIZE = 256; +constexpr inline size_t ANGULAR_RATE_BUFFER_SIZE = 256; + +auto init() -> std::expected; +auto readAndProcessData() -> std::expected; + +} // namespace lsm6dso \ No newline at end of file diff --git a/boards/main/main.cpp b/boards/main/main.cpp index da74946..7982b6d 100755 --- a/boards/main/main.cpp +++ b/boards/main/main.cpp @@ -1,11 +1,101 @@ +#include "bme280.h" #include "esp_log.h" +#include "esp_timer.h" #include "freertos/FreeRTOS.h" +#include "freertos/idf_additions.h" +#include "i2c.h" +#include "lsm6dso.h" +#include "storage.h" -static const char *TAG = "rocket"; +constexpr inline const char *TAG = "ROCKET"; + +// delay between sensor reads (in milliseconds) +static constexpr inline int MAIN_TICK_INTERVAL = 50; + +static void mainLoopTimerCallback(void *arg); extern "C" void app_main(void) { + ESP_LOGI(TAG, "Initializing ROCKET"); + + if (i2c::init().has_value()) { + if (auto res = bme280::init(); !res.has_value()) { + ESP_LOGE(TAG, + "Initializing BME280 failed (CODE %d). Proceeding anyways", + res.error()); + } + if (auto res = lsm6dso::init(); !res.has_value()) { + ESP_LOGE( + TAG, + "Initializing LSM6DSO failed (CODE %d). Proceeding anyways", + res.error()); + } + } else { + ESP_LOGE(TAG, "Initializing I2C failed. Proceeding anyways"); + } + + auto main_loop_semaphore = xSemaphoreCreateBinary(); + if (main_loop_semaphore == nullptr) { + ESP_LOGE(TAG, + "Failed to create main loop semaphore. Proceeding anyways"); + } else { + const esp_timer_create_args_t timer_config = { + .callback = mainLoopTimerCallback, + .arg = main_loop_semaphore, + .dispatch_method = ESP_TIMER_TASK, + .name = "MAIN_LOOP_TIMER", + .skip_unhandled_events = true}; + esp_timer_handle_t timer; + if (auto res = esp_timer_create(&timer_config, &timer); res != ESP_OK) { + ESP_LOGE(TAG, + "Main loop timer creation failed (code: %d). Proceeding " + "anyways", + res); + } else if (auto res = esp_timer_start_periodic( + timer, MAIN_TICK_INTERVAL * 1000LLU); + res != ESP_OK) { + ESP_LOGE( + TAG, + "Main loop timer start failed (code: %d). Proceeding anyways", + res); + } + } + + if (!storage::init().has_value()) { + ESP_LOGE(TAG, "Initializing STORAGE failed. Proceeding anyways"); + } + while (true) { - ESP_LOGI(TAG, "Hello World!"); - vTaskDelay(1000 / portTICK_PERIOD_MS); + static int64_t last_time = 0; + if (last_time == 0) { + last_time = esp_timer_get_time(); + } + + if (xSemaphoreTake(main_loop_semaphore, 500) == pdTRUE) { + auto time = esp_timer_get_time(); + auto time_diff = time - last_time; + last_time = time; + + auto res = bme280::readAndProcessData(); + if (!res.has_value()) { + ESP_LOGE(TAG, "Reading BME280 data failed (CODE %d)", + res.error()); + } + res = lsm6dso::readAndProcessData(); + if (!res.has_value()) { + ESP_LOGE(TAG, "Reading LSM6DSO data failed (CODE %d)", + res.error()); + } + + ESP_LOGI(TAG, + "Sensor data has been read.\tSince last read: " + "%2.2fms\tReading data took: %.2fms", + time_diff / 1000.0f, + (esp_timer_get_time() - time) / 1000.0f); + } } } + +static void mainLoopTimerCallback(void *arg) { + auto main_loop_semaphore = static_cast(arg); + xSemaphoreGive(main_loop_semaphore); +} diff --git a/boards/main/storage.cpp b/boards/main/storage.cpp new file mode 100644 index 0000000..f1a02df --- /dev/null +++ b/boards/main/storage.cpp @@ -0,0 +1,144 @@ +#include "storage.h" + +#include +#include +#include + +#include +#include + +#include "common.h" +#include "esp_err.h" +#include "esp_log.h" +#include "esp_timer.h" + +namespace storage { + +constexpr inline const char *TAG = "STORAGE"; + +// 1000ms delay between data flushes +static constexpr inline int DATA_FLUSH_INTERVAL = 1000; + +// Thread safe (atomic) Single Producer Single Consumer ring buffer +SPSCQueue g_barometer_data_buffer; +SPSCQueue, 1024> g_acceleration_buffer; +SPSCQueue, 1024> g_angular_rate_buffer; +// SPSCQueue g_gps_data_buffer; + +static void timerCallback(void *arg) { + auto sem = static_cast(arg); + xSemaphoreGive(sem); +} + +static void flushTask(void *pvParameters) { + auto sem = static_cast(pvParameters); + + while (true) { + if (xSemaphoreTake(sem, 500) == pdTRUE) { + ESP_LOGI(TAG, "Storage flush started"); + + ESP_LOGI(TAG, "BME280 data:"); + while (!g_barometer_data_buffer.empty()) { + bme280::Data data{}; + g_barometer_data_buffer.pop(data); + + ESP_LOGI(TAG, "%6.2fPa, %2.2f%%, %2.2fC", data.air_pressure, + data.humidity, data.temperature); + } + + ESP_LOGI(TAG, "LSM6DSO32 acceleration:"); + int cnt = 0; + while (!g_acceleration_buffer.empty()) { + std::array data{}; + g_acceleration_buffer.pop(data); + + if (cnt < 5) + ESP_LOGI(TAG, "X: %6d Y: %6d Z: %6d", data[0], data[1], + data[2]); + + cnt++; + } + ESP_LOGI(TAG, "And %d more records", cnt - 5); + + ESP_LOGI(TAG, "LSM6DSO32 angular rate:"); + cnt = 0; + while (!g_angular_rate_buffer.empty()) { + std::array data{}; + g_angular_rate_buffer.pop(data); + + if (cnt < 5) + ESP_LOGI(TAG, "X: %6d Y: %6d Z: %6d", data[0], data[1], + data[2]); + + cnt++; + } + ESP_LOGI(TAG, "And %d more records", cnt - 5); + + // ESP_LOGI(TAG, "GPS data:"); + // while (!g_gps_data_buffer.empty()) { + // gps::Data data; + // g_gps_data_buffer.pop(data); + + // ESP_LOGI(TAG, "%s\n", data.data()); + // } + + ESP_LOGI(TAG, "Storage flush complete"); + } + } +} + +auto init() -> std::expected { + auto sem = xSemaphoreCreateBinary(); + if (sem == nullptr) { + ESP_LOGE(TAG, "Semaphore creation error"); + return std::unexpected(Error::STORAGE_INIT_FAILED); + } + + const esp_timer_create_args_t timer_config = { + .callback = timerCallback, + .arg = sem, + .dispatch_method = ESP_TIMER_TASK, + .name = "STORAGE_FLUSH_TIMER", + .skip_unhandled_events = true}; + esp_timer_handle_t timer; + if (auto res = esp_timer_create(&timer_config, &timer); res != ESP_OK) { + ESP_LOGE(TAG, "ESP timer creation failed (code: %d)", res); + return std::unexpected(Error::STORAGE_INIT_FAILED); + } + if (auto res = + esp_timer_start_periodic(timer, DATA_FLUSH_INTERVAL * 1000LLU); + res != ESP_OK) { + ESP_LOGE(TAG, "ESP timer start failed (code: %d)", res); + return std::unexpected(Error::STORAGE_INIT_FAILED); + } + + if (xTaskCreate(flushTask, "STORAGE_FLUSH_TASK", /* ucStackDepth = */ 4096, + sem, /* uxPriority = */ 11, nullptr) != pdPASS) { + ESP_LOGE(TAG, "Failed to create RTOS task"); + return std::unexpected(Error::STORAGE_INIT_FAILED); + } + + return Success{}; +} + +void postBarometerData(const bme280::Data &data) { + if (!g_barometer_data_buffer.push(data)) + ESP_LOGE(TAG, "Storage barometer data buffer overflow"); +} + +void postAccelerationData(const std::array &data) { + if (!g_acceleration_buffer.push(data)) + ESP_LOGE(TAG, "Storage acceleration buffer overflow"); +} + +void postAngularRateData(const std::array &data) { + if (!g_angular_rate_buffer.push(data)) + ESP_LOGE(TAG, "Storage angular rate buffer overflow"); +} + +// void postGpsData(const gps::Data &data) { +// if (!g_gps_data_buffer.push(data)) +// ESP_LOGE(TAG, "GPS data buffer overflow"); +// } + +} // namespace storage \ No newline at end of file diff --git a/boards/main/storage.h b/boards/main/storage.h new file mode 100644 index 0000000..96392f9 --- /dev/null +++ b/boards/main/storage.h @@ -0,0 +1,16 @@ +#pragma once + +#include + +#include "bme280.h" +#include "common.h" + +namespace storage { + +auto init() -> std::expected; +void postBarometerData(const bme280::Data &data); +void postAccelerationData(const std::array &data); +void postAngularRateData(const std::array &data); +// void postGpsData(const gps::Data &data); + +} // namespace storage