diff --git a/CMakeLists.txt b/CMakeLists.txt index 8859335116208741211e067fce992995e81be572..b449783ff1651d17fb6ab05a4d92c36902f542be 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -416,6 +416,9 @@ sbs_target(test-lis2mdl stm32f429zi_stm32f4discovery) add_executable(test-h3lis331dl src/tests/sensors/test-h3lis331dl.cpp) sbs_target(test-h3lis331dl stm32f407vg_stm32f4discovery) +add_executable(test-lps28dfw src/tests/sensors/test-lps28dfw.cpp) +sbs_target(test-lps28dfw stm32f767zi_nucleo) + #-----------------------------------------------------------------------------# # Tests - Utils # #-----------------------------------------------------------------------------# diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake index 43a510b067c0d8d8de3bffa666ed8f14b1ab224a..e88d029b7f44399f15aa01dfd8f31e781c37158a 100644 --- a/cmake/boardcore.cmake +++ b/cmake/boardcore.cmake @@ -108,6 +108,7 @@ foreach(OPT_BOARD ${BOARDS}) ${SBS_BASE}/src/shared/sensors/UBXGPS/UBXGPSSpi.cpp ${SBS_BASE}/src/shared/sensors/VN100/VN100.cpp ${SBS_BASE}/src/shared/sensors/LIS2MDL/LIS2MDL.cpp + ${SBS_BASE}/src/shared/sensors/LPS28DFW/LPS28DFW.cpp # Calibration ${SBS_BASE}/src/shared/sensors/calibration/BiasCalibration/BiasCalibration.cpp diff --git a/src/shared/sensors/LPS28DFW/LPS28DFW.cpp b/src/shared/sensors/LPS28DFW/LPS28DFW.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a73590c84386fe812b78f1ca1b0269b95bb226f7 --- /dev/null +++ b/src/shared/sensors/LPS28DFW/LPS28DFW.cpp @@ -0,0 +1,307 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * 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 "LPS28DFW.h" + +#include <drivers/timer/TimestampTimer.h> + +#include "LPS28DFWDefs.h" + +using namespace Boardcore::LPS28DFWDefs; + +namespace Boardcore +{ +LPS28DFW::LPS28DFW(I2C& i2c, SensorConfig sensorConfig) + : i2cConfig{sensorConfig.sa0 ? lsp28dfwAddress1 : lsp28dfwAddress0, + I2CDriver::Addressing::BIT7, I2CDriver::Speed::MAX_SPEED}, + i2c(i2c), sensorConfig(sensorConfig) +{ + pressureSensitivity = (sensorConfig.fsr == FullScaleRange::FS_1260 + ? pressureSensitivity1260hPa + : pressureSensitivity4060hPa); +} + +bool LPS28DFW::init() +{ + if (isInitialized) + { + LOG_ERR(logger, "Attempted to initialized sensor twice but failed"); + lastError = ALREADY_INIT; + return false; + } + + // Setting the actual sensor configurations (Mode, ODR, AVG, FSR, DRDY) + if (!setConfig(sensorConfig)) + { + LOG_ERR(logger, "Configuration not applied correctly"); + lastError = SensorErrors::INIT_FAIL; + return false; + } + + lastError = SensorErrors::NO_ERRORS; + isInitialized = true; + return true; +} + +bool LPS28DFW::selfTest() +{ + // Since the sensor doesn't provide any self-test feature we just try to + // probe the sensor and read his whoami register. + if (!isInitialized) + { + LOG_ERR(logger, "Invoked selfTest() but sensor was uninitialized"); + lastError = NOT_INIT; + return false; + } + + // Trying to probe the sensor to check if it is connected + if (!i2c.probe(i2cConfig)) + { + LOG_ERR(logger, + "Can't communicate with the sensor or sensor not attached"); + lastError = BUS_FAULT; + return false; + } + + // Reading the whoami value to assure communication + uint8_t whoamiValue{0}; + if (!i2c.readRegister(i2cConfig, WHO_AM_I, whoamiValue)) + { + LOG_ERR(logger, "Can't communicate with the sensor"); + lastError = BUS_FAULT; + return false; + } + + // Checking the whoami value to assure correct communication + if (whoamiValue != WHO_AM_I_VALUE) + { + LOG_ERR(logger, + "WHO_AM_I value differs from expectation: read 0x{02x} " + "but expected 0x{02x}", + whoamiValue, WHO_AM_I_VALUE); + lastError = INVALID_WHOAMI; + return false; + } + + return true; +} + +bool LPS28DFW::setConfig(const SensorConfig& newSensorConfig) +{ + // Setting the FIFO_CTRL register to the correct mode (according to the + // ODR set: BYPASS for the one shot mode and CONTINUOUS for the + // continuous mode). + if (!i2c.writeRegister( + i2cConfig, FIFO_CTRL, + (newSensorConfig.odr == ODR::ONE_SHOT ? FIFO_CTRL::BYPASS + : FIFO_CTRL::CONTINUOUS))) + { + lastError = BUS_FAULT; + return false; + } + + if (!(setFullScaleRange(sensorConfig.fsr) && setAverage(sensorConfig.avg) && + setOutputDataRate(sensorConfig.odr) && + setDRDYInterrupt(sensorConfig.drdy))) + { + LOG_ERR(logger, "Sensor not configured"); + return false; + } + + return true; +} + +bool LPS28DFW::setAverage(AVG avg) +{ + // Since the CTRL_REG1 contains only the AVG and ODR settings, we use the + // internal driver state to set the register with the wanted ODR and AVG + // without previously reading it. This allows to avoid a useless + // transaction. + if (!i2c.writeRegister(i2cConfig, CTRL_REG1, sensorConfig.odr | avg)) + { + lastError = BUS_FAULT; + return false; + } + + sensorConfig.avg = avg; + return true; +} + +bool LPS28DFW::setOutputDataRate(ODR odr) +{ + // Since the CTRL_REG1 contains only the AVG and ODR settings, we use the + // internal driver state to set the register with the wanted ODR and AVG + // without previously reading it. This allows to avoid a useless + // transaction. + if (!i2c.writeRegister(i2cConfig, CTRL_REG1, odr | sensorConfig.avg)) + { + lastError = BUS_FAULT; + return false; + } + + sensorConfig.odr = odr; + return true; +} + +bool LPS28DFW::setFullScaleRange(FullScaleRange fs) +{ + uint8_t ctrl_reg2; + if (!i2c.readRegister(i2cConfig, CTRL_REG2, ctrl_reg2)) + { + lastError = BUS_FAULT; + return false; + } + + if (fs == FullScaleRange::FS_1260) + { + pressureSensitivity = pressureSensitivity1260hPa; + ctrl_reg2 = (ctrl_reg2 & ~CTRL_REG2::FS_MODE); + } + else + { + pressureSensitivity = pressureSensitivity4060hPa; + ctrl_reg2 = (ctrl_reg2 | CTRL_REG2::FS_MODE); + } + + if (!i2c.writeRegister(i2cConfig, CTRL_REG2, ctrl_reg2)) + { + lastError = BUS_FAULT; + return false; + } + + sensorConfig.fsr = fs; + return true; +} + +bool LPS28DFW::setDRDYInterrupt(bool drdy) +{ + if (!i2c.writeRegister(i2cConfig, CTRL_REG4, (drdy ? (INT_EN | DRDY) : 0))) + { + lastError = BUS_FAULT; + return false; + } + + sensorConfig.drdy = drdy; + return true; +} + +float LPS28DFW::convertPressure(uint8_t pressXL, uint8_t pressL, uint8_t pressH) +{ + // Pressure conversion + // sign extending the 27-bit value: shifting to the right a signed type + // extends its sign. So positioning the bytes shifted to the left of 8 + // bits, casting the result in a signed int8_t and then shifting the + // result to the right of 8 bits will make the work. + int32_t press_temp = + ((int32_t)((pressH << 24) | (pressL << 16) | (pressXL << 8))) >> 8; + return ((float)press_temp / pressureSensitivity); +} + +float LPS28DFW::convertTemperature(uint8_t tempL, uint8_t tempH) +{ + // Temperature conversion + int16_t temp_temp = (tempH << 8) | (tempL << 0); + return ((float)temp_temp) / temperatureSensitivity; +} + +LPS28DFWData LPS28DFW::sampleImpl() +{ + uint8_t statusValue{0}; + uint8_t val[5] = {0}; + LPS28DFWData data; + + lastError = NO_ERRORS; + + if (sensorConfig.odr == ODR::ONE_SHOT) + { + uint8_t ctrl_reg2_val{0}; + + // Triggering sampling + if (!(i2c.readRegister(i2cConfig, CTRL_REG2, ctrl_reg2_val) && + i2c.writeRegister(i2cConfig, CTRL_REG2, + ctrl_reg2_val | CTRL_REG2::ONE_SHOT_START))) + { + lastError = BUS_FAULT; + return lastSample; + } + + // Poll status register until the sample is ready + do + { + if (!i2c.readRegister(i2cConfig, STATUS, statusValue)) + { + lastError = BUS_FAULT; + return lastSample; + } + } while ((statusValue & (STATUS::P_DA | STATUS::T_DA)) != + (STATUS::P_DA | STATUS::T_DA)); + } + else + { + // read status register value + if (!i2c.readRegister(i2cConfig, STATUS, statusValue)) + { + lastError = BUS_FAULT; + return lastSample; + } + } + + auto ts = TimestampTimer::getTimestamp(); + + // reading 5 bytes if also Temperature new sample, otherwise only the 3 + // pressure bytes + if (!i2c.readFromRegister(i2cConfig, PRESS_OUT_XL, val, + ((statusValue & STATUS::T_DA) ? 5 : 3))) + { + lastError = BUS_FAULT; + return lastSample; + } + + // If pressure new data present + if (statusValue & STATUS::P_DA) + { + data.pressureTimestamp = ts; + data.pressure = convertPressure(val[0], val[1], val[2]); + } + else + { + lastError = NO_NEW_DATA; + data.pressureTimestamp = lastSample.pressureTimestamp; + data.pressure = lastSample.pressure; + } + + // If temperature new data present + if (statusValue & STATUS::T_DA) + { + data.temperatureTimestamp = ts; + data.temperature = convertTemperature(val[3], val[4]); + } + else + { + data.temperatureTimestamp = lastSample.temperatureTimestamp; + data.temperature = lastSample.temperature; + } + + return data; +} + +} // namespace Boardcore diff --git a/src/shared/sensors/LPS28DFW/LPS28DFW.h b/src/shared/sensors/LPS28DFW/LPS28DFW.h new file mode 100644 index 0000000000000000000000000000000000000000..33012760c5b6eeac46f921c3bee81ab5af94ccd0 --- /dev/null +++ b/src/shared/sensors/LPS28DFW/LPS28DFW.h @@ -0,0 +1,180 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * 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 <drivers/i2c/I2C.h> +#include <sensors/Sensor.h> + +#include "LPS28DFWData.h" + +namespace Boardcore +{ + +/** + * @brief Driver for LPS28DFW STMicroelectronics digital pressure sensor working + * in I2C. + */ +class LPS28DFW : public Sensor<LPS28DFWData> +{ +public: + /** + * @brief Enumeration for the full scale range to set on the sensor. + * Available are 1260 hPa or 4060 hPa. + */ + enum FullScaleRange : uint8_t + { + FS_1260, + FS_4060 + }; + + /** + * @brief Enumeration for Output Data Rate Configuration. + * + * Available are One shot (only one sample calculated when signal sent), 1 + * Hz to 200 Hz. ODR configuration is valid for both pressure and + * temperature. + */ + enum ODR : uint8_t + { + ONE_SHOT = (0b0000 << 3), + ODR_1 = (0b0001 << 3), + ODR_4 = (0b0010 << 3), + ODR_10 = (0b0011 << 3), + ODR_25 = (0b0100 << 3), + ODR_50 = (0b0101 << 3), + ODR_75 = (0b0110 << 3), + ODR_100 = (0b0111 << 3), + ODR_200 = (0b1000 << 3) + }; + + /** + * @brief Enumeration for the oversampling to set on the sensor. + * + * The value read from the sensor will actually be the average of multiple + * samples. Available are from 4 to 512 averaged samples. + * + * @warning For an AGV value of 512, 128, 64 the maximum ODR values are + * respectively of 25, 75 and 100 Hz. For any other AVG value all ODR + * configurations are possible. + */ + enum AVG : uint8_t + { + AVG_4 = 0b000, + AVG_8 = 0b001, + AVG_16 = 0b010, + AVG_32 = 0b011, + AVG_64 = 0b100, + AVG_128 = 0b101, + AVG_512 = 0b111 + }; + + /** + * @brief Struct that sums up all the settings of the sensor. + */ + typedef struct + { + bool sa0; ///< Last bit of the slave address; tells if the SA0 pin on + ///< the sensor is connected to GND or VDD (3.3V, not 5V!). + FullScaleRange fsr; ///< Full scale range + AVG avg; ///< Average avg samples + ODR odr; ///< Output data rate + bool drdy; ///< Enable Interrupt for Data Ready + } SensorConfig; + + /** + * @brief Constructor that stores the initial settings (without applying + * them to the sensor). + * @param i2c I2C Peripheral that will be used to communicate with the + * sensor. + * @param sensorConfig Configuration of the sensor with ODR, AVG, FSR, + * interrupt enabled and fifo size. + */ + LPS28DFW(I2C& i2c, SensorConfig sensorConfig); + + /** + * @brief Initializes the sensor with the current settings. + * @return true if initialization succeeded, false otherwise. + */ + bool init() override; + + /** + * @brief Sets and saves the configurations passed on the parameters. + */ + bool setConfig(const SensorConfig& config); + + /** + * @brief The self test method returns true if we read the right whoami + * value. We can't make a better self test due to the fact that the sensor + * doesn't support this feature. + * @return true if the right whoami has been read. + */ + bool selfTest() override; + + /** + * @brief Sets and saves the oversampling on the sensor. + * @return True if setting succeeded, false otherwise. + */ + bool setAverage(AVG avg); + + /** + * @brief Sets and saves the output data rate. + * @return True if setting succeeded, false otherwise. + */ + bool setOutputDataRate(ODR odr); + + /** + * @brief Sets and saves the full scale range. + * @return True if setting succeeded, false otherwise. + */ + bool setFullScaleRange(FullScaleRange fs); + + /** + * @brief Sets and saves the full scale range. + * @return True if setting succeeded, false otherwise. + */ + bool setDRDYInterrupt(bool drdy); + +private: + LPS28DFWData sampleImpl() override; + + /** + * @brief Converting the bytes read from the sensor to the pressure in Pa. + */ + float convertPressure(uint8_t pressXL, uint8_t pressL, uint8_t pressH); + + /** + * @brief Converting the bytes read from the sensor to the temperature in + * °C. + */ + float convertTemperature(uint8_t tempL, uint8_t tempH); + + I2CDriver::I2CSlaveConfig i2cConfig; ///< I2C address and speed mode + I2C& i2c; ///< I2C bus on which the sensor lays + SensorConfig sensorConfig; ///< Sensor settings + bool isInitialized = + false; ///< Flag to tell if the sensor has been initialized + uint16_t pressureSensitivity; ///< pressure sensitivity [LSB/Pa] + PrintLogger logger = Logging::getLogger("lps28dfw"); +}; + +} // namespace Boardcore \ No newline at end of file diff --git a/src/shared/sensors/LPS28DFW/LPS28DFWData.h b/src/shared/sensors/LPS28DFW/LPS28DFWData.h new file mode 100644 index 0000000000000000000000000000000000000000..9f3f38d22fe1328cf66a1121c80a294d68a28b52 --- /dev/null +++ b/src/shared/sensors/LPS28DFW/LPS28DFWData.h @@ -0,0 +1,57 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * 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 <sensors/SensorData.h> + +namespace Boardcore +{ + +/** + * @brief Struct for the LPS28DFW barometer data. Pressures stored in Pa and + * Temperature in °C. + */ +struct LPS28DFWData : public PressureData, TemperatureData +{ + + LPS28DFWData() : PressureData{0, 0.0}, TemperatureData{0, 0.0} {} + + LPS28DFWData(uint64_t pressT, float pressure, uint64_t tempT, + float temperature) + : PressureData{pressT, pressure}, TemperatureData{tempT, temperature} + { + } + + static std::string header() + { + return "pressureTimestamp,pressure,temperatureTimestamp,temperature\n "; + } + + void print(std::ostream& os) const + { + os << pressureTimestamp << "," << pressure << "," + << temperatureTimestamp << "," << temperature << "\n"; + } +}; + +} // namespace Boardcore diff --git a/src/shared/sensors/LPS28DFW/LPS28DFWDefs.h b/src/shared/sensors/LPS28DFW/LPS28DFWDefs.h new file mode 100644 index 0000000000000000000000000000000000000000..d189ca321f48f16846cc539365eaad064dbef74c --- /dev/null +++ b/src/shared/sensors/LPS28DFW/LPS28DFWDefs.h @@ -0,0 +1,151 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * 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 + +namespace Boardcore +{ + +/** + * @brief Various LPS28DFW register/enums definitions. + */ +namespace LPS28DFWDefs +{ +static const uint16_t lsp28dfwAddress0{0b1011100}; +static const uint16_t lsp28dfwAddress1{0b1011101}; +static const float WHO_AM_I_VALUE{0xB4}; ///< Device Who am I value +static const float pressureSensitivity1260hPa{ + 40.96}; ///< pressure sensitivity with FSR at 1260 hPa [LSB/Pa] +static const float pressureSensitivity4060hPa{ + 20.48}; ///< pressure sensitivity with FSR at 4060 hPa [LSB/Pa] +static const float temperatureSensitivity{ + 100}; ///< temperature sensitivity [LSB/°C] + +enum Registers : uint8_t +{ + INTERRUPT_CFG = 0x0B, ///< Interrupt mode for pressure acquisition + + THS_P_L = 0x0C, ///< User-defined threshold LSB register + THS_P_H = 0x0D, ///< User-defined threshold MSB register + + IF_CTRL = 0x0E, ///< Interface control register + + WHO_AM_I = 0x0F, ///< Device Who am I register + + CTRL_REG1 = 0x10, ///< Control Register 1 [ODR, AVG] + CTRL_REG2 = 0x11, ///< Control Register 2 + CTRL_REG3 = 0x12, ///< Control Register 3 + CTRL_REG4 = 0x13, ///< Control Register 4 + + FIFO_CTRL = 0x14, ///< FIFO control register + FIFO_WTM = 0x15, ///< FIFO threshold setting register + + REF_P_L = 0x16, ///< Reference pressure LSB data + REF_P_H = 0x17, ///< Reference pressure MSB data + + RPDS_L = 0x1a, ///< Pressure offset LSB data + RPDS_H = 0x1b, ///< Pressure offset HSB data + + INT_SOURCE = 0x24, ///< Interrupt source register + + FIFO_STATUS1 = 0x25, ///< FIFO status register 1 + FIFO_STATUS2 = 0x26, ///< FIFO status register 2 + + STATUS = 0x27, ///< Status register + + PRESS_OUT_XL = 0x28, ///< Pressure output value LSB data + PRESS_OUT_L = 0x29, ///< Pressure output value middle data + PRESS_OUT_H = 0x2a, ///< Pressure output value MSB data + + TEMP_OUT_L = 0x2b, ///< Temperature output value LSB data + TEMP_OUT_H = 0x2c, ///< Temperature output value MSB data + + FIFO_DATA_OUT_PRESS_XL = 0x78, ///< FIFO pressure output LSB data + FIFO_DATA_OUT_PRESS_L = 0x79, ///< FIFO pressure output middle data + FIFO_DATA_OUT_PRESS_H = 0x7a, ///< FIFO pressure output MSB data +}; + +enum CTRL_REG2 : uint8_t +{ + ONE_SHOT_START = (0b1 << 0), ///< Enable one-shot mode + SWRESET = (0b1 << 2), ///< Software reset + BDU = (0b1 << 3), ///< Block data update + EN_LPFP = (0b1 << 4), ///< Enable low-pass filter on pressure data + LFPF_CFG = (0b1 << 5), ///< Low-pass filter configuration + FS_MODE = (0b1 << 6), ///< Full-scale selection + BOOT = (0b1 << 7) ///< Reboot memory content +}; + +enum CTRL_REG3 : uint8_t +{ + IF_ADD_INC = + (0b1 << 0), ///< Increment register during a multiple byte access + PP_OD = (0b1 << 1), ///< Push-pull/open-drain selection on interrupt pin + INT_H_L = (0b1 << 3) ///< Select interrupt active-high, active-low +}; + +enum CTRL_REG4 : uint8_t +{ + INT_F_OVR = (0b1 << 0), ///< FIFO overrun status on INT_DRDY pin + INT_F_WTM = (0b1 << 1), ///< FIFO threshold status on INT_DRDY pin + INT_F_FULL = (0b1 << 2), ///< FIFO full flag on INT_DRDY pin + INT_EN = (0b1 << 4), ///< Interrupt signal on INT_DRDY pin + DRDY = (0b1 << 5), ///< Date-ready signal on INT_DRDY pin + DRDY_PLS = (0b1 << 6) ///< Data-ready pulsed on INT_DRDY pin +}; + +enum FIFO_CTRL : uint8_t +{ + BYPASS = 0b000, + FIFO = 0b001, + CONTINUOUS = 0b010, + BYPASS_TO_FIFO = 0b101, + BYPASS_TO_CONTINUOUS = 0b110, + CONTINUOUS_TO_FIFO = 0b111, + STOP_ON_WTM = (0b1 << 3) ///< Stop-on-FIFO watermark +}; + +enum INT_SOURCE : uint8_t +{ + PH = (0b1 << 0), ///< Differential pressure High + PL = (0b1 << 1), ///< Differential pressure Low + IA = (0b1 << 2), ///< Interrupt active + BOOT_ON = (0b1 << 7) ///< Reboot phase is running +}; + +enum FIFO_STATUS2 : uint8_t +{ + FIFO_FULL_IA = (0b1 << 5), ///< FIFO full status + FIFO_OVR_IA = (0b1 << 6), ///< FIFO overrun status + FIFO_WTM_IA = (0b1 << 7) ///< FIFO threshold status +}; + +enum STATUS : uint8_t +{ + P_DA = (0b1 << 0), ///< Pressure data available + T_DA = (0b1 << 1), ///< Temperature data available + P_OR = (0b1 << 4), ///< Pressure data overrun + T_OR = (0b1 << 5) ///< Temperature data overrun +}; + +} // namespace LPS28DFWDefs +} // namespace Boardcore \ No newline at end of file diff --git a/src/tests/sensors/test-lps28dfw.cpp b/src/tests/sensors/test-lps28dfw.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e5a287d1fb5cd276323b9ce20206ca1c04575ef1 --- /dev/null +++ b/src/tests/sensors/test-lps28dfw.cpp @@ -0,0 +1,181 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * 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 <drivers/interrupt/external_interrupts.h> + +#include <iostream> + +#include "miosix.h" +#include "sensors/LPS28DFW/LPS28DFW.h" +#include "sensors/LPS28DFW/LPS28DFWData.h" +#include "string" +#include "string.h" +#include "thread" + +using namespace std; +using namespace miosix; +using namespace Boardcore; + +// I2C1 +typedef Gpio<GPIOB_BASE, 8> i1scl; +typedef Gpio<GPIOB_BASE, 9> i1sda; + +uint8_t nSamples = 10; +/** + * ONE_SHOT | AVG4 : 161.6 us/samp (W1+R1+W2+W1+R5) @1Hz: 1.7 uA [FROM DS] + * ONE_SHOT | AVG512 : 162 us/samp (W1+R1+W2+W1+R5) @1Hz: 32.2 uA [FROM DS] + * ODR1 | AVG4 : 127.8 us/samp (W1+R1+W1+R5) @1Hz: 2.5 uA [FROM DS] + * ODR1 | AVG512 : 127.8 us/samp (W1+R1+W1+R5) @1Hz: 32.8 uA [FROM DS] + */ +miosix::Thread *waiting = 0; +bool sampleAvailable = false; +void __attribute__((used)) EXTI5_IRQHandlerImpl() +{ + if (waiting) + { + sampleAvailable = true; + waiting->IRQwakeup(); + } +} + +void sampleOneShotMode(I2C &i2c) +{ + printf("Start One-Shot\n"); + + // Setting up the Sensor + LPS28DFW::SensorConfig lps28dfwConfig{ + false, LPS28DFW::FullScaleRange::FS_1260, LPS28DFW::AVG::AVG_64, + LPS28DFW::ODR::ONE_SHOT, false}; + LPS28DFW lps28dfw(i2c, lps28dfwConfig); + if (!lps28dfw.init()) + { + printf("Error initialization of sensor\n"); + return; + } + + for (uint8_t i = 0; i < nSamples; i++) + { + lps28dfw.sample(); + + if (lps28dfw.getLastError() == SensorErrors::NO_ERRORS) + { + lps28dfw.getLastSample().print(std::cout); + } + else + { + printf("Error: %d\n", lps28dfw.getLastError()); + } + Thread::sleep(100); + } + + printf("End One-Shot\n"); +} + +void sampleContinuousMode(I2C &i2c) +{ + printf("Start Continuous\n"); + + // Setting up the Sensor + LPS28DFW::SensorConfig lps28dfwConfig{ + false, LPS28DFW::FullScaleRange::FS_1260, LPS28DFW::AVG::AVG_64, + LPS28DFW::ODR::ODR_10, false}; + LPS28DFW lps28dfw(i2c, lps28dfwConfig); + if (!lps28dfw.init()) + { + printf("Error initialization of sensor\n"); + return; + } + + for (uint8_t i = 0; i < nSamples; i++) + { + Thread::sleep(100); + + lps28dfw.sample(); + + if (lps28dfw.getLastError() == SensorErrors::NO_ERRORS) + { + lps28dfw.getLastSample().print(std::cout); + } + else + { + printf("Error: %d\n", lps28dfw.getLastError()); + } + } + + printf("End Continuous\n"); +} + +void sampleInterruptMode(I2C &i2c) +{ + printf("Start Interrupt\n"); + // Setting up the Sensor + LPS28DFW::SensorConfig lps28dfwConfig{ + false, LPS28DFW::FullScaleRange::FS_1260, LPS28DFW::AVG::AVG_64, + LPS28DFW::ODR::ODR_10, true}; + LPS28DFW lps28dfw(i2c, lps28dfwConfig); + if (!lps28dfw.init()) + { + printf("Error initialization of sensor\n"); + return; + } + + for (uint8_t i = 0; i < nSamples; i++) + { + + while (!sampleAvailable) + { + waiting->wait(); + } + + sampleAvailable = false; + lps28dfw.sample(); + + if (lps28dfw.getLastError() == SensorErrors::NO_ERRORS) + { + lps28dfw.getLastSample().print(std::cout); + } + else + { + printf("Error: %d\n", lps28dfw.getLastError()); + } + } + + printf("End Interrupt\n"); +} + +int main() +{ + I2C i2c(I2C1, i1scl::getPin(), i1sda::getPin()); + + waiting = Thread::getCurrentThread(); + enableExternalInterrupt(GPIOE_BASE, 5, InterruptTrigger::RISING_EDGE); + + for (;;) + { + printf("\nNew set of samples\n"); + sampleOneShotMode(i2c); + sampleContinuousMode(i2c); + sampleInterruptMode(i2c); + } + + return 0; +}