.vscode/c_cpp_properties.json

@@ -12,6 +12,7 @@
             "${workspaceFolder}/libs/mavlink-skyward-lib",
             "${workspaceFolder}/libs/miosix-kernel/miosix",
             "${workspaceFolder}/libs/miosix-kernel/miosix/arch/common",
+            "${workspaceFolder}/libs/miosix-kernel/miosix/arch/common/CMSIS/Include",
             "${workspaceFolder}/libs/mxgui",
             "${workspaceFolder}/libs/tscpp",
             "${workspaceFolder}/src/shared",
@@ -60,7 +61,7 @@
                 "${defaultIncludePaths}",
                 "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
                 "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f407vg_stm32f4discovery",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f407vg_stm32f4discovery",
+                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f407vg_stm32f4discovery"
             ]
         },
         {
@@ -338,6 +339,29 @@
                 "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM7_stm32f7/stm32f767zi_compute_unit"
             ]
         },
+        {
+            "name": "stm32f756zg_nucleo",
+            "cStandard": "c11",
+            "cppStandard": "c++14",
+            "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+            "defines": [
+                "{defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f756zg_nucleo",
+                "D_BOARD_STM32F756ZG_NUCLEO",
+                "_ARCH_CORTEXM7_STM32F7",
+                "STM32F756xx",
+                "HSE_VALUE=25000000",
+                "SYSCLK_FREQ_216MHz=216000000",
+                "__ENABLE_XRAM",
+                "V_DDA_VOLTAGE=3.3f"
+            ],
+            "includePath": [
+                "${defaultIncludePaths}",
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/common",
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/stm32f756zg_nucleo",
+                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM7_stm32f7/stm32f756zg_nucleo"
+            ]
+        },
         {
             "name": "stm32f767zi_nucleo",
             "cStandard": "c11",

.vscode/launch.json

@@ -15,6 +15,21 @@
                 "${workspaceFolder}/data/gdb/stm32f4-stlinv2.cfg"
             ]
         },
