From 05d47c259231e53b7cb24e672a91eae106318c51 Mon Sep 17 00:00:00 2001 From: Tomasz Klimek Date: Fri, 9 Jan 2026 19:09:02 +0100 Subject: [PATCH 1/7] Add I2C driver abstraction --- boards/main/CMakeLists.txt | 3 +- boards/main/Kconfig.projbuild | 13 +++++ boards/main/common.h | 12 +++++ boards/main/i2c.cpp | 98 +++++++++++++++++++++++++++++++++++ boards/main/i2c.h | 13 +++++ boards/main/main.cpp | 7 +-- 6 files changed, 142 insertions(+), 4 deletions(-) create mode 100644 boards/main/Kconfig.projbuild create mode 100644 boards/main/common.h create mode 100644 boards/main/i2c.cpp create mode 100644 boards/main/i2c.h diff --git a/boards/main/CMakeLists.txt b/boards/main/CMakeLists.txt index 67233cd..67ee619 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) diff --git a/boards/main/Kconfig.projbuild b/boards/main/Kconfig.projbuild new file mode 100644 index 0000000..c27d2b9 --- /dev/null +++ b/boards/main/Kconfig.projbuild @@ -0,0 +1,13 @@ +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 +endmenu \ No newline at end of file diff --git a/boards/main/common.h b/boards/main/common.h new file mode 100644 index 0000000..aa25d6d --- /dev/null +++ b/boards/main/common.h @@ -0,0 +1,12 @@ +#pragma once + +#include + +enum class Result : uint8_t { + SUCCESS = 0, + FAILURE, + + I2C_INIT_FAILED, + I2C_WRITE_FAILED, + I2C_READ_FAILED, +}; diff --git a/boards/main/i2c.cpp b/boards/main/i2c.cpp new file mode 100644 index 0000000..44086d0 --- /dev/null +++ b/boards/main/i2c.cpp @@ -0,0 +1,98 @@ +#include "i2c.h" + +#include + +#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; + +Result init() { + 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 Result::I2C_INIT_FAILED; + } + + if (i2c_driver_install(BUS_NUMBER, conf.mode, 0, 0, 0) != ESP_OK) { + ESP_LOGE(TAG, "Init error"); + return Result::I2C_INIT_FAILED; + } + + return Result::SUCCESS; +} + +Result read(uint8_t addr, uint8_t reg, uint8_t *buffer, uint16_t size) { + 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 Result::I2C_READ_FAILED; + } + + return Result::SUCCESS; +} + +static Result performWriteTransaction(uint8_t addr, uint8_t reg, + const uint8_t *buffer, uint16_t size, + i2c_cmd_handle_t command) { + if (i2c_master_start(command) != ESP_OK) { + return Result::I2C_WRITE_FAILED; + } + if (i2c_master_write_byte(command, addr << 1U, true) != ESP_OK) { + return Result::I2C_WRITE_FAILED; + } + if (i2c_master_write_byte(command, reg, true) != ESP_OK) { + return Result::I2C_WRITE_FAILED; + } + if (i2c_master_write(command, buffer, size, true) != ESP_OK) { + return Result::I2C_WRITE_FAILED; + } + if (i2c_master_stop(command) != ESP_OK) { + return Result::I2C_WRITE_FAILED; + } + if (i2c_master_cmd_begin(BUS_NUMBER, command, TIMEOUT) != ESP_OK) { + return Result::I2C_WRITE_FAILED; + } + return Result::SUCCESS; +} + +Result write(uint8_t addr, uint8_t reg, const uint8_t *buffer, uint16_t size) { + 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)); + + Result res = performWriteTransaction(addr, reg, buffer, size, command); + if (res != Result::SUCCESS) { + ESP_LOGE(TAG, "Failed to write"); + i2c_cmd_link_delete_static(command); + + return res; + } + + i2c_cmd_link_delete_static(command); + + return Result::SUCCESS; +} + +} // namespace i2c diff --git a/boards/main/i2c.h b/boards/main/i2c.h new file mode 100644 index 0000000..494dac2 --- /dev/null +++ b/boards/main/i2c.h @@ -0,0 +1,13 @@ +#pragma once + +#include + +#include "common.h" + +namespace i2c { + +Result init(); +Result read(uint8_t addr, uint8_t reg, uint8_t *buffer, uint16_t size); +Result write(uint8_t addr, uint8_t reg, const uint8_t *buffer, uint16_t size); + +} // namespace i2c diff --git a/boards/main/main.cpp b/boards/main/main.cpp index da74946..a682382 100755 --- a/boards/main/main.cpp +++ b/boards/main/main.cpp @@ -1,11 +1,12 @@ -#include "esp_log.h" #include "freertos/FreeRTOS.h" +#include "i2c.h" static const char *TAG = "rocket"; extern "C" void app_main(void) { + i2c::init(); + while (true) { - ESP_LOGI(TAG, "Hello World!"); - vTaskDelay(1000 / portTICK_PERIOD_MS); + vTaskDelay(10 / portTICK_PERIOD_MS); } } From 34fcbf47daf40ab56d1b72e4a480d9b889cdf0e6 Mon Sep 17 00:00:00 2001 From: Tomasz Klimek Date: Fri, 30 Jan 2026 20:01:02 +0100 Subject: [PATCH 2/7] Add BME280 driver abstraction, improve error handling --- .gitmodules | 3 + boards/CMakeLists.txt | 9 +- boards/components/BME280/BME280_SensorAPI | 1 + boards/components/BME280/CMakeLists.txt | 2 + boards/main/CMakeLists.txt | 2 +- boards/main/Kconfig.projbuild | 5 + boards/main/bme280.cpp | 132 ++++++++++++++++++++++ boards/main/bme280.h | 18 +++ boards/main/common.h | 14 ++- boards/main/i2c.cpp | 55 ++++----- boards/main/i2c.h | 8 +- boards/main/main.cpp | 23 +++- 12 files changed, 235 insertions(+), 37 deletions(-) create mode 100644 .gitmodules create mode 160000 boards/components/BME280/BME280_SensorAPI create mode 100644 boards/components/BME280/CMakeLists.txt create mode 100644 boards/main/bme280.cpp create mode 100644 boards/main/bme280.h diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..54af1d1 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "boards/components/BME280/BME280_SensorAPI"] + path = boards/components/BME280/BME280_SensorAPI + url = https://github.com/boschsensortec/BME280_SensorAPI.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/main/CMakeLists.txt b/boards/main/CMakeLists.txt index 67ee619..207147a 100755 --- a/boards/main/CMakeLists.txt +++ b/boards/main/CMakeLists.txt @@ -2,4 +2,4 @@ file(GLOB_RECURSE MAIN_SOURCES "*.cpp") idf_component_register(SRCS ${MAIN_SOURCES} INCLUDE_DIRS "." - REQUIRES driver) + REQUIRES driver BME280) diff --git a/boards/main/Kconfig.projbuild b/boards/main/Kconfig.projbuild index c27d2b9..3d360ac 100644 --- a/boards/main/Kconfig.projbuild +++ b/boards/main/Kconfig.projbuild @@ -10,4 +10,9 @@ menu "Rocket board configuration" 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 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..7d6a75c --- /dev/null +++ b/boards/main/bme280.cpp @@ -0,0 +1,132 @@ +#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" + +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) { + i2c::write(CONFIG_BME280_I2C_ADDR, reg_addr, reg_data, length) + .transform([](Success) -> BME280_INTF_RET_TYPE { return 0; }) + .value_or(-1); + return 0; +} + +void bme280_delay_us(uint32_t period, void *intf_ptr) { usleep(period); } + +auto init() -> Result { + 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() -> Result { + 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(); + 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() -> Result { + auto ret = readData(); + + return ret.and_then([](Data const &data) -> Result { + // storage::postBarometerData(data); + ESP_LOGI(TAG, "%f %f %f", data.air_pressure, data.humidity, + data.temperature); + return Success{}; + }); +} + +} // namespace bme280 diff --git a/boards/main/bme280.h b/boards/main/bme280.h new file mode 100644 index 0000000..2ad1f3f --- /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() -> Result; +auto readAndProcessData() -> std::expected; + +} // namespace bme280 \ No newline at end of file diff --git a/boards/main/common.h b/boards/main/common.h index aa25d6d..3fb1452 100644 --- a/boards/main/common.h +++ b/boards/main/common.h @@ -1,12 +1,20 @@ #pragma once #include +#include -enum class Result : uint8_t { - SUCCESS = 0, +struct Success {}; + +enum class Error : uint8_t { FAILURE, - I2C_INIT_FAILED, + I2C_INIT_FAILED = 0x10, I2C_WRITE_FAILED, I2C_READ_FAILED, + + BME280_INIT_FAILED = 0x20, + BME280_READ_FAILED, }; + +template +using Result = std::expected; diff --git a/boards/main/i2c.cpp b/boards/main/i2c.cpp index 44086d0..952b925 100644 --- a/boards/main/i2c.cpp +++ b/boards/main/i2c.cpp @@ -1,7 +1,9 @@ #include "i2c.h" #include +#include +#include "common.h" #include "driver/i2c.h" #include "esp_log.h" @@ -16,7 +18,7 @@ constexpr inline int FREQUENCY = 400000; constexpr inline uint32_t TIMEOUT = 50; -Result init() { +auto init() -> Result { i2c_config_t conf = { .mode = I2C_MODE_MASTER, .sda_io_num = CONFIG_I2C_SDA_GPIO, @@ -32,67 +34,68 @@ Result init() { if (i2c_param_config(BUS_NUMBER, &conf) != ESP_OK) { ESP_LOGE(TAG, "Config error"); - return Result::I2C_INIT_FAILED; + 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 Result::I2C_INIT_FAILED; + return std::unexpected(Error::I2C_INIT_FAILED); } - return Result::SUCCESS; + return Success{}; } -Result read(uint8_t addr, uint8_t reg, uint8_t *buffer, uint16_t size) { +auto read(uint8_t addr, uint8_t reg, uint8_t *buffer, uint16_t size) + -> Result { 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 Result::I2C_READ_FAILED; + return std::unexpected(Error::I2C_READ_FAILED); } - return Result::SUCCESS; + return Success{}; } -static Result performWriteTransaction(uint8_t addr, uint8_t reg, - const uint8_t *buffer, uint16_t size, - i2c_cmd_handle_t command) { +static auto performWriteTransaction(uint8_t addr, uint8_t reg, + const uint8_t *buffer, uint16_t size, + i2c_cmd_handle_t command) + -> Result { if (i2c_master_start(command) != ESP_OK) { - return Result::I2C_WRITE_FAILED; + return std::unexpected(Error::I2C_WRITE_FAILED); } if (i2c_master_write_byte(command, addr << 1U, true) != ESP_OK) { - return Result::I2C_WRITE_FAILED; + return std::unexpected(Error::I2C_WRITE_FAILED); } if (i2c_master_write_byte(command, reg, true) != ESP_OK) { - return Result::I2C_WRITE_FAILED; + return std::unexpected(Error::I2C_WRITE_FAILED); } if (i2c_master_write(command, buffer, size, true) != ESP_OK) { - return Result::I2C_WRITE_FAILED; + return std::unexpected(Error::I2C_WRITE_FAILED); } if (i2c_master_stop(command) != ESP_OK) { - return Result::I2C_WRITE_FAILED; + return std::unexpected(Error::I2C_WRITE_FAILED); } if (i2c_master_cmd_begin(BUS_NUMBER, command, TIMEOUT) != ESP_OK) { - return Result::I2C_WRITE_FAILED; + return std::unexpected(Error::I2C_WRITE_FAILED); } - return Result::SUCCESS; + return Success{}; } -Result write(uint8_t addr, uint8_t reg, const uint8_t *buffer, uint16_t size) { +auto write(uint8_t addr, uint8_t reg, const uint8_t *buffer, uint16_t size) + -> Result { 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)); - Result res = performWriteTransaction(addr, reg, buffer, size, command); - if (res != Result::SUCCESS) { - ESP_LOGE(TAG, "Failed to write"); - i2c_cmd_link_delete_static(command); - - return res; - } + auto res = performWriteTransaction(addr, reg, buffer, size, command); i2c_cmd_link_delete_static(command); - return Result::SUCCESS; + return res.or_else([](Error const &error) -> Result { + ESP_LOGE(TAG, "Failed to write"); + + return std::unexpected(error); + }); } } // namespace i2c diff --git a/boards/main/i2c.h b/boards/main/i2c.h index 494dac2..b9521f5 100644 --- a/boards/main/i2c.h +++ b/boards/main/i2c.h @@ -6,8 +6,10 @@ namespace i2c { -Result init(); -Result read(uint8_t addr, uint8_t reg, uint8_t *buffer, uint16_t size); -Result write(uint8_t addr, uint8_t reg, const uint8_t *buffer, uint16_t size); +auto init() -> Result; +auto read(uint8_t addr, uint8_t reg, uint8_t *buffer, uint16_t size) + -> Result; +auto write(uint8_t addr, uint8_t reg, const uint8_t *buffer, uint16_t size) + -> Result; } // namespace i2c diff --git a/boards/main/main.cpp b/boards/main/main.cpp index a682382..b004c47 100755 --- a/boards/main/main.cpp +++ b/boards/main/main.cpp @@ -1,12 +1,29 @@ +#include "bme280.h" +#include "esp_log.h" #include "freertos/FreeRTOS.h" #include "i2c.h" -static const char *TAG = "rocket"; +constexpr inline const char *TAG = "ROCKET"; extern "C" void app_main(void) { - i2c::init(); + ESP_LOGI(TAG, "INITIALIZING ROCKET"); + + if (i2c::init().has_value()) { + auto res = bme280::init(); + if (!res.has_value()) { + ESP_LOGE(TAG, + "ERROR INITIALIZING BME280 (CODE %d). PROCEEDING ANYWAYS", + res.error()); + } + } else { + ESP_LOGE(TAG, "ERROR INITIALIZING I2C. PROCEEDING ANYWAYS"); + } while (true) { - vTaskDelay(10 / portTICK_PERIOD_MS); + auto res = bme280::readAndProcessData(); + if (!res.has_value()) { + ESP_LOGE(TAG, "ERROR READING BME280 DATA (CODE %d)", res.error()); + } + vTaskDelay(2000 / portTICK_PERIOD_MS); } } From d46072e18e3cd1ec6b37d68ab461c65d6f9e1f4b Mon Sep 17 00:00:00 2001 From: Tomasz Klimek Date: Fri, 6 Feb 2026 19:28:00 +0100 Subject: [PATCH 3/7] Add LSM6DSO driver --- .gitmodules | 3 + boards/components/LSM6DSO/CMakeLists.txt | 2 + boards/components/LSM6DSO/lsm6dso-pid | 1 + boards/main/CMakeLists.txt | 2 +- boards/main/Kconfig.projbuild | 5 + boards/main/common.h | 3 + boards/main/lsm6dso.cpp | 183 +++++++++++++++++++++++ boards/main/lsm6dso.h | 15 ++ boards/main/main.cpp | 12 ++ 9 files changed, 225 insertions(+), 1 deletion(-) create mode 100644 boards/components/LSM6DSO/CMakeLists.txt create mode 160000 boards/components/LSM6DSO/lsm6dso-pid create mode 100644 boards/main/lsm6dso.cpp create mode 100644 boards/main/lsm6dso.h diff --git a/.gitmodules b/.gitmodules index 54af1d1..fb48ead 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,6 @@ [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 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/main/CMakeLists.txt b/boards/main/CMakeLists.txt index 207147a..c9a5f14 100755 --- a/boards/main/CMakeLists.txt +++ b/boards/main/CMakeLists.txt @@ -2,4 +2,4 @@ file(GLOB_RECURSE MAIN_SOURCES "*.cpp") idf_component_register(SRCS ${MAIN_SOURCES} INCLUDE_DIRS "." - REQUIRES driver BME280) + REQUIRES driver BME280 LSM6DSO) diff --git a/boards/main/Kconfig.projbuild b/boards/main/Kconfig.projbuild index 3d360ac..99c00dc 100644 --- a/boards/main/Kconfig.projbuild +++ b/boards/main/Kconfig.projbuild @@ -15,4 +15,9 @@ menu "Rocket board configuration" 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/common.h b/boards/main/common.h index 3fb1452..f89c654 100644 --- a/boards/main/common.h +++ b/boards/main/common.h @@ -14,6 +14,9 @@ enum class Error : uint8_t { BME280_INIT_FAILED = 0x20, BME280_READ_FAILED, + + LSM6DSO_INIT_FAILED = 0x30, + LSM6DSO_READ_FAILED, }; template diff --git a/boards/main/lsm6dso.cpp b/boards/main/lsm6dso.cpp new file mode 100644 index 0000000..0985746 --- /dev/null +++ b/boards/main/lsm6dso.cpp @@ -0,0 +1,183 @@ +#include "lsm6dso.h" + +#include +#include +#include + +#include +#include +#include + +#include "common.h" +#include "esp_log.h" +#include "i2c.h" +#include "sdkconfig.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() -> Result { + 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() -> Result { + 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{}; + + bool first = true; + + 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)); + auto data = convertToSignedShort(raw_data_buffer); + if (first) { + ESP_LOGI(TAG, "ACC: %d %d %d\n", data[0], data[1], data[2]); + first = false; + } + 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)); + auto data = convertToSignedShort(raw_data_buffer); + if (first) { + ESP_LOGI(TAG, "GYRO: %d %d %d\n", data[0], data[1], + data[2]); + } + 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..0eb0ceb --- /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() -> Result; +auto readAndProcessData() -> Result; + +} // namespace lsm6dso \ No newline at end of file diff --git a/boards/main/main.cpp b/boards/main/main.cpp index b004c47..63b3a1d 100755 --- a/boards/main/main.cpp +++ b/boards/main/main.cpp @@ -2,6 +2,7 @@ #include "esp_log.h" #include "freertos/FreeRTOS.h" #include "i2c.h" +#include "lsm6dso.h" constexpr inline const char *TAG = "ROCKET"; @@ -15,6 +16,12 @@ extern "C" void app_main(void) { "ERROR INITIALIZING BME280 (CODE %d). PROCEEDING ANYWAYS", res.error()); } + res = lsm6dso::init(); + if (!res.has_value()) { + ESP_LOGE(TAG, + "ERROR INITIALIZING LSM6DSO (CODE %d). PROCEEDING ANYWAYS", + res.error()); + } } else { ESP_LOGE(TAG, "ERROR INITIALIZING I2C. PROCEEDING ANYWAYS"); } @@ -24,6 +31,11 @@ extern "C" void app_main(void) { if (!res.has_value()) { ESP_LOGE(TAG, "ERROR READING BME280 DATA (CODE %d)", res.error()); } + res = lsm6dso::readAndProcessData(); + if (!res.has_value()) { + ESP_LOGE(TAG, "ERROR READING LSM6DSO DATA (CODE %d)", res.error()); + } + vTaskDelay(2000 / portTICK_PERIOD_MS); } } From 57c34dc766c9cefe43b0d1fdcb4b7078e7ea724d Mon Sep 17 00:00:00 2001 From: Tomasz Klimek Date: Fri, 6 Feb 2026 19:29:19 +0100 Subject: [PATCH 4/7] Fix bme280::platform_write ignoring error values --- boards/main/bme280.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/boards/main/bme280.cpp b/boards/main/bme280.cpp index 7d6a75c..d2268ac 100644 --- a/boards/main/bme280.cpp +++ b/boards/main/bme280.cpp @@ -34,10 +34,9 @@ BME280_INTF_RET_TYPE platform_read(uint8_t reg_addr, uint8_t *reg_data, BME280_INTF_RET_TYPE platform_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t length, void *intf_ptr) { - i2c::write(CONFIG_BME280_I2C_ADDR, reg_addr, reg_data, length) + return i2c::write(CONFIG_BME280_I2C_ADDR, reg_addr, reg_data, length) .transform([](Success) -> BME280_INTF_RET_TYPE { return 0; }) .value_or(-1); - return 0; } void bme280_delay_us(uint32_t period, void *intf_ptr) { usleep(period); } @@ -102,6 +101,7 @@ static auto readData() -> Result { 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( From c4ba287b94f5b7b3d260af80088f40248085b157 Mon Sep 17 00:00:00 2001 From: Tomasz Klimek Date: Fri, 6 Feb 2026 20:48:53 +0100 Subject: [PATCH 5/7] Add ultra-low-latency-ring-buffer component --- .gitmodules | 3 +++ boards/components/ring_buffer/CMakeLists.txt | 1 + boards/components/ring_buffer/ring_buffer.h | 1 + boards/components/ring_buffer/ultra-low-latency-ring-buffer | 1 + 4 files changed, 6 insertions(+) create mode 100644 boards/components/ring_buffer/CMakeLists.txt create mode 100644 boards/components/ring_buffer/ring_buffer.h create mode 160000 boards/components/ring_buffer/ultra-low-latency-ring-buffer diff --git a/.gitmodules b/.gitmodules index fb48ead..3726424 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,6 @@ [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/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 From 6c91066ab455379427cbb4278a788e60051fccb9 Mon Sep 17 00:00:00 2001 From: Tomasz Klimek Date: Fri, 6 Feb 2026 20:50:14 +0100 Subject: [PATCH 6/7] Add storage, update log messages --- boards/main/CMakeLists.txt | 2 +- boards/main/bme280.cpp | 5 +- boards/main/common.h | 2 + boards/main/lsm6dso.cpp | 21 ++---- boards/main/main.cpp | 96 ++++++++++++++++++++----- boards/main/storage.cpp | 144 +++++++++++++++++++++++++++++++++++++ boards/main/storage.h | 16 +++++ 7 files changed, 248 insertions(+), 38 deletions(-) create mode 100644 boards/main/storage.cpp create mode 100644 boards/main/storage.h diff --git a/boards/main/CMakeLists.txt b/boards/main/CMakeLists.txt index c9a5f14..a366ec2 100755 --- a/boards/main/CMakeLists.txt +++ b/boards/main/CMakeLists.txt @@ -2,4 +2,4 @@ file(GLOB_RECURSE MAIN_SOURCES "*.cpp") idf_component_register(SRCS ${MAIN_SOURCES} INCLUDE_DIRS "." - REQUIRES driver BME280 LSM6DSO) + REQUIRES driver esp_timer BME280 LSM6DSO ring_buffer) diff --git a/boards/main/bme280.cpp b/boards/main/bme280.cpp index d2268ac..cece6bb 100644 --- a/boards/main/bme280.cpp +++ b/boards/main/bme280.cpp @@ -13,6 +13,7 @@ #include "esp_log.h" #include "i2c.h" #include "sdkconfig.h" +#include "storage.h" namespace bme280 { @@ -122,9 +123,7 @@ auto readAndProcessData() -> Result { auto ret = readData(); return ret.and_then([](Data const &data) -> Result { - // storage::postBarometerData(data); - ESP_LOGI(TAG, "%f %f %f", data.air_pressure, data.humidity, - data.temperature); + storage::postBarometerData(data); return Success{}; }); } diff --git a/boards/main/common.h b/boards/main/common.h index f89c654..c909912 100644 --- a/boards/main/common.h +++ b/boards/main/common.h @@ -17,6 +17,8 @@ enum class Error : uint8_t { LSM6DSO_INIT_FAILED = 0x30, LSM6DSO_READ_FAILED, + + STORAGE_INIT_FAILED = 0x40, }; template diff --git a/boards/main/lsm6dso.cpp b/boards/main/lsm6dso.cpp index 0985746..291c00f 100644 --- a/boards/main/lsm6dso.cpp +++ b/boards/main/lsm6dso.cpp @@ -12,6 +12,7 @@ #include "esp_log.h" #include "i2c.h" #include "sdkconfig.h" +#include "storage.h" namespace lsm6dso { @@ -130,8 +131,6 @@ auto readAndProcessData() -> Result { std::array raw_data_buffer{}; - bool first = true; - while (data_count--) { lsm6dso_fifo_sensor_tag_get(&g_sensor_ctx, &tag); switch (tag) { @@ -140,13 +139,8 @@ auto readAndProcessData() -> Result { raw_data_buffer.data()) != 0) { return std::unexpected(Error::LSM6DSO_READ_FAILED); } - // storage::postAccelerationData( - // convertToSignedShort(raw_data_buffer)); - auto data = convertToSignedShort(raw_data_buffer); - if (first) { - ESP_LOGI(TAG, "ACC: %d %d %d\n", data[0], data[1], data[2]); - first = false; - } + storage::postAccelerationData( + convertToSignedShort(raw_data_buffer)); break; } case LSM6DSO_GYRO_NC_TAG: { @@ -154,13 +148,8 @@ auto readAndProcessData() -> Result { raw_data_buffer.data()) != 0) { return std::unexpected(Error::LSM6DSO_READ_FAILED); } - // storage::postAngularRateData( - // convertToSignedShort(raw_data_buffer)); - auto data = convertToSignedShort(raw_data_buffer); - if (first) { - ESP_LOGI(TAG, "GYRO: %d %d %d\n", data[0], data[1], - data[2]); - } + storage::postAngularRateData( + convertToSignedShort(raw_data_buffer)); break; } default: { diff --git a/boards/main/main.cpp b/boards/main/main.cpp index 63b3a1d..7982b6d 100755 --- a/boards/main/main.cpp +++ b/boards/main/main.cpp @@ -1,41 +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" 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"); + ESP_LOGI(TAG, "Initializing ROCKET"); if (i2c::init().has_value()) { - auto res = bme280::init(); - if (!res.has_value()) { + if (auto res = bme280::init(); !res.has_value()) { ESP_LOGE(TAG, - "ERROR INITIALIZING BME280 (CODE %d). PROCEEDING ANYWAYS", + "Initializing BME280 failed (CODE %d). Proceeding anyways", res.error()); } - res = lsm6dso::init(); - if (!res.has_value()) { - ESP_LOGE(TAG, - "ERROR INITIALIZING LSM6DSO (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, "ERROR INITIALIZING I2C. PROCEEDING ANYWAYS"); + ESP_LOGE(TAG, "Initializing I2C failed. Proceeding anyways"); } - while (true) { - auto res = bme280::readAndProcessData(); - if (!res.has_value()) { - ESP_LOGE(TAG, "ERROR READING BME280 DATA (CODE %d)", res.error()); + 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); } - res = lsm6dso::readAndProcessData(); - if (!res.has_value()) { - ESP_LOGE(TAG, "ERROR READING LSM6DSO DATA (CODE %d)", res.error()); + } + + if (!storage::init().has_value()) { + ESP_LOGE(TAG, "Initializing STORAGE failed. Proceeding anyways"); + } + + while (true) { + static int64_t last_time = 0; + if (last_time == 0) { + last_time = esp_timer_get_time(); } - vTaskDelay(2000 / portTICK_PERIOD_MS); + 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..89f93f2 --- /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() -> Result { + 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..9e86d51 --- /dev/null +++ b/boards/main/storage.h @@ -0,0 +1,16 @@ +#pragma once + +#include + +#include "bme280.h" +#include "common.h" + +namespace storage { + +auto init() -> Result; +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 From 03a7ea919d727047b5fda813885bbe7fa02cb8e7 Mon Sep 17 00:00:00 2001 From: Tomasz Klimek Date: Fri, 13 Feb 2026 18:30:51 +0100 Subject: [PATCH 7/7] Remove Result template --- boards/main/bme280.cpp | 8 ++++---- boards/main/bme280.h | 2 +- boards/main/common.h | 3 --- boards/main/i2c.cpp | 10 +++++----- boards/main/i2c.h | 6 +++--- boards/main/lsm6dso.cpp | 4 ++-- boards/main/lsm6dso.h | 4 ++-- boards/main/storage.cpp | 2 +- boards/main/storage.h | 2 +- 9 files changed, 19 insertions(+), 22 deletions(-) diff --git a/boards/main/bme280.cpp b/boards/main/bme280.cpp index cece6bb..7724be1 100644 --- a/boards/main/bme280.cpp +++ b/boards/main/bme280.cpp @@ -42,7 +42,7 @@ BME280_INTF_RET_TYPE platform_write(uint8_t reg_addr, const uint8_t *reg_data, void bme280_delay_us(uint32_t period, void *intf_ptr) { usleep(period); } -auto init() -> Result { +auto init() -> std::expected { g_sensor.intf = BME280_I2C_INTF; g_sensor.intf_ptr = nullptr; // Not used g_sensor.read = platform_read; @@ -92,7 +92,7 @@ auto init() -> Result { return Success{}; } -static auto readData() -> Result { +static auto readData() -> std::expected { Data data{}; if (g_init_error) { return std::unexpected(Error::BME280_INIT_FAILED); @@ -119,10 +119,10 @@ static auto readData() -> Result { return data; } -auto readAndProcessData() -> Result { +auto readAndProcessData() -> std::expected { auto ret = readData(); - return ret.and_then([](Data const &data) -> Result { + return ret.and_then([](Data const &data) -> std::expected { storage::postBarometerData(data); return Success{}; }); diff --git a/boards/main/bme280.h b/boards/main/bme280.h index 2ad1f3f..a47383c 100644 --- a/boards/main/bme280.h +++ b/boards/main/bme280.h @@ -12,7 +12,7 @@ struct Data { float temperature; }; -auto init() -> Result; +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 index c909912..39daca9 100644 --- a/boards/main/common.h +++ b/boards/main/common.h @@ -20,6 +20,3 @@ enum class Error : uint8_t { STORAGE_INIT_FAILED = 0x40, }; - -template -using Result = std::expected; diff --git a/boards/main/i2c.cpp b/boards/main/i2c.cpp index 952b925..cc86592 100644 --- a/boards/main/i2c.cpp +++ b/boards/main/i2c.cpp @@ -18,7 +18,7 @@ constexpr inline int FREQUENCY = 400000; constexpr inline uint32_t TIMEOUT = 50; -auto init() -> Result { +auto init() -> std::expected { i2c_config_t conf = { .mode = I2C_MODE_MASTER, .sda_io_num = CONFIG_I2C_SDA_GPIO, @@ -46,7 +46,7 @@ auto init() -> Result { } auto read(uint8_t addr, uint8_t reg, uint8_t *buffer, uint16_t size) - -> Result { + -> 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"); @@ -59,7 +59,7 @@ auto read(uint8_t addr, uint8_t reg, uint8_t *buffer, uint16_t size) static auto performWriteTransaction(uint8_t addr, uint8_t reg, const uint8_t *buffer, uint16_t size, i2c_cmd_handle_t command) - -> Result { + -> std::expected { if (i2c_master_start(command) != ESP_OK) { return std::unexpected(Error::I2C_WRITE_FAILED); } @@ -82,7 +82,7 @@ static auto performWriteTransaction(uint8_t addr, uint8_t reg, } auto write(uint8_t addr, uint8_t reg, const uint8_t *buffer, uint16_t size) - -> Result { + -> 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)); @@ -91,7 +91,7 @@ auto write(uint8_t addr, uint8_t reg, const uint8_t *buffer, uint16_t size) i2c_cmd_link_delete_static(command); - return res.or_else([](Error const &error) -> Result { + return res.or_else([](Error const &error) -> std::expected { ESP_LOGE(TAG, "Failed to write"); return std::unexpected(error); diff --git a/boards/main/i2c.h b/boards/main/i2c.h index b9521f5..13f3bf1 100644 --- a/boards/main/i2c.h +++ b/boards/main/i2c.h @@ -6,10 +6,10 @@ namespace i2c { -auto init() -> Result; +auto init() -> std::expected; auto read(uint8_t addr, uint8_t reg, uint8_t *buffer, uint16_t size) - -> Result; + -> std::expected; auto write(uint8_t addr, uint8_t reg, const uint8_t *buffer, uint16_t size) - -> Result; + -> std::expected; } // namespace i2c diff --git a/boards/main/lsm6dso.cpp b/boards/main/lsm6dso.cpp index 291c00f..5bc8c9f 100644 --- a/boards/main/lsm6dso.cpp +++ b/boards/main/lsm6dso.cpp @@ -51,7 +51,7 @@ static int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp, static void platform_delay(uint32_t ms) { usleep(ms * 1000); } -auto init() -> Result { +auto init() -> std::expected { g_sensor_ctx.write_reg = platform_write; g_sensor_ctx.read_reg = platform_read; g_sensor_ctx.mdelay = platform_delay; @@ -110,7 +110,7 @@ static std::array convertToSignedShort( return tmp; } -auto readAndProcessData() -> Result { +auto readAndProcessData() -> std::expected { lsm6dso_fifo_tag_t tag{}; uint16_t data_count = 0; diff --git a/boards/main/lsm6dso.h b/boards/main/lsm6dso.h index 0eb0ceb..090d83f 100644 --- a/boards/main/lsm6dso.h +++ b/boards/main/lsm6dso.h @@ -9,7 +9,7 @@ namespace lsm6dso { constexpr inline size_t ACCEL_BUFFER_SIZE = 256; constexpr inline size_t ANGULAR_RATE_BUFFER_SIZE = 256; -auto init() -> Result; -auto readAndProcessData() -> Result; +auto init() -> std::expected; +auto readAndProcessData() -> std::expected; } // namespace lsm6dso \ No newline at end of file diff --git a/boards/main/storage.cpp b/boards/main/storage.cpp index 89f93f2..f1a02df 100644 --- a/boards/main/storage.cpp +++ b/boards/main/storage.cpp @@ -87,7 +87,7 @@ static void flushTask(void *pvParameters) { } } -auto init() -> Result { +auto init() -> std::expected { auto sem = xSemaphoreCreateBinary(); if (sem == nullptr) { ESP_LOGE(TAG, "Semaphore creation error"); diff --git a/boards/main/storage.h b/boards/main/storage.h index 9e86d51..96392f9 100644 --- a/boards/main/storage.h +++ b/boards/main/storage.h @@ -7,7 +7,7 @@ namespace storage { -auto init() -> Result; +auto init() -> std::expected; void postBarometerData(const bme280::Data &data); void postAccelerationData(const std::array &data); void postAngularRateData(const std::array &data);