+        {
+            "cwd": "${workspaceRoot}",
+            "executable": "${workspaceFolder}/build/${fileBasenameNoExtension}",
+            "name": "F756",
+            "request": "attach",
+            "type": "cortex-debug",
+            "servertype": "openocd",
+            "device": "STM32F756ZG",
+            "svdFile": "${workspaceFolder}/data/gdb/stm32f750.svd",
+            "armToolchainPath": "/opt/arm-miosix-eabi/bin",
+            "toolchainPrefix": "arm-miosix-eabi",
+            "configFiles": [
+                "${workspaceFolder}/data/gdb/stm32f7-stlinv2.cfg"
+            ]
+        },
         {
             "cwd": "${workspaceRoot}",
             "executable": "${workspaceFolder}/build/${fileBasenameNoExtension}",

CMakeLists.txt

@@ -94,7 +94,7 @@ add_executable(test-sensormanager src/tests/test-sensormanager.cpp)
 sbs_target(test-sensormanager stm32f429zi_skyward_death_stack_x)
 
 add_executable(test-serial src/tests/test-serial.cpp)
-sbs_target(test-serial stm32f767zi_compute_unit)
+sbs_target(test-serial stm32f756zg_nucleo)
 
 add_executable(test-taskscheduler src/tests/scheduler/test-taskscheduler.cpp)
 sbs_target(test-taskscheduler stm32f407vg_stm32f4discovery)
@@ -338,11 +338,17 @@ sbs_target(test-calibration-benchmark stm32f407vg_stm32f4discovery)
 add_executable(test-calibration-stats src/tests/sensors/calibration/test-calibration-stats.cpp)
 sbs_target(test-calibration-stats stm32f407vg_stm32f4discovery)
 
-add_executable(test-bme280 src/tests/sensors/test-bme280.cpp)
-sbs_target(test-bme280 stm32f429zi_stm32f4discovery)
+add_executable(test-bme280-spi src/tests/sensors/test-bme280-spi.cpp)
+sbs_target(test-bme280-spi stm32f429zi_stm32f4discovery)
 
-add_executable(test-bmp280 src/tests/sensors/test-bmp280.cpp)
-sbs_target(test-bmp280 stm32f429zi_stm32f4discovery)
+add_executable(test-bme280-i2c src/tests/sensors/test-bme280-i2c.cpp)
+sbs_target(test-bme280-i2c stm32f429zi_stm32f4discovery)
+
+add_executable(test-bmp280-spi src/tests/sensors/test-bmp280-spi.cpp)
+sbs_target(test-bmp280-spi stm32f429zi_stm32f4discovery)
+
+add_executable(test-bmp280-i2c src/tests/sensors/test-bmp280-i2c.cpp)
+sbs_target(test-bmp280-i2c stm32f429zi_stm32f4discovery)
 
 add_executable(test-bmx160 src/tests/sensors/test-bmx160.cpp)
 sbs_target(test-bmx160 stm32f429zi_skyward_death_stack_x)
@@ -368,6 +374,9 @@ sbs_target(test-lis3mdl stm32f429zi_skyward_death_stack_x)
 add_executable(test-lis331hh src/tests/sensors/test-lis331hh.cpp)
 sbs_target(test-lis331hh stm32f205rc_skyward_ciuti)
 
+add_executable(test-lps331ap src/tests/sensors/test-lps331ap.cpp)
+sbs_target(test-lps331ap stm32f429zi_stm32f4discovery)
+
 add_executable(test-max6675 src/tests/sensors/test-max6675.cpp)
 sbs_target(test-max6675 stm32f429zi_stm32f4discovery)
 
@@ -377,8 +386,11 @@ sbs_target(test-max31855 stm32f429zi_stm32f4discovery)
 add_executable(test-mpu9250 src/tests/sensors/test-mpu9250.cpp)
 sbs_target(test-mpu9250 stm32f429zi_skyward_parafoil)
 
-add_executable(test-ms5803 src/tests/sensors/test-ms5803.cpp)
-sbs_target(test-ms5803 stm32f429zi_skyward_death_stack_x)
+add_executable(test-ms5803-spi src/tests/sensors/test-ms5803-spi.cpp)
+sbs_target(test-ms5803-spi stm32f429zi_skyward_death_stack_x)
+
+add_executable(test-ms5803-i2c src/tests/sensors/test-ms5803-i2c.cpp)
+sbs_target(test-ms5803-i2c stm32f429zi_stm32f4discovery)
 
 add_executable(test-ubxgps-serial src/tests/sensors/test-ubxgps-serial.cpp)
 sbs_target(test-ubxgps-serial stm32f429zi_skyward_death_stack_x)

cmake/boardcore.cmake

@@ -82,17 +82,21 @@ foreach(OPT_BOARD ${BOARDS})
         ${SBS_BASE}/src/shared/sensors/ADS1118/ADS1118.cpp
         ${SBS_BASE}/src/shared/sensors/ADS131M04/ADS131M04.cpp
         ${SBS_BASE}/src/shared/sensors/BME280/BME280.cpp
+        ${SBS_BASE}/src/shared/sensors/BME280/BME280I2C.cpp
         ${SBS_BASE}/src/shared/sensors/BMP280/BMP280.cpp
+        ${SBS_BASE}/src/shared/sensors/BMP280/BMP280I2C.cpp
         ${SBS_BASE}/src/shared/sensors/BMX160/BMX160.cpp
         ${SBS_BASE}/src/shared/sensors/BMX160/BMX160WithCorrection.cpp
         ${SBS_BASE}/src/shared/sensors/HX711/HX711.cpp
         ${SBS_BASE}/src/shared/sensors/LIS3MDL/LIS3MDL.cpp
         ${SBS_BASE}/src/shared/sensors/LIS331HH/LIS331HH.cpp
+        ${SBS_BASE}/src/shared/sensors/LPS331AP/LPS331AP.cpp
         ${SBS_BASE}/src/shared/sensors/MAX6675/MAX6675.cpp
         ${SBS_BASE}/src/shared/sensors/MAX31855/MAX31855.cpp
         ${SBS_BASE}/src/shared/sensors/MBLoadCell/MBLoadCell.cpp
         ${SBS_BASE}/src/shared/sensors/MPU9250/MPU9250.cpp
         ${SBS_BASE}/src/shared/sensors/MS5803/MS5803.cpp
+        ${SBS_BASE}/src/shared/sensors/MS5803/MS5803I2C.cpp
         ${SBS_BASE}/src/shared/sensors/SensorManager.cpp
         ${SBS_BASE}/src/shared/sensors/SensorSampler.cpp
         ${SBS_BASE}/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp

data/gdb/stm32f7-stlinv2.cfg

+# This is an STM32F7 discovery board with a single STM32F407VGT6 chip.
+# http://www.st.com/internet/evalboard/product/252419.jsp
+
+source [find interface/stlink-v2-1.cfg]
+
+source [find target/stm32f7x.cfg]
+
+# use hardware reset, connect under reset
+reset_config srst_only srst_nogate

miosix-kernel @ 58286bd3

-Subproject commit ca9c636f88fb58243847d9dca9cc1c000dcadeb5
+Subproject commit 58286bd3dd404764346b77737ac589189af90e60

src/shared/drivers/adc/InternalADC.cpp

@@ -36,7 +36,7 @@ namespace Boardcore
 static const float CAL_PT1_TEMP = 30;
 static const float CAL_PT2_TEMP = 110;
 static const float CAL_V_DDA    = 3.3f;
-#elif defined(STM32F767xx) || defined(STM32F769xx)
+#elif defined(STM32F767xx) || defined(STM32F769xx) || defined(STM32F756xx)
 #define CAL_PT1_VALUE ((uint16_t volatile *)((uint32_t)0x1FF0F44C))
 #define CAL_PT2_VALUE ((uint16_t volatile *)((uint32_t)0x1FF0F44E))
 static const float CAL_PT1_TEMP           = 30;
@@ -50,7 +50,8 @@ static const float CAL_V_DDA              = 3.3f;
 static const InternalADC::Channel TEMP_CH = InternalADC::CH16;
 static const InternalADC::Channel VBAT_CH = InternalADC::CH18;
 static const float VBAT_DIV               = 2.0f;
-#elif defined(STM32F429xx) || defined(STM32F767xx) || defined(STM32F769xx)
+#elif defined(STM32F429xx) || defined(STM32F767xx) || defined(STM32F769xx) || \
+    defined(STM32F756xx)
 static const InternalADC::Channel TEMP_CH = InternalADC::CH18;
 static const InternalADC::Channel VBAT_CH = InternalADC::CH18;
 static const float VBAT_DIV               = 4.0f;

src/shared/drivers/i2c/I2C.h

@@ -20,7 +20,10 @@
  * THE SOFTWARE.
  */
 
+#pragma once
+
 #include "I2CDriver.h"
+
 namespace Boardcore
 {
 

src/shared/drivers/i2c/I2CDriver-f7.cpp

@@ -74,6 +74,17 @@ I2CTimings calculateTimings(uint32_t f, uint32_t fi2c)
     // for SCLDEL and SDADEL
     uint32_t temp_presc = f / (64 * fi2c);
 
+#if defined(_BOARD_STM32F756ZG_NUCLEO)
+    const uint16_t correction = 10;
+#elif defined(_BOARD_STM32F767ZI_COMPUTE_UNIT) || \
+    defined(_BOARD_STM32F767ZI_NUCLEO)
+    const uint16_t correction = 7;
+#else
+    const uint16_t correction = 0;
+#warning \
+    "I2C Timings not corrected, actual frequency could be lower than nominal"
+#endif
+
     // presc is 4 bit long, so avoiding overflow
     if (temp_presc >= 16)
     {
@@ -93,7 +104,7 @@ I2CTimings calculateTimings(uint32_t f, uint32_t fi2c)
     // distributing the correction on SCLL and SCLH
     i2cTimings.sclh = i2cTimings.scll =
         (f / (fi2c * 2 * (i2cTimings.presc + 1)) - 1) -
-        (7 / (i2cTimings.presc + 1));
+        (correction / (i2cTimings.presc + 1));
 
     // SCLDEL >= (t_r + t_su) / ((PRESC+1)*t_i2c) - 1 ; approximated without
     // subtracting 1. scldly and sdadly are calculated using values taken from

src/shared/sensors/BME280/BME280.cpp

@@ -54,64 +54,39 @@ BME280::BME280(SPISlave spiSlave, BME280Config config)
 
 bool BME280::init()
 {
-    // Check WHO AM I
     if (!checkWhoAmI())
     {
         LOG_ERR(logger, "Invalid WHO AM I");
 
         lastError = SensorErrors::INVALID_WHOAMI;
-
         return false;
     }
 
+    reset();
+    miosix::Thread::sleep(3);
+
     loadCompensationParameters();
 
-    // Set the configuration 10 times to be sure
-    for (int i = 0; i < 10; i++)
-    {
     // Read once the temperature to compute fineTemperature
     setConfiguration(BME280_CONFIG_TEMP_SINGLE);
     miosix::Thread::sleep(
         calculateMaxMeasurementTime(BME280_CONFIG_TEMP_SINGLE));
     readTemperature();
 
+    // Set the target configuration
     setConfiguration();
-    }
-
-    // Set a sleep time to allow the sensor to change internally the data
-    miosix::Thread::sleep(100);
-
-    // I create the config state which represents the logic or of all the
-    // 5 readConfiguration controls (We perform 5 checks to avoid that the
-    // sensor is busy implicating in wrong responses)
-    bool readConfigResult = false;
-    BME280Config readBackConfig;
-
-    for (int i = 0; i < 10; i++)
-    {
-        readBackConfig = readConfiguration();
-        // Check if the configration on the device matches ours
-        if (config.bytes.ctrlHumidity == readBackConfig.bytes.ctrlHumidity &&
-            config.bytes.ctrlPressureAndTemperature ==
-                readBackConfig.bytes.ctrlPressureAndTemperature &&
-            config.bytes.config == readBackConfig.bytes.config)
-        {
-            readConfigResult = true;
-            break;
-        }
 
-        // After the check i sleep 100 milliseconds
-        miosix::Thread::sleep(20);
-    }
+    BME280Config readBackConfig = readConfiguration();
 
-    //   If after the 5 iterations the sensor didn't report the configuration
-    //   set I can report the init error
-    if (!readConfigResult)
+    // Check if the configuration on the device matches ours
+    if (config.bytes.ctrlHumidity != readBackConfig.bytes.ctrlHumidity ||
+        config.bytes.ctrlPressureAndTemperature !=
+            readBackConfig.bytes.ctrlPressureAndTemperature ||
+        config.bytes.config != readBackConfig.bytes.config)
     {
-        LOG_ERR(logger, "Device configuration incorrect, setup failed");
+        LOG_ERR(logger, "Device configuration incorrect, setup failed.");
 
         lastError = SensorErrors::NOT_INIT;
-
         return false;
     }
 
@@ -163,77 +138,72 @@ void BME280::setStandbyTime(StandbyTime standbyTime)
 HumidityData BME280::readHumidity()
 {
     uint8_t buffer[2];
-    int32_t adc_H = 0;
-
     {
         SPITransaction transaction(spiSlave);
 
         transaction.readRegisters(REG_HUM_MSB, buffer, 2);
     }
 
-    adc_H |= ((uint32_t)buffer[0] << 8);
+    int32_t adc_H = ((uint32_t)buffer[0] << 8);
     adc_H |= buffer[1];
 
-    // Compensate humidity
-    lastSample.humidityTimestamp = TimestampTimer::getTimestamp();
-    lastSample.humidity =
-        (float)compensateHumidity(adc_H) / 1024;  // Converto to %RH
+    HumidityData data;
+    data.humidityTimestamp = TimestampTimer::getTimestamp();
+    data.humidity          = compensateHumidity(adc_H);
+    data.humidity /= 1024;  // Convert to to %RH
 
-    return lastSample;
+    return data;
 }
 
 PressureData BME280::readPressure()
 {
     uint8_t buffer[3];
-    int32_t adc_P = 0;
-
     {
         SPITransaction transaction(spiSlave);
 
         transaction.readRegisters(REG_PRESS_MSB, buffer, 3);
     }
 
-    adc_P |= ((uint32_t)buffer[0]) << 12;
+    int32_t adc_P = ((uint32_t)buffer[0]) << 12;
     adc_P |= ((uint32_t)buffer[1]) << 4;
     adc_P |= (buffer[2] >> 4) & 0x0F;
 
-    // Compensate pressure
-    lastSample.pressureTimestamp = TimestampTimer::getTimestamp();
-    lastSample.pressure =
-        (float)compensatePressure(adc_P) / 256;  // Convert to Pa
+    PressureData data;
+    data.pressureTimestamp = TimestampTimer::getTimestamp();
+    data.pressure          = compensatePressure(adc_P);
+    data.pressure /= 256;  // Convert to Pa
 
-    return lastSample;
+    return data;
 }
 
 TemperatureData BME280::readTemperature()
 {
     uint8_t buffer[3];
-    int32_t adcTemperature = 0;
-
     {
         SPITransaction transaction(spiSlave);
 
         transaction.readRegisters(REG_TEMP_MSB, buffer, 3);
     }
 
-    adcTemperature |= ((uint32_t)buffer[0]) << 12;
+    int32_t adcTemperature = ((uint32_t)buffer[0]) << 12;
     adcTemperature |= ((uint32_t)buffer[1]) << 4;
     adcTemperature |= (buffer[2] >> 4) & 0x0F;
 
-    // Compensate temperature
     fineTemperature = computeFineTemperature(adcTemperature);
-    lastSample.temperatureTimestamp = TimestampTimer::getTimestamp();
-    lastSample.temperature = (float)compensateTemperature(fineTemperature) /
-                             100;  // Converto to DegC
 
-    return lastSample;
+    TemperatureData data;
+    data.temperatureTimestamp = TimestampTimer::getTimestamp();
+    data.temperature          = compensateTemperature(fineTemperature);
+    data.temperature /= 100;  // Convert to to DegC
+
+    return data;
 }
 
-unsigned int BME280::calculateMaxMeasurementTime(BME280Config config_)
+unsigned int BME280::calculateMaxMeasurementTime(BME280Config config)
 {
-    return ceil(1.25 + (2.3 * config_.bits.oversamplingTemperature) +
-                (2.3 * config_.bits.oversamplingPressure + 0.575) +
-                (2.3 * config_.bits.oversamplingHumidity + 0.575));
+    return ceil(1.25 + (2.3 * config.bits.oversamplingTemperature) +
+                (2.3 * config.bits.oversamplingPressure + 0.575) +
+                (2.3 * config.bits.oversamplingHumidity + 0.575));
 }
 
 unsigned int BME280::getMaxMeasurementTime()
@@ -245,49 +215,54 @@ bool BME280::selfTest() { return checkWhoAmI(); }
 
 BME280Data BME280::sampleImpl()
 {
-    uint8_t buffer[8];
-    int32_t adcTemperature = 0;
-    int32_t adc_P          = 0;
-    int32_t adc_H          = 0;
-    BME280Data data;
-
     // TODO: implement selective read!
 
-    // Burst read pressure, temperature and humidity
+    uint8_t buffer[8];
     {
         SPITransaction transaction(spiSlave);
 
         transaction.readRegisters(REG_PRESS_MSB, buffer, 8);
     }
 
-    adcTemperature |= ((uint32_t)buffer[3]) << 12;
+    BME280Data data;
+
+    int32_t adcTemperature = ((uint32_t)buffer[3]) << 12;
     adcTemperature |= ((uint32_t)buffer[4]) << 4;
     adcTemperature |= (buffer[5] >> 4) & 0x0F;
 
-    adc_P |= ((uint32_t)buffer[0]) << 12;
+    int32_t adc_P = ((uint32_t)buffer[0]) << 12;
     adc_P |= ((uint32_t)buffer[1]) << 4;
     adc_P |= (buffer[2] >> 4) & 0x0F;
 
-    adc_H |= ((uint32_t)buffer[6] << 8);
+    int32_t adc_H = ((uint32_t)buffer[6] << 8);
     adc_H |= buffer[7];
 
     // Compensate temperature
     fineTemperature           = computeFineTemperature(adcTemperature);
     data.temperatureTimestamp = TimestampTimer::getTimestamp();
-    data.temperature          = (float)compensateTemperature(fineTemperature) /
-                       100;  // Converto to DegC
+    data.temperature          = compensateTemperature(fineTemperature);
+    data.temperature /= 100;  // Convert to to DegC
 
     // Compensate pressure
     data.pressureTimestamp = TimestampTimer::getTimestamp();
-    data.pressure = (float)compensatePressure(adc_P) / 256;  // Convert to Pa
+    data.pressure          = compensatePressure(adc_P);
+    data.pressure /= 256;  // Convert to Pa
 
     // Compensate humidity
     data.humidityTimestamp = TimestampTimer::getTimestamp();
-    data.humidity = (float)compensateHumidity(adc_H) / 1024;  // Converto to %RH
+    data.humidity          = compensateHumidity(adc_H);
+    data.humidity /= 1024;  // Convert to to %RH
 
     return data;
 }
 
+void BME280::reset()
+{
+    SPITransaction transaction(spiSlave);
+
+    transaction.writeRegister(REG_RESET, 0xB6);
+}
+
 bool BME280::checkWhoAmI()
 {
     SPITransaction transaction(spiSlave);
@@ -299,14 +274,14 @@ bool BME280::checkWhoAmI()
 
 void BME280::setConfiguration() { setConfiguration(config); }
 
-void BME280::setConfiguration(BME280Config config_)
+void BME280::setConfiguration(BME280Config config)
 {
     SPITransaction transaction(spiSlave);
 
-    transaction.writeRegister(REG_CONFIG & 0x7F, config_.bytes.config);
-    transaction.writeRegister(REG_CTRL_HUM & 0x7F, config_.bytes.ctrlHumidity);
-    transaction.writeRegister(REG_CTRL_MEAS & 0x7F,
-                              config_.bytes.ctrlPressureAndTemperature);
+    transaction.writeRegister(REG_CONFIG, config.bytes.config);
+    transaction.writeRegister(REG_CTRL_HUM, config.bytes.ctrlHumidity);
+    transaction.writeRegister(REG_CTRL_MEAS,
+                              config.bytes.ctrlPressureAndTemperature);
 }
 
 BME280::BME280Config BME280::readConfiguration()
@@ -328,7 +303,7 @@ void BME280::loadCompensationParameters()
         transaction.readRegisters(REG_CALIB_0, (uint8_t *)&compParams, 25);
     }
 
-    // Reat second batch of compensation parameters
+    // Read second batch of compensation parameters
     {
         SPITransaction transaction(spiSlave);
 

src/shared/sensors/BME280/BME280.h

@@ -148,12 +148,14 @@ public:
 
     static constexpr uint8_t REG_ID_VAL = 0x60;  ///< Who am I value
 
-    static const BME280Config
-        BME280_DEFAULT_CONFIG;  ///< Default register values
-    static const BME280Config
-        BME280_CONFIG_ALL_ENABLED;  ///< Datasheet values for indoor navigation
-    static const BME280Config
-        BME280_CONFIG_TEMP_SINGLE;  ///< Temperature enabled in forced mode
+    ///< Default register values
+    static const BME280Config BME280_DEFAULT_CONFIG;
+
+    ///< Datasheet values for indoor navigation
+    static const BME280Config BME280_CONFIG_ALL_ENABLED;
+
+    ///< Temperature enabled in forced mode
+    static const BME280Config BME280_CONFIG_TEMP_SINGLE;
 
     explicit BME280(SPISlave spiSlave,
                     BME280Config config = BME280_CONFIG_ALL_ENABLED);
@@ -225,7 +227,7 @@ public:
      *
      * @return Time in milliseconds
      */
-    static unsigned int calculateMaxMeasurementTime(BME280Config config_);
+    static unsigned int calculateMaxMeasurementTime(BME280Config config);
 
     unsigned int getMaxMeasurementTime();
 
@@ -239,9 +241,11 @@ public:
 private:
     BME280Data sampleImpl() override;
 
+    void reset();
+
     void setConfiguration();
 
-    void setConfiguration(BME280Config config_);
+    void setConfiguration(BME280Config config);
 
     BME280Config readConfiguration();
 
@@ -269,25 +273,25 @@ private:
         REG_CALIB_0 = 0x88,
         // Calibration register 1-25
 
-        REG_ID    = 0xD0,
-        REG_RESET = 0xE0,
+        REG_ID    = 0x50,
+        REG_RESET = 0x60,
 
         REG_CALIB_26 = 0xE1,
         // Calibration register 27-41
 
-        REG_CTRL_HUM  = 0xF2,
-        REG_STATUS    = 0xF3,
-        REG_CTRL_MEAS = 0xF4,
-        REG_CONFIG    = 0xF5,
-
-        REG_PRESS_MSB  = 0xF7,
-        REG_PRESS_LSB  = 0xF8,
-        REG_PRESS_XLSB = 0xF9,
-        REG_TEMP_MSB   = 0xFA,
-        REG_TEMP_LSB   = 0xFB,
-        REG_TEMP_XLSB  = 0xFC,
-        REG_HUM_MSB    = 0xFD,
-        REG_HUM_LSB    = 0xFE,
+        REG_CTRL_HUM  = 0x72,
+        REG_STATUS    = 0x73,
+        REG_CTRL_MEAS = 0x74,
+        REG_CONFIG    = 0x75,
+
+        REG_PRESS_MSB  = 0x77,
+        REG_PRESS_LSB  = 0x78,
+        REG_PRESS_XLSB = 0x79,
+        REG_TEMP_MSB   = 0x7A,
+        REG_TEMP_LSB   = 0x7B,
+        REG_TEMP_XLSB  = 0x7C,
+        REG_HUM_MSB    = 0x7D,
+        REG_HUM_LSB    = 0x7E,
     };
 
     const SPISlave spiSlave;

src/shared/sensors/BME280/BME280I2C.cpp

+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "BME280I2C.h"
+
+#include <drivers/timer/TimestampTimer.h>
+#include <math.h>
+
+using namespace std;
+
+namespace Boardcore
+{
+
+const BME280I2C::BME280Config BME280I2C::BME280_DEFAULT_CONFIG = {
+    SKIPPED, 0, 0, SLEEP_MODE, SKIPPED, SKIPPED, 0, FILTER_OFF, STB_TIME_0_5};
+
+const BME280I2C::BME280Config BME280I2C::BME280_CONFIG_ALL_ENABLED = {
+    OVERSAMPLING_1,
+    0,
+    0,
+    NORMAL_MODE,
+    OVERSAMPLING_16,
+    OVERSAMPLING_2,
+    0,
+    FILTER_COEFF_16,
+    STB_TIME_0_5};
+
+const BME280I2C::BME280Config BME280I2C::BME280_CONFIG_TEMP_SINGLE = {
+    SKIPPED,        0, 0,          FORCED_MODE, SKIPPED,
+    OVERSAMPLING_1, 0, FILTER_OFF, STB_TIME_0_5};
+
+BME280I2C::BME280I2C(I2C &bus, BME280Config config) : bus(bus), config(config)
+{
+}
+
+bool BME280I2C::init()
+{
+    if (!checkWhoAmI())
+    {
+        LOG_ERR(logger, "Invalid WHO AM I");
+
+        lastError = SensorErrors::INVALID_WHOAMI;
+        return false;
+    }
+
+    if (!reset())
+    {
+        return false;
+    }
+    miosix::Thread::sleep(3);
+
+    loadCompensationParameters();
+
+    // Read once the temperature to compute fineTemperature
+    setConfiguration(BME280_CONFIG_TEMP_SINGLE);
+    miosix::Thread::sleep(
+        calculateMaxMeasurementTime(BME280_CONFIG_TEMP_SINGLE));
+    readTemperature();
+
+    // Set the target configuration
+    setConfiguration();
+
+    BME280Config readBackConfig = readConfiguration();
+
+    // Check if the configuration on the device matches ours
+    if (config.bytes.ctrlHumidity != readBackConfig.bytes.ctrlHumidity ||
+        config.bytes.ctrlPressureAndTemperature !=
+            readBackConfig.bytes.ctrlPressureAndTemperature ||
+        config.bytes.config != readBackConfig.bytes.config)
+    {
+        LOG_ERR(logger, "Device configuration incorrect, setup failed.");
+
+        lastError = SensorErrors::NOT_INIT;
+        return false;
+    }
+
+    return true;
+}
+
+void BME280I2C::setSensorMode(Mode mode)
+{
+    config.bits.mode = mode;
+
+    setConfiguration();
+}
+
+void BME280I2C::setHumidityOversampling(Oversampling oversampling)
+{
+    config.bits.oversamplingHumidity = oversampling;
+
+    setConfiguration();
+}
+
+void BME280I2C::setPressureOversampling(Oversampling oversampling)
+{
+    config.bits.oversamplingPressure = oversampling;
+
+    setConfiguration();
+}
+
+void BME280I2C::setTemperatureOversampling(Oversampling oversampling)
+{
+    config.bits.oversamplingTemperature = oversampling;
+
+    setConfiguration();
+}
+
+void BME280I2C::setFilterCoeff(FilterCoeff filterCoeff)
+{
+    config.bits.filter = filterCoeff;
+
+    setConfiguration();
+}
+
+void BME280I2C::setStandbyTime(StandbyTime standbyTime)
+{
+    config.bits.standbyTime = standbyTime;
+
+    setConfiguration();
+}
+
+HumidityData BME280I2C::readHumidity()
+{
+    uint8_t buffer[2];
+    if (bus.readFromRegister(slaveConfig, REG_HUM_MSB, buffer, 2))
+    {
+
+        int32_t adc_H = ((uint32_t)buffer[0] << 8);
+        adc_H |= buffer[1];
+
+        HumidityData data;
+        data.humidityTimestamp = TimestampTimer::getTimestamp();
+        data.humidity          = compensateHumidity(adc_H);
+        data.humidity /= 1024;  // Convert to to %RH
+
+        return data;
+    }
+    else
+    {
+        lastError = SensorErrors::BUS_FAULT;
+        return lastSample;
+    }
+}
+
+PressureData BME280I2C::readPressure()
+{
+    uint8_t buffer[3];
+    if (bus.readFromRegister(slaveConfig, REG_PRESS_MSB, buffer, 3))
+    {
+
+        int32_t adc_P = ((uint32_t)buffer[0]) << 12;
+        adc_P |= ((uint32_t)buffer[1]) << 4;
+        adc_P |= (buffer[2] >> 4) & 0x0F;
+
+        PressureData data;
+        data.pressureTimestamp = TimestampTimer::getTimestamp();
+        data.pressure          = compensatePressure(adc_P);
+        data.pressure /= 256;  // Convert to Pa
+
+        return data;
+    }
+    else
+    {
+        lastError = SensorErrors::BUS_FAULT;
+        return lastSample;
+    }
+}
+
+TemperatureData BME280I2C::readTemperature()
+{
+    uint8_t buffer[3];
+    if (bus.readFromRegister(slaveConfig, REG_TEMP_MSB, buffer, 3))
+    {
+        int32_t adcTemperature = ((uint32_t)buffer[0]) << 12;
+        adcTemperature |= ((uint32_t)buffer[1]) << 4;
+        adcTemperature |= (buffer[2] >> 4) & 0x0F;
+
+        fineTemperature = computeFineTemperature(adcTemperature);
+
+        TemperatureData data;
+        data.temperatureTimestamp = TimestampTimer::getTimestamp();
+        data.temperature          = compensateTemperature(fineTemperature);
+        data.temperature /= 100;  // Convert to to DegC
+
+        return data;
+    }
+    else
+    {
+        lastError = SensorErrors::BUS_FAULT;
+        return lastSample;
+    }
+}
+
+unsigned int BME280I2C::calculateMaxMeasurementTime(BME280Config config)
+{
+    return ceil(1.25 + (2.3 * config.bits.oversamplingTemperature) +
+                (2.3 * config.bits.oversamplingPressure + 0.575) +
+                (2.3 * config.bits.oversamplingHumidity + 0.575));
+}
+
+unsigned int BME280I2C::getMaxMeasurementTime()
+{
+    return calculateMaxMeasurementTime(config);
+}
+
+bool BME280I2C::selfTest() { return checkWhoAmI(); }
+
+BME280Data BME280I2C::sampleImpl()
+{
+    // TODO: implement selective read!
+
+    uint8_t buffer[8];
+    if (bus.readFromRegister(slaveConfig, REG_PRESS_MSB, buffer, 8))
+    {
+        BME280Data data;
+
+        int32_t adcTemperature = ((uint32_t)buffer[3]) << 12;
+        adcTemperature |= ((uint32_t)buffer[4]) << 4;
+        adcTemperature |= (buffer[5] >> 4) & 0x0F;
+
+        int32_t adc_P = ((uint32_t)buffer[0]) << 12;
+        adc_P |= ((uint32_t)buffer[1]) << 4;
+        adc_P |= (buffer[2] >> 4) & 0x0F;
+
+        int32_t adc_H = ((uint32_t)buffer[6] << 8);
+        adc_H |= buffer[7];
+
+        // Compensate temperature
+        fineTemperature           = computeFineTemperature(adcTemperature);
+        data.temperatureTimestamp = TimestampTimer::getTimestamp();
+        data.temperature          = compensateTemperature(fineTemperature);
+        data.temperature /= 100;  // Convert to to DegC
+
+        // Compensate pressure
+        data.pressureTimestamp = TimestampTimer::getTimestamp();
+        data.pressure          = compensatePressure(adc_P);
+        data.pressure /= 256;  // Convert to Pa
+
+        // Compensate humidity
+        data.humidityTimestamp = TimestampTimer::getTimestamp();
+        data.humidity          = compensateHumidity(adc_H);
+        data.humidity /= 1024;  // Convert to to %RH
+
+        return data;
+    }
+    else
+    {
+        lastError = SensorErrors::BUS_FAULT;
+        return lastSample;
+    }
+}
+
+bool BME280I2C::reset()
+{
+    if (!bus.writeRegister(slaveConfig, REG_RESET, 0xB6))
+    {
+        lastError = SensorErrors::BUS_FAULT;
+        return false;
+    }
+
+    return true;
+}
+
+bool BME280I2C::checkWhoAmI()
+{
+    uint8_t whoAmIValue;
+
+    if (bus.readRegister(slaveConfig, REG_ID, whoAmIValue))
+    {
+
+        return whoAmIValue == REG_ID_VAL;
+    }
+    else
+    {
+        lastError = SensorErrors::BUS_FAULT;
+        return false;
+    }
+}
+
+void BME280I2C::setConfiguration() { setConfiguration(config); }
+
+void BME280I2C::setConfiguration(BME280Config config)
+{
+    if (!bus.writeRegister(slaveConfig, REG_CONFIG, config.bytes.config))
+    {
+        LOG_ERR(logger, "Error while writing to register REG_CONFIG");
+        return;
+    }
+
+    if (!bus.writeRegister(slaveConfig, REG_CTRL_HUM,
+                           config.bytes.ctrlHumidity))
+    {
+        LOG_ERR(logger, "Error while writing to register REG_CTRL_HUM");
+        return;
+    }
+
+    if (!bus.writeRegister(slaveConfig, REG_CTRL_MEAS,
+                           config.bytes.ctrlPressureAndTemperature))
+    {
+        LOG_ERR(logger, "Error while writing to register REG_CTRL_MEAS");
+        return;
+    }
+}
+
+BME280I2C::BME280Config BME280I2C::readConfiguration()
+{
+    BME280Config tmp;
+
+    if (bus.readFromRegister(slaveConfig, REG_CTRL_HUM, (uint8_t *)&tmp, 4))
+    {
+        return tmp;
+    }
+    else
+    {
+        lastError = SensorErrors::BUS_FAULT;
+        return BME280_DEFAULT_CONFIG;
+    }
+}
+
+void BME280I2C::loadCompensationParameters()
+{
+    // Read first batch of compensation parameters
+    if (!bus.readFromRegister(slaveConfig, REG_CALIB_0, (uint8_t *)&compParams,
+                              25))
+    {
+        lastError = SensorErrors::BUS_FAULT;
+        return;
+    }
+
+    // Read second batch of compensation parameters
+    if (!bus.readFromRegister(slaveConfig, REG_CALIB_26,
+                              (uint8_t *)&compParams.bits.dig_H2, 7))
+    {
+        lastError = SensorErrors::BUS_FAULT;
+        return;
+    }
+
+    // Adjust unaligned data
+    compParams.bytesArray[29] =
+        (compParams.bytesArray[29] << 4) | (compParams.bytesArray[29] >> 4);
+    compParams.bits.dig_H4 =
+        (compParams.bits.dig_H4 << 4) | (compParams.bits.dig_H4 >> 8);
+}
+
+int32_t BME280I2C::computeFineTemperature(int32_t adcTemperature)
+{
+    int32_t var1, var2;
+    var1 = ((((adcTemperature >> 3) - ((int32_t)compParams.bits.dig_T1 << 1))) *
+            ((int32_t)compParams.bits.dig_T2)) >>
+           11;
+    var2 = (((((adcTemperature >> 4) - ((int32_t)compParams.bits.dig_T1)) *
+              ((adcTemperature >> 4) - ((int32_t)compParams.bits.dig_T1))) >>
+             12) *
+            ((int32_t)compParams.bits.dig_T3)) >>
+           14;
+    return var1 + var2;
+}
+
+int32_t BME280I2C::compensateTemperature(int32_t fineTemperature)
+{
+    return (fineTemperature * 5 + 128) >> 8;
+}
+
+uint32_t BME280I2C::compensatePressure(int32_t adc_P)
+{
+    int64_t var1, var2, p;
+    var1 = ((int64_t)fineTemperature) - 128000;
+    var2 = var1 * var1 * (int64_t)compParams.bits.dig_P6;
+    var2 = var2 + ((var1 * (int64_t)compParams.bits.dig_P5) << 17);
+    var2 = var2 + (((int64_t)compParams.bits.dig_P4) << 35);
+    var1 = ((var1 * var1 * (int64_t)compParams.bits.dig_P3) >> 8) +
+           ((var1 * ((int64_t)compParams.bits.dig_P2) << 12));
+    var1 =
+        ((((int64_t)1) << 47) + var1) * ((int64_t)compParams.bits.dig_P1) >> 33;
+    if (var1 == 0)
+    {
+        return 0;  // avoid exception caused by division by zero
+    }
+    p    = 1048576 - adc_P;
+    p    = (((p << 31) - var2) * 3125) / var1;
+    var1 = (((int64_t)compParams.bits.dig_P9) * (p >> 13) * (p >> 13)) >> 25;
+    var2 = (((int64_t)compParams.bits.dig_P8) * p) >> 19;
+    p    = ((p + var1 + var2) >> 8) + (((int64_t)compParams.bits.dig_P7) << 4);
+    return (uint32_t)p;
+}
+
+uint32_t BME280I2C::compensateHumidity(int32_t adc_H)
+{
+    int32_t v_x1_u32r;
+
+    v_x1_u32r = (fineTemperature - ((int32_t)768000));
+    v_x1_u32r = (((((adc_H << 14) - (((int32_t)compParams.bits.dig_H4) << 20) -
+                    (((int32_t)compParams.bits.dig_H5) * v_x1_u32r)) +
+                   ((int32_t)16384)) >>
+                  15) *
+                 (((((((v_x1_u32r * ((int32_t)compParams.bits.dig_H6)) >> 10) *
+                      (((v_x1_u32r * ((int32_t)compParams.bits.dig_H3)) >> 11) +
+                       ((int32_t)32768))) >>
+                     10) +
+                    ((int32_t)2097152)) *
+                       ((int32_t)compParams.bits.dig_H2) +
+                   8192) >>
+                  14));
+    v_x1_u32r = (v_x1_u32r - (((((v_x1_u32r >> 15) * (v_x1_u32r >> 15)) >> 7) *
+                               ((int32_t)compParams.bits.dig_H1)) >>
+                              4));
+    v_x1_u32r = (v_x1_u32r < 0 ? 0 : v_x1_u32r);
+    v_x1_u32r = (v_x1_u32r > 419430400 ? 419430400 : v_x1_u32r);
+    return (uint32_t)(v_x1_u32r >> 12);
+}
+
+}  // namespace Boardcore

src/shared/sensors/BME280/BME280I2C.h

+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <diagnostic/PrintLogger.h>
+#include <drivers/i2c/I2C.h>
+#include <sensors/Sensor.h>
+
+#include "BME280Data.h"
+
+namespace Boardcore
+{
+
+class BME280I2C : public Sensor<BME280Data>
+{
+public:
+    enum Oversampling
+    {
+        SKIPPED         = 0x0,  ///< Skipped (output set to 0x8000)
+        OVERSAMPLING_1  = 0x1,  ///< Oversampling x1
+        OVERSAMPLING_2  = 0x2,  ///< Oversampling x2
+        OVERSAMPLING_4  = 0x3,  ///< Oversampling x4
+        OVERSAMPLING_8  = 0x4,  ///< Oversampling x8
+        OVERSAMPLING_16 = 0x5,  ///< Oversampling x16
+    };
+
+    enum Mode
+    {
+        SLEEP_MODE  = 0x0,  ///< Sleep mode
+        FORCED_MODE = 0x1,  ///< Forced mode
+        NORMAL_MODE = 0x3   ///< Normal mode
+    };
+
+    enum StandbyTime
+    {
+        STB_TIME_0_5  = 0x0,  ///< 0.5 ms
+        STB_TIME_62_5 = 0x1,  ///< 62.5 ms
+        STB_TIME_125  = 0x2,  ///< 125 ms
+        STB_TIME_250  = 0x3,  ///< 250 ms
+        STB_TIME_500  = 0x4,  ///< 500 ms
+        STB_TIME_1000 = 0x5,  ///< 1000 ms
+        STB_TIME_10   = 0x6,  ///< 10 ms
+        STB_TIME_20   = 0x7   ///< 20 ms
+    };
+
+    enum FilterCoeff
+    {
+        FILTER_OFF      = 0x0,  ///< Filter off
+        FILTER_COEFF_2  = 0x1,  ///< Filter coefficient = 2
+        FILTER_COEFF_4  = 0x2,  ///< Filter coefficient = 4
+        FILTER_COEFF_8  = 0x3,  ///< Filter coefficient = 8
+        FILTER_COEFF_16 = 0x4   ///< Filter coefficient = 16
+    };
+
+    union BME280Config
+    {
+        struct __attribute__((packed)) BME280ConfigBits
+        {
+            Oversampling
+                oversamplingHumidity : 3;  ///< Oversampling of humidity
+            uint8_t : 5;
+
+            // status
+            /**
+             * '1' when the NVM data are being copied to image registers, '0'
+             * when the copying is done
+             */
+            uint8_t imUpdate : 1;
+            uint8_t : 2;
+            /**
+             * '1' whenever a conversion is running, '0' when the result have
+             * been transferred to the data registers
+             */
+            uint8_t measuring : 1;
+            uint8_t : 4;
+
+            Mode mode : 2;  ///< Device modes
+            Oversampling
+                oversamplingPressure : 3;  ///< Oversampling of pressure
+            Oversampling
+                oversamplingTemperature : 3;  ///< Oversampling of temperature
+
+            // config
+            uint8_t spi3wEn : 1;  ///< Enables 3-wire SPI interface
+            uint8_t : 1;
+            FilterCoeff filter : 3;       ///< Time constant of the IIR filter
+            StandbyTime standbyTime : 3;  ///< Inactive duration in normal mode
+        } bits;
+
+        struct
+        {
+            uint8_t ctrlHumidity;                ///< Humidity options
+            uint8_t status;                      ///< Device status
+            uint8_t ctrlPressureAndTemperature;  ///< Pressure and temperature
+                                                 ///< options
+            uint8_t config;  ///< Rate, filter and interface options
+        } bytes;
+
+        uint8_t bytesArray[4];
+    };
+
+    union BME280Comp
+    {
+        struct __attribute__((packed))
+        {
+            uint16_t dig_T1;
+            int16_t dig_T2;
+            int16_t dig_T3;
+            uint16_t dig_P1;
+            int16_t dig_P2;
+            int16_t dig_P3;
+            int16_t dig_P4;
+            int16_t dig_P5;
+            int16_t dig_P6;
+            int16_t dig_P7;
+            int16_t dig_P8;
+            int16_t dig_P9;
+            uint8_t dig_H1;
+            int16_t dig_H2;
+            uint8_t dig_H3;
+            int16_t dig_H4 : 12;
+            int16_t dig_H5 : 12;
+            int8_t dig_H6;
+        } bits;
+
+        uint8_t bytesArray[32];
+    };
+
+    static constexpr uint8_t REG_ID_VAL = 0x60;  ///< Who am I value
+
+    ///< Default register values
+    static const BME280Config BME280_DEFAULT_CONFIG;
+
+    ///< Datasheet values for indoor navigation
+    static const BME280Config BME280_CONFIG_ALL_ENABLED;
+
+    ///< Temperature enabled in forced mode
+    static const BME280Config BME280_CONFIG_TEMP_SINGLE;
+
+    explicit BME280I2C(I2C& bus,
+                       BME280Config config = BME280_CONFIG_ALL_ENABLED);
+
+    /**
+     * @brief Initialize the device with the specified configuration
+     */
+    bool init() override;
+
+    /**
+     * @brief Sets the sensor mode
+     *
+     * Values:
+     * - SLEEP_MODE: No measurements are performed
+     * - FORCED_MODE: A single measurement is performed when this function
+     * writes the configuration, after the measurement the sensor return to
+     * sleep mode
+     * - NORMAL_MODE: Automated cycling between measurements, standby time can
+     * be set using setStandbyTime()
+     */
+    void setSensorMode(Mode mode);
+
+    /**
+     * @brief Sets the oversampling for humidity readings, use SKIPPED to
+     * disable humidity sampling
+     */
+    void setHumidityOversampling(Oversampling oversampling);
+
+    /**
+     * @brief Sets the oversampling for pressure readings, use SKIPPED to
+     * disable pressure sampling
+     */
+    void setPressureOversampling(Oversampling oversampling);
+
+    /**
+     * @brief Sets the oversampling for temperature readings, use SKIPPED to
+     * disable temperature sampling
+     */
+    void setTemperatureOversampling(Oversampling oversampling);
+
+    /**
+     * @brief Sets the coefficient for the IIR filter (applied to temperature
+     * and pressure)
+     */
+    void setFilterCoeff(FilterCoeff filterCoeff);
+
+    /**
+     * @brief Sets the standby time between readings in normal mode
+     */
+    void setStandbyTime(StandbyTime standbyTime);
+
+    /**
+     * @brief Reads only the humidity, does not set the configuration
+     */
+    HumidityData readHumidity();
+
+    /**
+     * @brief Reads only the pressure, does not set the configuration
+     */
+    PressureData readPressure();
+
+    /**
+     * @brief Reads only the temperature, does not set the configuration
+     */
+    TemperatureData readTemperature();
+
+    /**
+     * @brief Maximum measurement time formula from datasheet page 51
+     *
+     * @return Time in milliseconds
+     */
+    static unsigned int calculateMaxMeasurementTime(BME280Config config);
+
+    unsigned int getMaxMeasurementTime();
+
+    /**
+     * @brief Reads the WHO AM I register
+     *
+     * @return True if everything ok
+     */
+    bool selfTest() override;
+
+private:
+    BME280Data sampleImpl() override;
+
+    bool reset();
+
+    void setConfiguration();
+
+    void setConfiguration(BME280Config config);
+
+    BME280Config readConfiguration();
+
+    void loadCompensationParameters();
+
+    // Compensation algorithm rev.1.1 from Bosh datasheet
+
+    int32_t computeFineTemperature(int32_t adcTemperature);
+
+    int32_t compensateTemperature(int32_t fineTemperature);
+
+    uint32_t compensatePressure(int32_t adcPressure);
+
+    uint32_t compensateHumidity(int32_t adcHumidity);
+
+    /**
+     * @brief Check the WHO AM I code from the device.
+     *
+     * @return true if the device is recognized
+     */
+    bool checkWhoAmI();
+
+    enum Registers : uint8_t
+    {
+        REG_CALIB_0 = 0x88,
+        // Calibration register 1-25
+
+        REG_ID    = 0xD0,
+        REG_RESET = 0xE0,
+
+        REG_CALIB_26 = 0xE1,
+        // Calibration register 27-41
+
+        REG_CTRL_HUM  = 0xF2,
+        REG_STATUS    = 0xF3,
+        REG_CTRL_MEAS = 0xF4,
+        REG_CONFIG    = 0xF5,
+
+        REG_PRESS_MSB  = 0xF7,
+        REG_PRESS_LSB  = 0xF8,
+        REG_PRESS_XLSB = 0xF9,
+        REG_TEMP_MSB   = 0xFA,
+        REG_TEMP_LSB   = 0xFB,
+        REG_TEMP_XLSB  = 0xFC,
+        REG_HUM_MSB    = 0xFD,
+        REG_HUM_LSB    = 0xFE,
+    };
+
+    I2C& bus;
+    I2CDriver::I2CSlaveConfig slaveConfig{0x76, I2CDriver::Addressing::BIT7,
+                                          I2CDriver::Speed::STANDARD};
+
+    BME280Config config;
+    BME280Comp compParams;
+    int32_t fineTemperature;  // Used in compensation algorithm
+
+    PrintLogger logger = Logging::getLogger("bme280");
+};
+
+}  // namespace Boardcore

src/shared/sensors/BMP280/BMP280.cpp

@@ -52,16 +52,17 @@ BMP280::BMP280(SPISlave spiSlave, BMP280Config config)
 
 bool BMP280::init()
 {
-    // Check WHO AM I
     if (!checkWhoAmI())
     {
         LOG_ERR(logger, "Invalid WHO AM I");
 
         lastError = SensorErrors::INVALID_WHOAMI;
-
         return false;
     }
 
+    reset();
+    miosix::Thread::sleep(3);
+
     loadCompensationParameters();
 
     // Read once the temperature to compute fineTemperature
@@ -70,11 +71,12 @@ bool BMP280::init()
         calculateMaxMeasurementTime(BMP280_CONFIG_TEMP_SINGLE));
     readTemperature();
 
+    // Set the target configuration
     setConfiguration();
 
     BMP280Config readBackConfig = readConfiguration();
 
-    // Check if the configration on the device matches ours
+    // Check if the configuration on the device matches ours
     if (config.bytes.ctrlPressureAndTemperature !=
             readBackConfig.bytes.ctrlPressureAndTemperature ||
         config.bytes.config != readBackConfig.bytes.config)
@@ -82,7 +84,6 @@ bool BMP280::init()
         LOG_ERR(logger, "Device configuration incorrect, setup failed");
 
         lastError = SensorErrors::NOT_INIT;
-
         return false;
     }
 
@@ -127,7 +128,6 @@ void BMP280::setStandbyTime(StandbyTime standbyTime)
 PressureData BMP280::readPressure()
 {
     uint8_t buffer[3];
-    int32_t adc_P = 0;
 
     {
         SPITransaction transaction(spiSlave);
@@ -135,22 +135,21 @@ PressureData BMP280::readPressure()
         transaction.readRegisters(REG_PRESS_MSB, buffer, 3);
     }
 
-    adc_P |= ((uint32_t)buffer[0]) << 12;
+    int32_t adc_P = ((uint32_t)buffer[0]) << 12;
     adc_P |= ((uint32_t)buffer[1]) << 4;
     adc_P |= (buffer[2] >> 4) & 0x0F;
 
-    // Compensate pressure
-    lastSample.pressureTimestamp = TimestampTimer::getTimestamp();
-    lastSample.pressure =
-        (float)compensatePressure(adc_P) / 256;  // Convert to Pa
+    PressureData data;
+    data.pressureTimestamp = TimestampTimer::getTimestamp();
+    data.pressure          = compensatePressure(adc_P);
+    data.pressure /= 256;  // Convert to Pa
 
-    return lastSample;
+    return data;
 }
 
 TemperatureData BMP280::readTemperature()
 {
     uint8_t buffer[3];
-    int32_t adcTemperature = 0;
 
     {
         SPITransaction transaction(spiSlave);
@@ -158,25 +157,26 @@ TemperatureData BMP280::readTemperature()
         transaction.readRegisters(REG_TEMP_MSB, buffer, 3);
     }
 
-    adcTemperature |= ((uint32_t)buffer[0]) << 12;
+    int32_t adcTemperature = ((uint32_t)buffer[0]) << 12;
     adcTemperature |= ((uint32_t)buffer[1]) << 4;
     adcTemperature |= (buffer[2] >> 4) & 0x0F;
 
-    // Compensate temperature
     fineTemperature = computeFineTemperature(adcTemperature);
-    lastSample.temperatureTimestamp = TimestampTimer::getTimestamp();
-    lastSample.temperature = (float)compensateTemperature(fineTemperature) /
-                             100;  // Converto to DegC
 
-    return lastSample;
+    TemperatureData data;
+    data.temperatureTimestamp = TimestampTimer::getTimestamp();
+    data.temperature          = compensateTemperature(fineTemperature);
+    data.temperature /= 100;  // Convert to to DegC
+
+    return data;
 }
 
-unsigned int BMP280::calculateMaxMeasurementTime(BMP280Config config_)
+unsigned int BMP280::calculateMaxMeasurementTime(BMP280Config config)
 {
-    // TODO: This folrmula is not present in the BMP280's datasheet, it should
+    // TODO: This formula is not present in the BMP280's datasheet, it should
     // be checked
-    return ceil(1.25 + (2.3 * config_.bits.oversamplingTemperature) +
-                (2.3 * config_.bits.oversamplingPressure + 0.575));
+    return ceil(1.25 + (2.3 * config.bits.oversamplingTemperature) +
+                (2.3 * config.bits.oversamplingPressure + 0.575));
 }
 
 unsigned int BMP280::getMaxMeasurementTime()
@@ -188,41 +188,46 @@ bool BMP280::selfTest() { return checkWhoAmI(); }
 
 BMP280Data BMP280::sampleImpl()
 {
-    uint8_t buffer[8];
-    int32_t adcTemperature = 0;
-    int32_t adc_P          = 0;
-    BMP280Data data;
-
     // TODO: implement selective read!
 
-    // Burst read pressure, temperature and humidity
+    uint8_t buffer[8];
     {
         SPITransaction transaction(spiSlave);
 
         transaction.readRegisters(REG_PRESS_MSB, buffer, 8);
     }
 
-    adcTemperature |= ((uint32_t)buffer[3]) << 12;
+    BMP280Data data;
+
+    int32_t adcTemperature = ((uint32_t)buffer[3]) << 12;
     adcTemperature |= ((uint32_t)buffer[4]) << 4;
     adcTemperature |= (buffer[5] >> 4) & 0x0F;
 
-    adc_P |= ((uint32_t)buffer[0]) << 12;
+    int32_t adc_P = ((uint32_t)buffer[0]) << 12;
     adc_P |= ((uint32_t)buffer[1]) << 4;
     adc_P |= (buffer[2] >> 4) & 0x0F;
 
     // Compensate temperature
     fineTemperature           = computeFineTemperature(adcTemperature);
     data.temperatureTimestamp = TimestampTimer::getTimestamp();
-    data.temperature          = (float)compensateTemperature(fineTemperature) /
-                       100;  // Converto to DegC
+    data.temperature          = compensateTemperature(fineTemperature);
+    data.temperature /= 100;  // Convert to to DegC
 
     // Compensate pressure
     data.pressureTimestamp = TimestampTimer::getTimestamp();
-    data.pressure = (float)compensatePressure(adc_P) / 256;  // Convert to Pa
+    data.pressure          = compensatePressure(adc_P);
+    data.pressure /= 256;  // Convert to Pa
 
     return data;
 }
 
+void BMP280::reset()
+{
+    SPITransaction transaction(spiSlave);
+
+    transaction.writeRegister(REG_RESET, 0xB6);
+}
+
 bool BMP280::checkWhoAmI()
 {
     SPITransaction transaction(spiSlave);
@@ -234,13 +239,13 @@ bool BMP280::checkWhoAmI()
 
 void BMP280::setConfiguration() { setConfiguration(config); }
 
-void BMP280::setConfiguration(BMP280Config config_)
+void BMP280::setConfiguration(BMP280Config config)
 {
     SPITransaction transaction(spiSlave);
 
-    transaction.writeRegister(REG_CONFIG & 0x7F, config_.bytes.config);
-    transaction.writeRegister(REG_CTRL_MEAS & 0x7F,
-                              config_.bytes.ctrlPressureAndTemperature);
+    transaction.writeRegister(REG_CONFIG, config.bytes.config);
+    transaction.writeRegister(REG_CTRL_MEAS,
+                              config.bytes.ctrlPressureAndTemperature);
 }
 
 BMP280::BMP280Config BMP280::readConfiguration()

src/shared/sensors/BMP280/BMP280.h

@@ -137,15 +137,14 @@ public:
 
     static constexpr uint8_t REG_ID_VAL = 0x58;  ///< Who am I value
 
-    static const BMP280Config BMP280_DEFAULT_CONFIG;      ///< Default register
-                                                          ///< values
-    static const BMP280Config BMP280_CONFIG_ALL_ENABLED;  ///< Datasheet
-                                                          ///< values for
-                                                          ///< indoor
-                                                          ///< navigation
-    static const BMP280Config BMP280_CONFIG_TEMP_SINGLE;  ///< Temperature
-                                                          ///< enabled in
-                                                          ///< forced mode
+    ///< Default register values
+    static const BMP280Config BMP280_DEFAULT_CONFIG;
+
+    ///< Datasheet values for indoor navigation
+    static const BMP280Config BMP280_CONFIG_ALL_ENABLED;
+
+    ///< Temperature enabled in forced mode
+    static const BMP280Config BMP280_CONFIG_TEMP_SINGLE;
 
     explicit BMP280(SPISlave spiSlave,
                     BMP280Config config = BMP280_CONFIG_ALL_ENABLED);
@@ -206,7 +205,7 @@ public:
      *
      * @return Time in milliseconds
      */
-    static unsigned int calculateMaxMeasurementTime(BMP280Config config_);
+    static unsigned int calculateMaxMeasurementTime(BMP280Config config);
 
     unsigned int getMaxMeasurementTime();
 
@@ -220,9 +219,11 @@ public:
 private:
     BMP280Data sampleImpl() override;
 
+    void reset();
+
     void setConfiguration();
 
-    void setConfiguration(BMP280Config config_);
+    void setConfiguration(BMP280Config config);
 
     BMP280Config readConfiguration();
 
@@ -244,22 +245,22 @@ private:
 
     enum Registers : uint8_t
     {
-        REG_CALIB_0 = 0x88,
+        REG_CALIB_0 = 0x08,
         // Calibration register 1-25
 
-        REG_ID    = 0xD0,
-        REG_RESET = 0xE0,
+        REG_ID    = 0x50,
+        REG_RESET = 0x60,
 
-        REG_STATUS    = 0xF3,
-        REG_CTRL_MEAS = 0xF4,
-        REG_CONFIG    = 0xF5,
+        REG_STATUS    = 0x73,
+        REG_CTRL_MEAS = 0x74,
+        REG_CONFIG    = 0x75,
 
-        REG_PRESS_MSB  = 0xF7,
-        REG_PRESS_LSB  = 0xF8,
-        REG_PRESS_XLSB = 0xF9,
-        REG_TEMP_MSB   = 0xFA,
-        REG_TEMP_LSB   = 0xFB,
-        REG_TEMP_XLSB  = 0xFC
+        REG_PRESS_MSB  = 0x77,
+        REG_PRESS_LSB  = 0x78,
+        REG_PRESS_XLSB = 0x79,
+        REG_TEMP_MSB   = 0x7A,
+        REG_TEMP_LSB   = 0x7B,
+        REG_TEMP_XLSB  = 0x7C
     };
 
     const SPISlave spiSlave;

src/shared/sensors/BMP280/BMP280I2C.cpp

+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "BMP280I2C.h"
+
+#include <drivers/timer/TimestampTimer.h>
+#include <math.h>
+
+using namespace std;
+
+namespace Boardcore
+{
+
+const BMP280I2C::BMP280Config BMP280I2C::BMP280_DEFAULT_CONFIG = {
+    0, 0, SLEEP_MODE, SKIPPED, SKIPPED, 0, FILTER_OFF, STB_TIME_0_5};
+
+const BMP280I2C::BMP280Config BMP280I2C::BMP280_CONFIG_ALL_ENABLED = {
+    0,
+    0,
+    NORMAL_MODE,
+    OVERSAMPLING_16,
+    OVERSAMPLING_2,
+    0,
+    FILTER_COEFF_16,
+    STB_TIME_0_5};
+
+const BMP280I2C::BMP280Config BMP280I2C::BMP280_CONFIG_TEMP_SINGLE = {
+    0, 0, FORCED_MODE, SKIPPED, OVERSAMPLING_1, 0, FILTER_OFF, STB_TIME_0_5};
+
+BMP280I2C::BMP280I2C(I2C &bus, BMP280Config config) : bus(bus), config(config)
+{
+}
+
+bool BMP280I2C::init()
+{
+    if (!checkWhoAmI())
+    {
+        LOG_ERR(logger, "Invalid WHO AM I");
+
+        lastError = SensorErrors::INVALID_WHOAMI;
+        return false;
+    }
+
+    if (!reset())
+    {
+        return false;
+    }
+    miosix::Thread::sleep(3);
+
+    loadCompensationParameters();
+
+    // Read once the temperature to compute fineTemperature
+    setConfiguration(BMP280_CONFIG_TEMP_SINGLE);
+    miosix::Thread::sleep(
+        calculateMaxMeasurementTime(BMP280_CONFIG_TEMP_SINGLE));
+    readTemperature();
+
+    // Set the target configuration
+    setConfiguration();
+
+    BMP280Config readBackConfig = readConfiguration();
+
+    // Check if the configuration on the device matches ours
+    if (config.bytes.ctrlPressureAndTemperature !=
+            readBackConfig.bytes.ctrlPressureAndTemperature ||
+        config.bytes.config != readBackConfig.bytes.config)
+    {
+        LOG_ERR(logger, "Device configuration incorrect, setup failed");
+
+        lastError = SensorErrors::NOT_INIT;
+        return false;
+    }
+
+    return true;
+}
+
+void BMP280I2C::setSensorMode(Mode mode)
+{
+    config.bits.mode = mode;
+
+    setConfiguration();
+}
+
+void BMP280I2C::setPressureOversampling(Oversampling oversampling)
+{
+    config.bits.oversamplingPressure = oversampling;
+
+    setConfiguration();
+}
+
+void BMP280I2C::setTemperatureOversampling(Oversampling oversampling)
+{
+    config.bits.oversamplingTemperature = oversampling;
+
+    setConfiguration();
+}
+
+void BMP280I2C::setFilterCoeff(FilterCoeff filterCoeff)
+{
+    config.bits.filter = filterCoeff;
+
+    setConfiguration();
+}
+
+void BMP280I2C::setStandbyTime(StandbyTime standbyTime)
+{
+    config.bits.standbyTime = standbyTime;
+
+    setConfiguration();
+}
+
+PressureData BMP280I2C::readPressure()
+{
+    uint8_t buffer[3];
+    if (bus.readFromRegister(slaveConfig, REG_PRESS_MSB, buffer, 3))
+    {
+
+        int32_t adc_P = ((uint32_t)buffer[0]) << 12;
+        adc_P |= ((uint32_t)buffer[1]) << 4;
+        adc_P |= (buffer[2] >> 4) & 0x0F;
+
+        PressureData data;
+        data.pressureTimestamp = TimestampTimer::getTimestamp();
+        data.pressure          = compensatePressure(adc_P);
+        data.pressure /= 256;  // Convert to Pa
+
+        return data;
+    }
+    else
+    {
+        lastError = SensorErrors::BUS_FAULT;
+        return lastSample;
+    }
+}
+
+TemperatureData BMP280I2C::readTemperature()
+{
+    uint8_t buffer[3];
+    if (bus.readFromRegister(slaveConfig, REG_TEMP_MSB, buffer, 3))
+    {
+        int32_t adcTemperature = ((uint32_t)buffer[0]) << 12;
+        adcTemperature |= ((uint32_t)buffer[1]) << 4;
+        adcTemperature |= (buffer[2] >> 4) & 0x0F;
+
+        fineTemperature = computeFineTemperature(adcTemperature);
+
+        TemperatureData data;
+        data.temperatureTimestamp = TimestampTimer::getTimestamp();
+        data.temperature          = compensateTemperature(fineTemperature);
+        data.temperature /= 100;  // Convert to to DegC
+
+        return data;
+    }
+    else
+    {
+        lastError = SensorErrors::BUS_FAULT;
+        return lastSample;
+    }
+}
+
+unsigned int BMP280I2C::calculateMaxMeasurementTime(BMP280Config config)
+{
+    // TODO: This formula is not present in the BMP280's datasheet, it should
+    // be checked
+    return ceil(1.25 + (2.3 * config.bits.oversamplingTemperature) +
+                (2.3 * config.bits.oversamplingPressure + 0.575));
+}
+
+unsigned int BMP280I2C::getMaxMeasurementTime()
+{
+    return calculateMaxMeasurementTime(config);
+}
+
+bool BMP280I2C::selfTest() { return checkWhoAmI(); }
+
+BMP280Data BMP280I2C::sampleImpl()
+{
+    // TODO: implement selective read!
+
+    uint8_t buffer[6];
+    if (bus.readFromRegister(slaveConfig, REG_PRESS_MSB, buffer, 6))
+    {
+        BMP280Data data;
+
+        int32_t adcTemperature = ((uint32_t)buffer[3]) << 12;
+        adcTemperature |= ((uint32_t)buffer[4]) << 4;
+        adcTemperature |= (buffer[5] >> 4) & 0x0F;
+
+        int32_t adc_P = ((uint32_t)buffer[0]) << 12;
+        adc_P |= ((uint32_t)buffer[1]) << 4;
+        adc_P |= (buffer[2] >> 4) & 0x0F;
+
+        // Compensate temperature
+        fineTemperature           = computeFineTemperature(adcTemperature);
+        data.temperatureTimestamp = TimestampTimer::getTimestamp();
+        data.temperature          = compensateTemperature(fineTemperature);
+        data.temperature /= 100;  // Convert to to DegC
+
+        // Compensate pressure
+        data.pressureTimestamp = TimestampTimer::getTimestamp();
+        data.pressure          = compensatePressure(adc_P);
+        data.pressure /= 256;  // Convert to Pa
+
+        return data;
+    }
+    else
+    {
+        lastError = SensorErrors::BUS_FAULT;
+        return lastSample;
+    }
+}
+
+bool BMP280I2C::reset()
+{
+    if (!bus.writeRegister(slaveConfig, REG_RESET, 0xB6))
+    {
+        lastError = SensorErrors::BUS_FAULT;
+        return false;
+    }
+
+    return true;
+}
+
+bool BMP280I2C::checkWhoAmI()
+{
+    uint8_t whoAmIValue;
+
+    if (bus.readRegister(slaveConfig, REG_ID, whoAmIValue))
+    {
+
+        return whoAmIValue == REG_ID_VAL;
+    }
+    else
+    {
+        lastError = SensorErrors::BUS_FAULT;
+        return false;
+    }
+}
+
+void BMP280I2C::setConfiguration() { setConfiguration(config); }
+
+void BMP280I2C::setConfiguration(BMP280Config config)
+{
+    if (!bus.writeRegister(slaveConfig, REG_CONFIG, config.bytes.config))
+    {
+        LOG_ERR(logger, "Error while writing to register REG_CONFIG");
+        return;
+    }
+
+    if (!bus.writeRegister(slaveConfig, REG_CTRL_MEAS,
+                           config.bytes.ctrlPressureAndTemperature))
+    {
+        LOG_ERR(logger, "Error while writing to register REG_CTRL_MEAS");
+        return;
+    }
+}
+
+BMP280I2C::BMP280Config BMP280I2C::readConfiguration()
+{
+    BMP280Config tmp;
+
+    if (bus.readFromRegister(slaveConfig, REG_STATUS, (uint8_t *)&tmp, 3))
+    {
+        return tmp;
+    }
+    else
+    {
+        lastError = SensorErrors::BUS_FAULT;
+        return BMP280_DEFAULT_CONFIG;
+    }
+}
+
+void BMP280I2C::loadCompensationParameters()
+{
+    // Read first batch of compensation parameters
+    if (!bus.readFromRegister(slaveConfig, REG_CALIB_0, (uint8_t *)&compParams,
+                              25))
+    {
+        lastError = SensorErrors::BUS_FAULT;
+        return;
+    }
+}
+
+int32_t BMP280I2C::computeFineTemperature(int32_t adcTemperature)
+{
+    int32_t var1, var2;
+    var1 = ((((adcTemperature >> 3) - ((int32_t)compParams.bits.dig_T1 << 1))) *
+            ((int32_t)compParams.bits.dig_T2)) >>
+           11;
+    var2 = (((((adcTemperature >> 4) - ((int32_t)compParams.bits.dig_T1)) *
+              ((adcTemperature >> 4) - ((int32_t)compParams.bits.dig_T1))) >>
+             12) *
+            ((int32_t)compParams.bits.dig_T3)) >>
+           14;
+    return var1 + var2;
+}
+
+int32_t BMP280I2C::compensateTemperature(int32_t fineTemperature)
+{
+    return (fineTemperature * 5 + 128) >> 8;
+}
+
+uint32_t BMP280I2C::compensatePressure(int32_t adc_P)
+{
+    int64_t var1, var2, p;
+    var1 = ((int64_t)fineTemperature) - 128000;
+    var2 = var1 * var1 * (int64_t)compParams.bits.dig_P6;
+    var2 = var2 + ((var1 * (int64_t)compParams.bits.dig_P5) << 17);
+    var2 = var2 + (((int64_t)compParams.bits.dig_P4) << 35);
+    var1 = ((var1 * var1 * (int64_t)compParams.bits.dig_P3) >> 8) +
+           ((var1 * ((int64_t)compParams.bits.dig_P2) << 12));
+    var1 =
+        ((((int64_t)1) << 47) + var1) * ((int64_t)compParams.bits.dig_P1) >> 33;
+    if (var1 == 0)
+    {
+        return 0;  // avoid exception caused by division by zero
+    }
+    p    = 1048576 - adc_P;
+    p    = (((p << 31) - var2) * 3125) / var1;
+    var1 = (((int64_t)compParams.bits.dig_P9) * (p >> 13) * (p >> 13)) >> 25;
+    var2 = (((int64_t)compParams.bits.dig_P8) * p) >> 19;
+    p    = ((p + var1 + var2) >> 8) + (((int64_t)compParams.bits.dig_P7) << 4);
+    return (uint32_t)p;
+}
+
+}  // namespace Boardcore

src/shared/sensors/BMP280/BMP280I2C.h

+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <diagnostic/PrintLogger.h>
+#include <drivers/i2c/I2C.h>
+#include <sensors/Sensor.h>
+
+#include "BMP280Data.h"
+
+namespace Boardcore
+{
+
+class BMP280I2C : public Sensor<BMP280Data>
+{
+public:
+    enum Oversampling
+    {
+        SKIPPED         = 0x0,  ///< Skipped (output set to 0x8000)
+        OVERSAMPLING_1  = 0x1,  ///< Oversampling x1
+        OVERSAMPLING_2  = 0x2,  ///< Oversampling x2
+        OVERSAMPLING_4  = 0x3,  ///< Oversampling x4
+        OVERSAMPLING_8  = 0x4,  ///< Oversampling x8
+        OVERSAMPLING_16 = 0x5,  ///< Oversampling x16
+    };
+
+    enum Mode
+    {
+        SLEEP_MODE  = 0x0,  ///< Sleep mode
+        FORCED_MODE = 0x1,  ///< Forced mode
+        NORMAL_MODE = 0x3   ///< Normal mode
+    };
+
+    enum StandbyTime
+    {
+        STB_TIME_0_5  = 0x0,  ///< 0.5 ms
+        STB_TIME_62_5 = 0x1,  ///< 62.5 ms
+        STB_TIME_125  = 0x2,  ///< 125 ms
+        STB_TIME_250  = 0x3,  ///< 250 ms
+        STB_TIME_500  = 0x4,  ///< 500 ms
+        STB_TIME_1000 = 0x5,  ///< 1000 ms
+        STB_TIME_20   = 0x6,  ///< 20 ms
+        STB_TIME_40   = 0x7   ///< 40 ms
+    };
+
+    enum FilterCoeff
+    {
+        FILTER_OFF      = 0x0,  ///< Filter off
+        FILTER_COEFF_2  = 0x1,  ///< Filter coefficient = 2
+        FILTER_COEFF_4  = 0x2,  ///< Filter coefficient = 4
+        FILTER_COEFF_8  = 0x3,  ///< Filter coefficient = 8
+        FILTER_COEFF_16 = 0x4   ///< Filter coefficient = 16
+    };
+
+    union BMP280Config
+    {
+        struct __attribute__((packed)) BMP280ConfigBits
+        {
+            // status
+            /**
+             * '1' when the NVM data are being copied to image registers, '0'
+             * when the copying is done
+             */
+            uint8_t imUpdate : 1;
+            uint8_t : 2;
+            /**
+             * '1' whenever a conversion is running, '0' when the result have
+             * been transferred to the data registers
+             */
+            uint8_t measuring : 1;
+            uint8_t : 4;
+
+            Mode mode : 2;  ///< Device modes
+            Oversampling
+                oversamplingPressure : 3;  ///< Oversampling of pressure
+            Oversampling
+                oversamplingTemperature : 3;  ///< Oversampling of temperature
+
+            // config
+            uint8_t spi3wEn : 1;  ///< Enables 3-wire SPI interface
+            uint8_t : 1;
+            FilterCoeff filter : 3;       ///< Time constant of the IIR filter
+            StandbyTime standbyTime : 3;  ///< Inactive duration in normal mode
+        } bits;
+
+        struct
+        {
+            uint8_t status;                      ///< Device status
+            uint8_t ctrlPressureAndTemperature;  ///< Pressure and temperature
+                                                 ///< options
+            uint8_t config;  ///< Rate, filter and interface options
+        } bytes;
+
+        uint8_t bytesArray[3];
+    };
+
+    union BMP280Comp
+    {
+        struct __attribute__((packed))
+        {
+            uint16_t dig_T1;
+            int16_t dig_T2;
+            int16_t dig_T3;
+            uint16_t dig_P1;
+            int16_t dig_P2;
+            int16_t dig_P3;
+            int16_t dig_P4;
+            int16_t dig_P5;
+            int16_t dig_P6;
+            int16_t dig_P7;
+            int16_t dig_P8;
+            int16_t dig_P9;
+        } bits;
+
+        uint8_t bytesArray[24];
+    };
+
+    static constexpr uint8_t REG_ID_VAL = 0x58;  ///< Who am I value
+
+    ///< Default register values
+    static const BMP280Config BMP280_DEFAULT_CONFIG;
+
+    ///< Datasheet values for indoor navigation
+    static const BMP280Config BMP280_CONFIG_ALL_ENABLED;
+
+    ///< Temperature enabled in forced mode
+    static const BMP280Config BMP280_CONFIG_TEMP_SINGLE;
+
+    explicit BMP280I2C(I2C& bus,
+                       BMP280Config config = BMP280_CONFIG_ALL_ENABLED);
+
+    /**
+     * @brief Initialize the device with the specified configuration
+     */
+    bool init() override;
+
+    /**
+     * @brief Sets the sensor mode
+     *
+     * Values:
+     * - SLEEP_MODE: No measurements are performed
+     * - FORCED_MODE: A single measurement is performed when this function
+     * writes the configuration, after the measurement the sensor return to
+     * sleep mode
+     * - NORMAL_MODE: Automated cycling between measurements, standby time can
+     * be set using setStandbyTime()
+     */
+    void setSensorMode(Mode mode);
+
+    /**
+     * @brief Sets the oversampling for pressure readings, use SKIPPED to
+     * disable pressure sampling
+     */
+    void setPressureOversampling(Oversampling oversampling);
+
+    /**
+     * @brief Sets the oversampling for temperature readings, use SKIPPED to
+     * disable temperature sampling
+     */
+    void setTemperatureOversampling(Oversampling oversampling);
+
+    /**
+     * @brief Sets the coefficient for the IIR filter (applied to temperature
+     * and pressure)
+     */
+    void setFilterCoeff(FilterCoeff filterCoeff);
+
+    /**
+     * @brief Sets the standby time between readings in normal mode
+     */
+    void setStandbyTime(StandbyTime standbyTime);
+
+    /**
+     * @brief Reads only the pressure, does not set the configuration
+     */
+    PressureData readPressure();
+
+    /**
+     * @brief Reads only the temperature, does not set the configuration
+     */
+    TemperatureData readTemperature();
+
+    /**
+     * @brief Maximum measurement time formula from datasheet page 51
+     *
+     * @return Time in milliseconds
+     */
+    static unsigned int calculateMaxMeasurementTime(BMP280Config config);
+
+    unsigned int getMaxMeasurementTime();
+
+    /**
+     * @brief Reads the WHO AM I register
+     *
+     * @return True if everything ok
+     */
+    bool selfTest() override;
+
+private:
+    BMP280Data sampleImpl() override;
+
+    bool reset();
+
+    void setConfiguration();
+
+    void setConfiguration(BMP280Config config);
+
+    BMP280Config readConfiguration();
+
+    void loadCompensationParameters();
+
+    // Compensation algorithm rev.1.1 from Bosh datasheet
+
+    int32_t computeFineTemperature(int32_t adcTemperature);
+
+    int32_t compensateTemperature(int32_t fineTemperature);
+
+    uint32_t compensatePressure(int32_t adcPressure);
+    /**
+     * @brief Check the WHO AM I code from the device.
+     *
+     * @return true if the device is recognized
+     */
+    bool checkWhoAmI();
+
+    enum Registers : uint8_t
+    {
+        REG_CALIB_0 = 0x88,
+        // Calibration register 1-25
+
+        REG_ID    = 0xD0,
+        REG_RESET = 0xE0,
+
+        REG_STATUS    = 0xF3,
+        REG_CTRL_MEAS = 0xF4,
+        REG_CONFIG    = 0xF5,
+
+        REG_PRESS_MSB  = 0xF7,
+        REG_PRESS_LSB  = 0xF8,
+        REG_PRESS_XLSB = 0xF9,
+        REG_TEMP_MSB   = 0xFA,
+        REG_TEMP_LSB   = 0xFB,
+        REG_TEMP_XLSB  = 0xFC
+    };
+
+    I2C& bus;
+    I2CDriver::I2CSlaveConfig slaveConfig{0x76, I2CDriver::Addressing::BIT7,
+                                          I2CDriver::Speed::STANDARD};
+
+    BMP280Config config;
+    BMP280Comp compParams;
+    int32_t fineTemperature;  // Used in compensation algorithm
+
+    PrintLogger logger = Logging::getLogger("bmp280");
+};
+
+}  // namespace Boardcore

src/shared/sensors/LPS331AP/LPS331AP.cpp

+
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "LPS331AP.h"
+
+#include <drivers/timer/TimestampTimer.h>
+
+namespace Boardcore
+{
+
+LPS331AP::LPS331AP(I2C& bus, ODR odr) : bus(bus), odr(odr) {}
+
+bool LPS331AP::init()
+{
+    if (!checkWhoAmI())
+    {
+        return false;
+    }
+
+    uint8_t ctrlReg1 = 0;
+    ctrlReg1 |= 0x80;  // Active mode
+    ctrlReg1 |= static_cast<uint8_t>(odr) << 4;
+
+    if (!bus.writeRegister(slaveConfig, REG_CTRL_REG1, ctrlReg1))
+    {
+
+        lastError = SensorErrors::BUS_FAULT;
+        return false;
+    }
+
+    // Maximum possible oversampling to lower the noise
+    uint8_t ctrlConf = odr != ODR::ODR_25Hz ? 0x69 : 0x7a;
+
+    if (!bus.writeRegister(slaveConfig, REG_RES_CONF, ctrlConf))
+    {
+        lastError = SensorErrors::BUS_FAULT;
+        return false;
+    }
+
+    return true;
+}
+
+bool LPS331AP::selfTest() { return checkWhoAmI(); }
+
+LPS331APData LPS331AP::sampleImpl()
+{
+    uint8_t buffer[5];
+    if (bus.readFromRegister(slaveConfig, REG_PRESS_XLSB, buffer, 5))
+    {
+        LPS331APData data;
+
+        int32_t pressure = 0;
+        pressure |= static_cast<uint32_t>(buffer[0]) << 16;
+        pressure |= static_cast<uint32_t>(buffer[1]) << 8;
+        pressure |= static_cast<uint32_t>(buffer[2]);
+
+        int32_t temperature = 0;
+        temperature |= static_cast<uint32_t>(buffer[3]) << 8;
+        temperature |= static_cast<uint32_t>(buffer[4]);
+
+        data.pressureTimestamp    = TimestampTimer::getTimestamp();
+        data.temperatureTimestamp = TimestampTimer::getTimestamp();
+        data.pressure             = pressure / 4096.0f;
+        data.temperature          = temperature / 480.0f + 42.5f;
+
+        return data;
+    }
+    else
+    {
+        lastError = SensorErrors::BUS_FAULT;
+        return lastSample;
+    }
+}
+
+bool LPS331AP::checkWhoAmI()
+{
+    uint8_t whoAmIValue;
+
+    if (bus.readRegister(slaveConfig, REG_WHO_AM_I, whoAmIValue))
+    {
+        if (whoAmIValue == WHO_AM_I_VAL)
+        {
+            return true;
+        }
+        else
+        {
+            LOG_ERR(logger, "Invalid WHO AM I");
+            lastError = SensorErrors::INVALID_WHOAMI;
+            return false;
+        }
+    }
+    else
+    {
+        lastError = SensorErrors::BUS_FAULT;
+        return false;
+    }
+}
+
+}  // namespace Boardcore

src/shared/sensors/LPS331AP/LPS331AP.h

+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <diagnostic/PrintLogger.h>
+#include <drivers/i2c/I2C.h>
+#include <sensors/Sensor.h>
+
+#include "LPS331APData.h"
+
+namespace Boardcore
+{
+
+class LPS331AP : public Sensor<LPS331APData>
+{
+public:
+    enum class ODR : uint8_t
+    {
+        ODR_1HZ    = 0x01,
+        ODR_7Hz    = 0x05,
+        ODR_12_5Hz = 0x06,
+        ODR_25Hz   = 0x07,
+    };
+
+    explicit LPS331AP(I2C& bus, ODR odr = ODR::ODR_25Hz);
+
+    bool init() override;
+
+    bool selfTest() override;
+
+private:
+    LPS331APData sampleImpl() override;
+
+    bool checkWhoAmI();
+
+    static constexpr uint8_t WHO_AM_I_VAL = 0x58;  ///< Who am I value
+
+    enum Registers : uint8_t
+    {
+        REG_WHO_AM_I   = 0x8f,
+        REG_RES_CONF   = 0x10,
+        REG_CTRL_REG1  = 0x20,
+        REG_PRESS_XLSB = 0x28,
+    };
+
+    I2C& bus;
+    I2CDriver::I2CSlaveConfig slaveConfig{0x5c, I2CDriver::Addressing::BIT7,
+                                          I2CDriver::Speed::STANDARD};
+
+    ODR odr;
+
+    PrintLogger logger = Logging::getLogger("lps331ap");
+};
+
+}  // namespace Boardcore