From 90434971dc039c61a4809f0c1af25dc689334bf3 Mon Sep 17 00:00:00 2001 From: Enzo PB Date: Wed, 10 Sep 2025 19:22:12 +0200 Subject: [PATCH 1/4] DPS310 baro library & gizmo --- src/bar/BarGizmoDPS310.h | 33 ++ src/bar/bar.cpp | 6 + src/bar/dps3/LICENSE.md | 21 + src/bar/dps3/README.md | 50 ++ src/bar/dps3/src/Dps3xx.cpp | 192 ++++++ src/bar/dps3/src/Dps3xx.h | 39 ++ src/bar/dps3/src/DpsClass.cpp | 818 ++++++++++++++++++++++++++ src/bar/dps3/src/DpsClass.h | 500 ++++++++++++++++ src/bar/dps3/src/util/DpsRegister.h | 19 + src/bar/dps3/src/util/dps3xx_config.h | 49 ++ src/bar/dps3/src/util/dps_config.h | 127 ++++ src/cfg/cfg.h | 2 +- 12 files changed, 1855 insertions(+), 1 deletion(-) create mode 100644 src/bar/BarGizmoDPS310.h create mode 100644 src/bar/dps3/LICENSE.md create mode 100644 src/bar/dps3/README.md create mode 100644 src/bar/dps3/src/Dps3xx.cpp create mode 100644 src/bar/dps3/src/Dps3xx.h create mode 100644 src/bar/dps3/src/DpsClass.cpp create mode 100644 src/bar/dps3/src/DpsClass.h create mode 100644 src/bar/dps3/src/util/DpsRegister.h create mode 100644 src/bar/dps3/src/util/dps3xx_config.h create mode 100644 src/bar/dps3/src/util/dps_config.h diff --git a/src/bar/BarGizmoDPS310.h b/src/bar/BarGizmoDPS310.h new file mode 100644 index 0000000..b64e3f8 --- /dev/null +++ b/src/bar/BarGizmoDPS310.h @@ -0,0 +1,33 @@ +#pragma once + +#include "bar.h" +#include "../hal/MF_I2C.h" +#include "dps3/src/Dps3xx.h" + +class BarGizmoDPS310: public BarGizmo { +protected: + Dps3xx pressureSensor; +public: + BarGizmoDPS310(MF_I2C *i2c, int8_t i2c_adr, uint32_t sampleRate) { + if (i2c_adr == 0) { + i2c_adr = 0x77; + } + pressureSensor.begin(*i2c, i2c_adr); + } + + bool update(float *press, float *temp) override { + int16_t ret; + + ret = pressureSensor.measurePressureOnce(*press); + if (ret != 0) { + return false; + } + + ret = pressureSensor.measureTempOnce(*temp); + if (ret != 0) { + return false; + } + + return true; + } +}; diff --git a/src/bar/bar.cpp b/src/bar/bar.cpp index 81707f3..122ee41 100644 --- a/src/bar/bar.cpp +++ b/src/bar/bar.cpp @@ -31,6 +31,7 @@ SOFTWARE. #include "BarGizmoMS5611.h" #include "BarGizmoHP203B.h" #include "BarGizmoBMP580.h" +#include "BarGizmoDPS310.h" #include //create global module instance @@ -78,6 +79,11 @@ int Bar::setup() { gizmo = new BarGizmoBMP580(config.i2c_bus, config.i2c_adr, config.sampleRate); } break; + case Cfg::bar_gizmo_enum::mf_DPS310 : + if(config.i2c_bus) { + gizmo = new BarGizmoDPS310(config.i2c_bus, config.i2c_adr, config.sampleRate); + } + break; } //check gizmo diff --git a/src/bar/dps3/LICENSE.md b/src/bar/dps3/LICENSE.md new file mode 100644 index 0000000..656bd01 --- /dev/null +++ b/src/bar/dps3/LICENSE.md @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2022 Infineon Technologies AG + +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. \ No newline at end of file diff --git a/src/bar/dps3/README.md b/src/bar/dps3/README.md new file mode 100644 index 0000000..86c9941 --- /dev/null +++ b/src/bar/dps3/README.md @@ -0,0 +1,50 @@ +[![Compile examples](https://github.com/Infineon/arduino-xensiv-dps3xx/actions/workflows/compile_examples.yml/badge.svg)](https://github.com/Infineon/arduino-xensiv-dps3xx/actions/workflows/compile_examples.yml) + +# XENSIV™ Digital Pressure Sensor Arduino Library + +Arduino library of Infineon's [**XENSIV™ Digital Pressure Sensors (DPS3xx)**](https://www.infineon.com/cms/en/product/sensor/pressure-sensors/pressure-sensors-for-iot/). + + + +## Supported Products + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Products
XENSIV™ DPS310XENSIV™ DPS368
Shield2Go
XENSIV™ DPS310 Shield2GoXENSIV™ DPS368 Shield2Go
Kit 2Go
XENSIV™ DPS310 Kit 2GoXENSIV™ DPS368 Kit 2Go
+ +## Getting Started + +### Installation +To install this library in the Arduino IDE, please go to **Sketch** > **Include Library** > **Manage Libraries...** and search for the `XENSIV Digital Pressure Sensor` library by `Infineon Technologies` in the Arduino library manager. + +### Usage +Please see the example sketches in the `/examples` directory in this repository to learn more about the usage of the library. Especially, take care of the respective SPI and I²C configuration of the sensor. You can find more information in the [Wiki](https://github.com/Infineon/arduino-xensiv-dps3xx/wiki). + +## License +See the [LICENSE](LICENSE.md) file for more details. diff --git a/src/bar/dps3/src/Dps3xx.cpp b/src/bar/dps3/src/Dps3xx.cpp new file mode 100644 index 0000000..29a48c8 --- /dev/null +++ b/src/bar/dps3/src/Dps3xx.cpp @@ -0,0 +1,192 @@ +#include "Dps3xx.h" + +using namespace dps; +using namespace dps3xx; + +int16_t Dps3xx::getContResults(float *tempBuffer, + uint8_t &tempCount, + float *prsBuffer, + uint8_t &prsCount) +{ + return DpsClass::getContResults(tempBuffer, tempCount, prsBuffer, prsCount, registers[FIFO_EMPTY]); +} + +int16_t Dps3xx::setInterruptSources(uint8_t intr_source, uint8_t polarity) +{ + #ifndef DPS_DISABLESPI + // Interrupts are not supported with 4 Wire SPI + if (!m_SpiI2c & !m_threeWire) + { + return DPS__FAIL_UNKNOWN; + } + #endif + return writeByteBitfield(intr_source, registers[INT_SEL]) || writeByteBitfield(polarity, registers[INT_HL]); +} + +void Dps3xx::init(void) +{ + int16_t prodId = readByteBitfield(registers[PROD_ID]); + if (prodId < 0) + { + // Connected device is not a Dps3xx + m_initFail = 1U; + return; + } + m_productID = prodId; + + int16_t revId = readByteBitfield(registers[REV_ID]); + if (revId < 0) + { + m_initFail = 1U; + return; + } + m_revisionID = revId; + + // find out which temperature sensor is calibrated with coefficients... + int16_t sensor = readByteBitfield(registers[TEMP_SENSORREC]); + if (sensor < 0) + { + m_initFail = 1U; + return; + } + + //...and use this sensor for temperature measurement + m_tempSensor = sensor; + if (writeByteBitfield((uint8_t)sensor, registers[TEMP_SENSOR]) < 0) + { + m_initFail = 1U; + return; + } + + // read coefficients + if (readcoeffs() < 0) + { + m_initFail = 1U; + return; + } + + // set to standby for further configuration + standby(); + + // set measurement precision and rate to standard values; + configTemp(DPS__MEASUREMENT_RATE_4, DPS__OVERSAMPLING_RATE_8); + configPressure(DPS__MEASUREMENT_RATE_4, DPS__OVERSAMPLING_RATE_8); + + // perform a first temperature measurement + // the most recent temperature will be saved internally + // and used for compensation when calculating pressure + float trash; + measureTempOnce(trash); + + // make sure the Dps3xx is in standby after initialization + standby(); + + // Fix IC with a fuse bit problem, which lead to a wrong temperature + // Should not affect ICs without this problem + correctTemp(); +} + +int16_t Dps3xx::readcoeffs(void) +{ + // TODO: remove magic number + uint8_t buffer[18]; + // read COEF registers to buffer + int16_t ret = readBlock(coeffBlock, buffer); + if (!ret) + return DPS__FAIL_INIT_FAILED; + + // compose coefficients from buffer content + m_c0Half = ((uint32_t)buffer[0] << 4) | (((uint32_t)buffer[1] >> 4) & 0x0F); + getTwosComplement(&m_c0Half, 12); + // c0 is only used as c0*0.5, so c0_half is calculated immediately + m_c0Half = m_c0Half / 2U; + + // now do the same thing for all other coefficients + m_c1 = (((uint32_t)buffer[1] & 0x0F) << 8) | (uint32_t)buffer[2]; + getTwosComplement(&m_c1, 12); + m_c00 = ((uint32_t)buffer[3] << 12) | ((uint32_t)buffer[4] << 4) | (((uint32_t)buffer[5] >> 4) & 0x0F); + getTwosComplement(&m_c00, 20); + m_c10 = (((uint32_t)buffer[5] & 0x0F) << 16) | ((uint32_t)buffer[6] << 8) | (uint32_t)buffer[7]; + getTwosComplement(&m_c10, 20); + + m_c01 = ((uint32_t)buffer[8] << 8) | (uint32_t)buffer[9]; + getTwosComplement(&m_c01, 16); + + m_c11 = ((uint32_t)buffer[10] << 8) | (uint32_t)buffer[11]; + getTwosComplement(&m_c11, 16); + m_c20 = ((uint32_t)buffer[12] << 8) | (uint32_t)buffer[13]; + getTwosComplement(&m_c20, 16); + m_c21 = ((uint32_t)buffer[14] << 8) | (uint32_t)buffer[15]; + getTwosComplement(&m_c21, 16); + m_c30 = ((uint32_t)buffer[16] << 8) | (uint32_t)buffer[17]; + getTwosComplement(&m_c30, 16); + return DPS__SUCCEEDED; +} + +int16_t Dps3xx::configTemp(uint8_t tempMr, uint8_t tempOsr) +{ + int16_t ret = DpsClass::configTemp(tempMr, tempOsr); + + writeByteBitfield(m_tempSensor, registers[TEMP_SENSOR]); + // set TEMP SHIFT ENABLE if oversampling rate higher than eight(2^3) + if (tempOsr > DPS3xx__OSR_SE) + { + ret = writeByteBitfield(1U, registers[TEMP_SE]); + } + else + { + ret = writeByteBitfield(0U, registers[TEMP_SE]); + } + return ret; +} + +int16_t Dps3xx::configPressure(uint8_t prsMr, uint8_t prsOsr) +{ + int16_t ret = DpsClass::configPressure(prsMr, prsOsr); + // set PM SHIFT ENABLE if oversampling rate higher than eight(2^3) + if (prsOsr > DPS3xx__OSR_SE) + { + ret = writeByteBitfield(1U, registers[PRS_SE]); + } + else + { + ret = writeByteBitfield(0U, registers[PRS_SE]); + } + return ret; +} + +float Dps3xx::calcTemp(int32_t raw) +{ + float temp = raw; + + // scale temperature according to scaling table and oversampling + temp /= scaling_facts[m_tempOsr]; + + // update last measured temperature + // it will be used for pressure compensation + m_lastTempScal = temp; + + // Calculate compensated temperature + temp = m_c0Half + m_c1 * temp; + + return temp; +} + +float Dps3xx::calcPressure(int32_t raw) +{ + float prs = raw; + + // scale pressure according to scaling table and oversampling + prs /= scaling_facts[m_prsOsr]; + + // Calculate compensated pressure + prs = m_c00 + prs * (m_c10 + prs * (m_c20 + prs * m_c30)) + m_lastTempScal * (m_c01 + prs * (m_c11 + prs * m_c21)); + + // return pressure + return prs; +} + +int16_t Dps3xx::flushFIFO() +{ + return writeByteBitfield(1U, registers[FIFO_FL]); +} diff --git a/src/bar/dps3/src/Dps3xx.h b/src/bar/dps3/src/Dps3xx.h new file mode 100644 index 0000000..048775f --- /dev/null +++ b/src/bar/dps3/src/Dps3xx.h @@ -0,0 +1,39 @@ +#ifndef DPS3xx_H_INCLUDED +#define DPS3xx_H_INCLUDED + +#include "DpsClass.h" +#include "util/dps3xx_config.h" + +class Dps3xx : public DpsClass +{ +public: + int16_t getContResults(float *tempBuffer, uint8_t &tempCount, float *prsBuffer, uint8_t &prsCount); + + /** + * @brief Set the source of interrupt (FIFO full, measurement values ready) + * + * @param intr_source Interrupt source as defined by Interrupt_source_3xx_e + * @param polarity + * @return status code + */ + int16_t setInterruptSources(uint8_t intr_source, uint8_t polarity = 1); + +protected: + uint8_t m_tempSensor; + + // compensation coefficients + int32_t m_c0Half; + int32_t m_c1; + + /////// implement pure virtual functions /////// + + void init(void); + int16_t configTemp(uint8_t temp_mr, uint8_t temp_osr); + int16_t configPressure(uint8_t prs_mr, uint8_t prs_osr); + int16_t readcoeffs(void); + int16_t flushFIFO(); + float calcTemp(int32_t raw); + float calcPressure(int32_t raw); +}; + +#endif \ No newline at end of file diff --git a/src/bar/dps3/src/DpsClass.cpp b/src/bar/dps3/src/DpsClass.cpp new file mode 100644 index 0000000..a401631 --- /dev/null +++ b/src/bar/dps3/src/DpsClass.cpp @@ -0,0 +1,818 @@ +#include "DpsClass.h" +#include "../../../hal/MF_I2C.h" +using namespace dps; + +const int32_t DpsClass::scaling_facts[DPS__NUM_OF_SCAL_FACTS] = {524288, 1572864, 3670016, 7864320, 253952, 516096, 1040384, 2088960}; + +//////// Constructor, Destructor, begin, end //////// + +DpsClass::DpsClass(void) +{ + // assume that initialization has failed before it has been done + m_initFail = 1U; +} + +DpsClass::~DpsClass(void) +{ + end(); +} + +void DpsClass::begin(MF_I2C &bus) +{ + begin(bus, DPS__STD_SLAVE_ADDRESS); +} + +void DpsClass::begin(MF_I2C &bus, uint8_t slaveAddress) +{ + // this flag will show if the initialization was successful + m_initFail = 0U; + + // Set I2C bus connection + m_SpiI2c = 1U; + m_i2cbus = &bus; + m_slaveAddress = slaveAddress; + + // Init bus + m_i2cbus->begin(); + + delay(50); // startup time of Dps3xx + + init(); +} + +#ifndef DPS_DISABLESPI +void DpsClass::begin(SPIClass &bus, int32_t chipSelect) +{ + begin(bus, chipSelect, 0U); +} +#endif + +#ifndef DPS_DISABLESPI +void DpsClass::begin(SPIClass &bus, int32_t chipSelect, uint8_t threeWire) +{ + // this flag will show if the initialization was successful + m_initFail = 0U; + + // Set SPI bus connection + m_SpiI2c = 0U; + m_spibus = &bus; + m_chipSelect = chipSelect; + + // Init bus + m_spibus->begin(); + + // Configure the SPI settings for the device + SPISettings settings(DPS3xx__SPI_MAX_FREQ, MSBFIRST, SPI_MODE3); + + // Start an SPI transaction to configure the device + m_spibus->beginTransaction(settings); + + pinMode(m_chipSelect, OUTPUT); + digitalWrite(m_chipSelect, HIGH); + + delay(50); // startup time of Dps3xx + + // End the SPI transaction + m_spibus->endTransaction(); + + // Switch to 3-wire mode if necessary + // do not use writeByteBitfield or check option to set SPI mode! + // Reading is not possible until SPI-mode is valid + if (threeWire) + { + m_threeWire = 1U; + m_spibus->beginTransaction(settings); // Ensure SPI transaction for 3-wire mode + if (writeByte(DPS3xx__REG_ADR_SPI3W, DPS3xx__REG_CONTENT_SPI3W)) + { + m_spibus->endTransaction(); // End transaction + m_initFail = 1U; + return; + } + m_spibus->endTransaction(); // End transaction + } + + init(); +} +#endif + +void DpsClass::end(void) +{ + standby(); +} + +//////// Declaration of other public functions starts here //////// + +uint8_t DpsClass::getProductId(void) +{ + return m_productID; +} + +uint8_t DpsClass::getRevisionId(void) +{ + return m_revisionID; +} + +int16_t DpsClass::getContResults(float *tempBuffer, + uint8_t &tempCount, + float *prsBuffer, + uint8_t &prsCount, RegMask_t fifo_empty_reg) +{ + if (m_initFail) + { + return DPS__FAIL_INIT_FAILED; + } + // abort if device is not in background mode + if (!(m_opMode & 0x04)) + { + return DPS__FAIL_TOOBUSY; + } + + if (!tempBuffer || !prsBuffer) + { + return DPS__FAIL_UNKNOWN; + } + tempCount = 0U; + prsCount = 0U; + + // while FIFO is not empty + while (readByteBitfield(fifo_empty_reg) == 0) + { + int32_t raw_result; + float result; + // read next result from FIFO + int16_t type = getFIFOvalue(&raw_result); + switch (type) + { + case 0: // temperature + if (tempCount < DPS__FIFO_SIZE) + { + result = calcTemp(raw_result); + tempBuffer[tempCount++] = result; + } + break; + case 1: // pressure + if (prsCount < DPS__FIFO_SIZE) + { + result = calcPressure(raw_result); + prsBuffer[prsCount++] = result; + } + break; + case -1: // read failed + break; + } + } + return DPS__SUCCEEDED; +} + +int16_t DpsClass::getSingleResult(float &result) +{ + // abort if initialization failed + if (m_initFail) + { + return DPS__FAIL_INIT_FAILED; + } + + // read finished bit for current opMode + int16_t rdy; + switch (m_opMode) + { + case CMD_TEMP: // temperature + rdy = readByteBitfield(config_registers[TEMP_RDY]); + break; + case CMD_PRS: // pressure + rdy = readByteBitfield(config_registers[PRS_RDY]); + break; + default: // DPS3xx not in command mode + return DPS__FAIL_TOOBUSY; + } + // read new measurement result + switch (rdy) + { + case DPS__FAIL_UNKNOWN: // could not read ready flag + return DPS__FAIL_UNKNOWN; + case 0: // ready flag not set, measurement still in progress + return DPS__FAIL_UNFINISHED; + case 1: // measurement ready, expected case + Mode oldMode = m_opMode; + m_opMode = IDLE; // opcode was automatically reset by DPS3xx + int32_t raw_val; + switch (oldMode) + { + case CMD_TEMP: // temperature + getRawResult(&raw_val, registerBlocks[TEMP]); + result = calcTemp(raw_val); + return DPS__SUCCEEDED; // TODO + case CMD_PRS: // pressure + getRawResult(&raw_val, registerBlocks[PRS]); + result = calcPressure(raw_val); + return DPS__SUCCEEDED; // TODO + default: + return DPS__FAIL_UNKNOWN; // should already be filtered above + } + } + return DPS__FAIL_UNKNOWN; +} + +int16_t DpsClass::measureTempOnce(float &result) +{ + return measureTempOnce(result, m_tempOsr); +} + +int16_t DpsClass::measureTempOnce(float &result, uint8_t oversamplingRate) +{ + // Start measurement + int16_t ret = startMeasureTempOnce(oversamplingRate); + if (ret != DPS__SUCCEEDED) + { + return ret; + } + + // wait until measurement is finished + delay(calcBusyTime(0U, m_tempOsr) / DPS__BUSYTIME_SCALING); + delay(DPS3xx__BUSYTIME_FAILSAFE); + + ret = getSingleResult(result); + if (ret != DPS__SUCCEEDED) + { + standby(); + } + return ret; +} + +int16_t DpsClass::startMeasureTempOnce(void) +{ + return startMeasureTempOnce(m_tempOsr); +} + +int16_t DpsClass::startMeasureTempOnce(uint8_t oversamplingRate) +{ + // abort if initialization failed + if (m_initFail) + { + return DPS__FAIL_INIT_FAILED; + } + // abort if device is not in idling mode + if (m_opMode != IDLE) + { + return DPS__FAIL_TOOBUSY; + } + + if (oversamplingRate != m_tempOsr) + { + // configuration of oversampling rate + if (configTemp(0U, oversamplingRate) != DPS__SUCCEEDED) + { + return DPS__FAIL_UNKNOWN; + } + } + + // set device to temperature measuring mode + return setOpMode(CMD_TEMP); +} + +int16_t DpsClass::measurePressureOnce(float &result) +{ + return measurePressureOnce(result, m_prsOsr); +} + +int16_t DpsClass::measurePressureOnce(float &result, uint8_t oversamplingRate) +{ + // start the measurement + int16_t ret = startMeasurePressureOnce(oversamplingRate); + if (ret != DPS__SUCCEEDED) + { + return ret; + } + + // wait until measurement is finished + delay(calcBusyTime(0U, m_prsOsr) / DPS__BUSYTIME_SCALING); + delay(DPS3xx__BUSYTIME_FAILSAFE); + + ret = getSingleResult(result); + if (ret != DPS__SUCCEEDED) + { + standby(); + } + return ret; +} + +int16_t DpsClass::startMeasurePressureOnce(void) +{ + return startMeasurePressureOnce(m_prsOsr); +} + +int16_t DpsClass::startMeasurePressureOnce(uint8_t oversamplingRate) +{ + // abort if initialization failed + if (m_initFail) + { + return DPS__FAIL_INIT_FAILED; + } + // abort if device is not in idling mode + if (m_opMode != IDLE) + { + return DPS__FAIL_TOOBUSY; + } + // configuration of oversampling rate, lowest measure rate to avoid conflicts + if (oversamplingRate != m_prsOsr) + { + if (configPressure(0U, oversamplingRate)) + { + return DPS__FAIL_UNKNOWN; + } + } + // set device to pressure measuring mode + return setOpMode(CMD_PRS); +} + +int16_t DpsClass::startMeasureTempCont(uint8_t measureRate, uint8_t oversamplingRate) +{ + // abort if initialization failed + if (m_initFail) + { + return DPS__FAIL_INIT_FAILED; + } + // abort if device is not in idling mode + if (m_opMode != IDLE) + { + return DPS__FAIL_TOOBUSY; + } + // abort if speed and precision are too high + if (calcBusyTime(measureRate, oversamplingRate) >= DPS3xx__MAX_BUSYTIME) + { + return DPS__FAIL_UNFINISHED; + } + // update precision and measuring rate + if (configTemp(measureRate, oversamplingRate)) + { + return DPS__FAIL_UNKNOWN; + } + + if (enableFIFO()) + { + return DPS__FAIL_UNKNOWN; + } + // Start measuring in background mode + if (DpsClass::setOpMode(CONT_TMP)) + { + return DPS__FAIL_UNKNOWN; + } + return DPS__SUCCEEDED; +} + +int16_t DpsClass::startMeasurePressureCont(uint8_t measureRate, uint8_t oversamplingRate) +{ + // abort if initialization failed + if (m_initFail) + { + return DPS__FAIL_INIT_FAILED; + } + // abort if device is not in idling mode + if (m_opMode != IDLE) + { + return DPS__FAIL_TOOBUSY; + } + // abort if speed and precision are too high + if (calcBusyTime(measureRate, oversamplingRate) >= DPS3xx__MAX_BUSYTIME) + { + return DPS__FAIL_UNFINISHED; + } + // update precision and measuring rate + if (configPressure(measureRate, oversamplingRate)) + return DPS__FAIL_UNKNOWN; + // enable result FIFO + if (enableFIFO()) + { + return DPS__FAIL_UNKNOWN; + } + // Start measuring in background mode + if (DpsClass::setOpMode(CONT_PRS)) + { + return DPS__FAIL_UNKNOWN; + } + return DPS__SUCCEEDED; +} + +int16_t DpsClass::startMeasureBothCont(uint8_t tempMr, + uint8_t tempOsr, + uint8_t prsMr, + uint8_t prsOsr) +{ + // abort if initialization failed + if (m_initFail) + { + return DPS__FAIL_INIT_FAILED; + } + // abort if device is not in idling mode + if (m_opMode != IDLE) + { + return DPS__FAIL_TOOBUSY; + } + // abort if speed and precision are too high + if (calcBusyTime(tempMr, tempOsr) + calcBusyTime(prsMr, prsOsr) >= DPS3xx__MAX_BUSYTIME) + { + return DPS__FAIL_UNFINISHED; + } + // update precision and measuring rate + if (configTemp(tempMr, tempOsr)) + { + return DPS__FAIL_UNKNOWN; + } + // update precision and measuring rate + if (configPressure(prsMr, prsOsr)) + return DPS__FAIL_UNKNOWN; + // enable result FIFO + if (enableFIFO()) + { + return DPS__FAIL_UNKNOWN; + } + // Start measuring in background mode + if (setOpMode(CONT_BOTH)) + { + return DPS__FAIL_UNKNOWN; + } + return DPS__SUCCEEDED; +} + +int16_t DpsClass::standby(void) +{ + // abort if initialization failed + if (m_initFail) + { + return DPS__FAIL_INIT_FAILED; + } + // set device to idling mode + int16_t ret = setOpMode(IDLE); + if (ret != DPS__SUCCEEDED) + { + return ret; + } + ret = disableFIFO(); + return ret; +} + +int16_t DpsClass::correctTemp(void) +{ + if (m_initFail) + { + return DPS__FAIL_INIT_FAILED; + } + writeByte(0x0E, 0xA5); + writeByte(0x0F, 0x96); + writeByte(0x62, 0x02); + writeByte(0x0E, 0x00); + writeByte(0x0F, 0x00); + + // perform a first temperature measurement (again) + // the most recent temperature will be saved internally + // and used for compensation when calculating pressure + float trash; + measureTempOnce(trash); + + return DPS__SUCCEEDED; +} + +int16_t DpsClass::getIntStatusFifoFull(void) +{ + return readByteBitfield(config_registers[INT_FLAG_FIFO]); +} + +int16_t DpsClass::getIntStatusTempReady(void) +{ + return readByteBitfield(config_registers[INT_FLAG_TEMP]); +} + +int16_t DpsClass::getIntStatusPrsReady(void) +{ + return readByteBitfield(config_registers[INT_FLAG_PRS]); +} + +//////// Declaration of private functions starts here //////// + +int16_t DpsClass::setOpMode(uint8_t opMode) +{ + if (writeByteBitfield(opMode, config_registers[MSR_CTRL]) == -1) + { + return DPS__FAIL_UNKNOWN; + } + m_opMode = (Mode)opMode; + return DPS__SUCCEEDED; +} + +int16_t DpsClass::configTemp(uint8_t tempMr, uint8_t tempOsr) +{ + tempMr &= 0x07; + tempOsr &= 0x07; + // two accesses to the same register; for readability + int16_t ret = writeByteBitfield(tempMr, config_registers[TEMP_MR]); + ret = writeByteBitfield(tempOsr, config_registers[TEMP_OSR]); + + // abort immediately on fail + if (ret != DPS__SUCCEEDED) + { + return DPS__FAIL_UNKNOWN; + } + m_tempMr = tempMr; + m_tempOsr = tempOsr; + return DPS__SUCCEEDED; +} + +int16_t DpsClass::configPressure(uint8_t prsMr, uint8_t prsOsr) +{ + prsMr &= 0x07; + prsOsr &= 0x07; + int16_t ret = writeByteBitfield(prsMr, config_registers[PRS_MR]); + ret = writeByteBitfield(prsOsr, config_registers[PRS_OSR]); + + // abort immediately on fail + if (ret != DPS__SUCCEEDED) + { + return DPS__FAIL_UNKNOWN; + } + m_prsMr = prsMr; + m_prsOsr = prsOsr; + return DPS__SUCCEEDED; + +} + +int16_t DpsClass::enableFIFO() +{ + return writeByteBitfield(1U, config_registers[FIFO_EN]); +} + +int16_t DpsClass::disableFIFO() +{ + int16_t ret = flushFIFO(); + ret = writeByteBitfield(0U, config_registers[FIFO_EN]); + return ret; +} + +uint16_t DpsClass::calcBusyTime(uint16_t mr, uint16_t osr) +{ + // formula from datasheet (optimized) + return ((uint32_t)20U << mr) + ((uint32_t)16U << (osr + mr)); +} + +int16_t DpsClass::getFIFOvalue(int32_t *value) +{ + uint8_t buffer[DPS__RESULT_BLOCK_LENGTH] = {0}; + + // abort on invalid argument or failed block reading + if (value == NULL || readBlock(registerBlocks[PRS], buffer) != DPS__RESULT_BLOCK_LENGTH) + return DPS__FAIL_UNKNOWN; + *value = (uint32_t)buffer[0] << 16 | (uint32_t)buffer[1] << 8 | (uint32_t)buffer[2]; + getTwosComplement(value, 24); + return buffer[2] & 0x01; +} + +int16_t DpsClass::readByte(uint8_t regAddress) +{ +#ifndef DPS_DISABLESPI + // delegate to specialized function if Dps3xx is connected via SPI + if (m_SpiI2c == 0) + { + return readByteSPI(regAddress); + } +#endif + + m_i2cbus->beginTransmission(m_slaveAddress); + m_i2cbus->write(regAddress); + m_i2cbus->endTransmission(false); + // request 1 byte from slave + if (m_i2cbus->requestFrom(m_slaveAddress, (uint8_t)1, (uint8_t)1) > 0) + { + return m_i2cbus->read(); // return this byte on success + } + else + { + return DPS__FAIL_UNKNOWN; // if 0 bytes were read successfully + } +} + +#ifndef DPS_DISABLESPI +int16_t DpsClass::readByteSPI(uint8_t regAddress) +{ + // this function is only made for communication via SPI + if (m_SpiI2c != 0) + { + return DPS__FAIL_UNKNOWN; + } + // mask regAddress + regAddress &= ~DPS3xx__SPI_RW_MASK; + // reserve and initialize bus + m_spibus->beginTransaction(SPISettings(DPS3xx__SPI_MAX_FREQ, + MSBFIRST, + SPI_MODE3)); + // enable ChipSelect for Dps3xx + digitalWrite(m_chipSelect, LOW); + // send address with read command to Dps3xx + m_spibus->transfer(regAddress | DPS3xx__SPI_READ_CMD); + // receive register content from Dps3xx + uint8_t ret = m_spibus->transfer(0xFF); // send a dummy byte while receiving + // disable ChipSelect for Dps3xx + digitalWrite(m_chipSelect, HIGH); + // close current SPI transaction + m_spibus->endTransaction(); + // return received data + return ret; +} +#endif + +#ifndef DPS_DISABLESPI +int16_t DpsClass::readBlockSPI(RegBlock_t regBlock, uint8_t *buffer) +{ + // this function is only made for communication via SPI + if (m_SpiI2c != 0) + { + return DPS__FAIL_UNKNOWN; + } + // do not read if there is no buffer + if (buffer == NULL) + { + return 0; // 0 bytes were read successfully + } + // mask regAddress + regBlock.regAddress &= ~DPS3xx__SPI_RW_MASK; + // reserve and initialize bus + m_spibus->beginTransaction(SPISettings(DPS3xx__SPI_MAX_FREQ, + MSBFIRST, + SPI_MODE3)); + // enable ChipSelect for Dps3xx + digitalWrite(m_chipSelect, LOW); + // send address with read command to Dps3xx + m_spibus->transfer(regBlock.regAddress | DPS3xx__SPI_READ_CMD); + + // receive register contents from Dps3xx + for (uint8_t count = 0; count < regBlock.length; count++) + { + buffer[count] = m_spibus->transfer(0xFF); // send a dummy byte while receiving + } + + // disable ChipSelect for Dps3xx + digitalWrite(m_chipSelect, HIGH); + // close current SPI transaction + m_spibus->endTransaction(); + // return received data + return regBlock.length; +} +#endif + +int16_t DpsClass::writeByte(uint8_t regAddress, uint8_t data) +{ + return writeByte(regAddress, data, 0U); +} + +int16_t DpsClass::writeByte(uint8_t regAddress, uint8_t data, uint8_t check) +{ +#ifndef DPS_DISABLESPI + // delegate to specialized function if Dps3xx is connected via SPI + if (m_SpiI2c == 0) + { + return writeByteSpi(regAddress, data, check); + } +#endif + m_i2cbus->beginTransmission(m_slaveAddress); + m_i2cbus->write(regAddress); // Write Register number to buffer + m_i2cbus->write(data); // Write data to buffer + if (m_i2cbus->endTransmission() != 0) // Send buffer content to slave + { + return DPS__FAIL_UNKNOWN; + } + else + { + if (check == 0) + return 0; // no checking + if (readByte(regAddress) == data) // check if desired by calling function + { + return DPS__SUCCEEDED; + } + else + { + return DPS__FAIL_UNKNOWN; + } + } +} + +#ifndef DPS_DISABLESPI +int16_t DpsClass::writeByteSpi(uint8_t regAddress, uint8_t data, uint8_t check) +{ + // this function is only made for communication via SPI + if (m_SpiI2c != 0) + { + return DPS__FAIL_UNKNOWN; + } + // mask regAddress + regAddress &= ~DPS3xx__SPI_RW_MASK; + // reserve and initialize bus + m_spibus->beginTransaction(SPISettings(DPS3xx__SPI_MAX_FREQ, + MSBFIRST, + SPI_MODE3)); + // enable ChipSelect for Dps3xx + digitalWrite(m_chipSelect, LOW); + // send address with read command to Dps3xx + m_spibus->transfer(regAddress | DPS3xx__SPI_WRITE_CMD); + + // write register content from Dps3xx + m_spibus->transfer(data); + + // disable ChipSelect for Dps3xx + digitalWrite(m_chipSelect, HIGH); + // close current SPI transaction + m_spibus->endTransaction(); + + // check if necessary + if (check == 0) + { + // no checking necessary + return DPS__SUCCEEDED; + } + // checking necessary + if (readByte(regAddress) == data) + { + // check passed + return DPS__SUCCEEDED; + } + else + { + // check failed + return DPS__FAIL_UNKNOWN; + } +} +#endif + +int16_t DpsClass::writeByteBitfield(uint8_t data, RegMask_t regMask) +{ + return writeByteBitfield(data, regMask.regAddress, regMask.mask, regMask.shift, 0U); +} + +int16_t DpsClass::writeByteBitfield(uint8_t data, + uint8_t regAddress, + uint8_t mask, + uint8_t shift, + uint8_t check) +{ + int16_t old = readByte(regAddress); + if (old < 0) + { + // fail while reading + return old; + } + return writeByte(regAddress, ((uint8_t)old & ~mask) | ((data << shift) & mask), check); +} + +int16_t DpsClass::readByteBitfield(RegMask_t regMask) +{ + int16_t ret = readByte(regMask.regAddress); + if (ret < 0) + { + return ret; + } + return (((uint8_t)ret) & regMask.mask) >> regMask.shift; +} + +int16_t DpsClass::readBlock(RegBlock_t regBlock, uint8_t *buffer) +{ +#ifndef DPS_DISABLESPI + // delegate to specialized function if Dps3xx is connected via SPI + if (m_SpiI2c == 0) + { + return readBlockSPI(regBlock, buffer); + } +#endif + // do not read if there is no buffer + if (buffer == NULL) + { + return 0; // 0 bytes read successfully + } + + m_i2cbus->beginTransmission(m_slaveAddress); + m_i2cbus->write(regBlock.regAddress); + m_i2cbus->endTransmission(false); + // request length bytes from slave + int16_t ret = m_i2cbus->requestFrom(m_slaveAddress, (uint8_t)regBlock.length, (uint8_t)1); + // read all received bytes to buffer + for (int16_t count = 0; count < ret; count++) + { + buffer[count] = m_i2cbus->read(); + } + return ret; +} + +void DpsClass::getTwosComplement(int32_t *raw, uint8_t length) +{ + if (*raw & ((uint32_t)1 << (length - 1))) + { + *raw -= (uint32_t)1 << length; + } +} + +int16_t DpsClass::getRawResult(int32_t *raw, RegBlock_t reg) +{ + uint8_t buffer[DPS__RESULT_BLOCK_LENGTH] = {0}; + if (readBlock(reg, buffer) != DPS__RESULT_BLOCK_LENGTH) + return DPS__FAIL_UNKNOWN; + + *raw = (uint32_t)buffer[0] << 16 | (uint32_t)buffer[1] << 8 | (uint32_t)buffer[2]; + getTwosComplement(raw, 24); + return DPS__SUCCEEDED; +} diff --git a/src/bar/dps3/src/DpsClass.h b/src/bar/dps3/src/DpsClass.h new file mode 100644 index 0000000..7380fd8 --- /dev/null +++ b/src/bar/dps3/src/DpsClass.h @@ -0,0 +1,500 @@ +/** + * Arduino library to control Dps3xx + * + * "Dps3xx" represents Infineon's high-sensitive pressure and temperature sensor. + * It measures in ranges of 300 - 1200 hPa and -40 and 85 °C. + * The sensor can be connected via SPI or I2C. + * It is able to perform single measurements + * or to perform continuous measurements of temperature and pressure at the same time, + * and stores the results in a FIFO to reduce bus communication. + * + * Have a look at the datasheet for more information. + */ + +#ifndef DPSCLASS_H_INCLUDED +#define DPSCLASS_H_INCLUDED + +#include + +// Disable SPI for currently not supported platforms. +#if defined(ARDUINO_ARCH_SAMD) || defined(ARDUINO_ARCH_RENESAS) +#define DPS_DISABLESPI +#endif + +#ifndef DPS_DISABLESPI +#include +#endif +#include +#include "util/dps_config.h" +#include "../../../hal/MF_I2C.h" + +class DpsClass +{ +public: + // constructor + DpsClass(void); + // destructor + ~DpsClass(void); + + /** + * I2C begin function with standard address + */ + void begin(MF_I2C &bus); + + /** + * Standard I2C begin function + * + * @param &bus: I2CBus which connects MC to the sensor + * @param slaveAddress: I2C address of the sensor (0x77 or 0x76) + */ + void begin(MF_I2C &bus, uint8_t slaveAddress); + +#ifndef DPS_DISABLESPI + /** + * SPI begin function for Dps3xx with 4-wire SPI + */ + void begin(SPIClass &bus, int32_t chipSelect); +#endif + +#ifndef DPS_DISABLESPI + /** + * Standard SPI begin function + * + * @param &bus: SPI bus which connects MC to Dps3xx + * @param chipSelect: Number of the CS line for the Dps3xx + * @param threeWire: 1 if Dps3xx is connected with 3-wire SPI + * 0 if Dps3xx is connected with 4-wire SPI (standard) + */ + void begin(SPIClass &bus, int32_t chipSelect, uint8_t threeWire); +#endif + + /** + * End function for Dps3xx + * Sets the sensor to idle mode + */ + void end(void); + + /** + * returns the Product ID of the connected Dps3xx sensor + */ + uint8_t getProductId(void); + + /** + * returns the Revision ID of the connected Dps3xx sensor + */ + uint8_t getRevisionId(void); + + /** + * Sets the Dps3xx to standby mode + * + * @return status code + */ + int16_t standby(void); + + /** + * performs one temperature measurement + * + * @param &result: reference to a float value where the result will be written + * @return status code + */ + int16_t measureTempOnce(float &result); + + /** + * performs one temperature measurement with specified oversamplingRate + * + * @param &result: reference to a float where the result will be written + * @param oversamplingRate: DPS__OVERSAMPLING_RATE_1, DPS__OVERSAMPLING_RATE_2, + * DPS__OVERSAMPLING_RATE_4 ... DPS__OVERSAMPLING_RATE_128, + * which are defined as integers 0 - 7 + * The number of measurements equals to 2^n, if the value written to + * the register field is n. 2^n internal measurements are combined to + * return a more exact measurement + * @return status code + */ + int16_t measureTempOnce(float &result, uint8_t oversamplingRate); + + /** + * starts a single temperature measurement + * + * @return status code + */ + int16_t startMeasureTempOnce(void); + + /** + * starts a single temperature measurement with specified oversamplingRate + * + * @param oversamplingRate: DPS__OVERSAMPLING_RATE_1, DPS__OVERSAMPLING_RATE_2, + * DPS__OVERSAMPLING_RATE_4 ... DPS__OVERSAMPLING_RATE_128, which are defined as integers 0 - 7 + * @return status code + */ + int16_t startMeasureTempOnce(uint8_t oversamplingRate); + + /** + * performs one pressure measurement + * + * @param &result: reference to a float value where the result will be written + * @return status code + */ + int16_t measurePressureOnce(float &result); + + /** + * performs one pressure measurement with specified oversamplingRate + * + * @param &result: reference to a float where the result will be written + * @param oversamplingRate: DPS__OVERSAMPLING_RATE_1, DPS__OVERSAMPLING_RATE_2, + * DPS__OVERSAMPLING_RATE_4 ... DPS__OVERSAMPLING_RATE_128 + * @return status code + */ + int16_t measurePressureOnce(float &result, uint8_t oversamplingRate); + + /** + * starts a single pressure measurement + * + * @return status code + */ + int16_t startMeasurePressureOnce(void); + + /** + * starts a single pressure measurement with specified oversamplingRate + * + * @param oversamplingRate: DPS__OVERSAMPLING_RATE_1, DPS__OVERSAMPLING_RATE_2, + * DPS__OVERSAMPLING_RATE_4 ... DPS__OVERSAMPLING_RATE_128 + * @return status code + */ + int16_t startMeasurePressureOnce(uint8_t oversamplingRate); + + /** + * gets the result a single temperature or pressure measurement in °C or Pa + * + * @param &result: reference to a float value where the result will be written + * @return status code + */ + int16_t getSingleResult(float &result); + + /** + * starts a continuous temperature measurement with specified measurement rate and oversampling rate + * If measure rate is n and oversampling rate is m, the DPS3xx performs 2^(n+m) internal measurements per second. + * The DPS3xx cannot operate with high precision and high speed at the same time. Consult the datasheet for more information. + * + * @param measureRate: DPS__MEASUREMENT_RATE_1, DPS__MEASUREMENT_RATE_2,DPS__MEASUREMENT_RATE_4 ... DPS__MEASUREMENT_RATE_128 + * @param oversamplingRate: DPS__OVERSAMPLING_RATE_1, DPS__OVERSAMPLING_RATE_2, DPS__OVERSAMPLING_RATE_4 ... DPS__OVERSAMPLING_RATE_128 + * + * @return status code + * + */ + int16_t startMeasureTempCont(uint8_t measureRate, uint8_t oversamplingRate); + + /** + * starts a continuous temperature measurement with specified measurement rate and oversampling rate + * + * @param measureRate: DPS__MEASUREMENT_RATE_1, DPS__MEASUREMENT_RATE_2,DPS__MEASUREMENT_RATE_4 ... DPS__MEASUREMENT_RATE_128 + * @param oversamplingRate: DPS__OVERSAMPLING_RATE_1, DPS__OVERSAMPLING_RATE_2, DPS__OVERSAMPLING_RATE_4 ... DPS__OVERSAMPLING_RATE_128 + * @return status code + */ + int16_t startMeasurePressureCont(uint8_t measureRate, uint8_t oversamplingRate); + + /** + * starts a continuous temperature and pressure measurement with specified measurement rate and oversampling rate for temperature and pressure measurement respectively. + * + * @param tempMr: measure rate for temperature + * @param tempOsr: oversampling rate for temperature + * @param prsMr: measure rate for pressure + * @param prsOsr: oversampling rate for pressure + * @return status code + */ + int16_t startMeasureBothCont(uint8_t tempMr, uint8_t tempOsr, uint8_t prsMr, uint8_t prsOsr); + + /** + * Gets the interrupt status flag of the FIFO + * + * @return 1 if the FIFO is full and caused an interrupt + * 0 if the FIFO is not full or FIFO interrupt is disabled + * -1 on fail + */ + int16_t getIntStatusFifoFull(void); + + /** + * Gets the interrupt status flag that indicates a finished temperature measurement + * + * @return 1 if a finished temperature measurement caused an interrupt; + * 0 if there is no finished temperature measurement or interrupts are disabled; + * -1 on fail. + */ + int16_t getIntStatusTempReady(void); + + /** + * Gets the interrupt status flag that indicates a finished pressure measurement + * + * @return 1 if a finished pressure measurement caused an interrupt; + * 0 if there is no finished pressure measurement or interrupts are disabled; + * -1 on fail. + */ + int16_t getIntStatusPrsReady(void); + + /** + * Function to fix a hardware problem on some devices + * You have this problem if you measure a temperature which is too high (e.g. 60°C when temperature is around 20°C) + * Call correctTemp() directly after begin() to fix this issue + */ + int16_t correctTemp(void); + +protected: + // scaling factor table + static const int32_t scaling_facts[DPS__NUM_OF_SCAL_FACTS]; + + dps::Mode m_opMode; + + // flags + uint8_t m_initFail; + + uint8_t m_productID; + uint8_t m_revisionID; + + // settings + uint8_t m_tempMr; + uint8_t m_tempOsr; + uint8_t m_prsMr; + uint8_t m_prsOsr; + + // compensation coefficients for both dps3xx and dps422 + int32_t m_c00; + int32_t m_c10; + int32_t m_c01; + int32_t m_c11; + int32_t m_c20; + int32_t m_c21; + int32_t m_c30; + + // last measured scaled temperature (necessary for pressure compensation) + float m_lastTempScal; + + // bus specific + uint8_t m_SpiI2c; // 0=SPI, 1=I2C + + // used for I2C + MF_I2C *m_i2cbus; + uint8_t m_slaveAddress; + +#ifndef DPS_DISABLESPI + // used for SPI + SPIClass *m_spibus; + int32_t m_chipSelect; + uint8_t m_threeWire; +#endif + /** + * Initializes the sensor. + * This function has to be called from begin() + * and requires a valid bus initialization. + */ + virtual void init(void) = 0; + + /** + * reads the compensation coefficients from the sensor + * this is called once from init(), which is called from begin() + * + * @return 0 on success, -1 on fail + */ + virtual int16_t readcoeffs(void) = 0; + + /** + * Sets the Operation Mode of the sensor + * + * @param opMode: the new OpMode as defined by dps::Mode; CMD_BOTH should not be used for DPS3xx + * @return 0 on success, + * -1 on fail + */ + int16_t setOpMode(uint8_t opMode); + + /** + * Configures temperature measurement + * + * @param temp_mr: DPS__MEASUREMENT_RATE_1, DPS__MEASUREMENT_RATE_2,DPS__MEASUREMENT_RATE_4 ... DPS__MEASUREMENT_RATE_128 + * @param temp_osr: DPS__OVERSAMPLING_RATE_1, DPS__OVERSAMPLING_RATE_2, DPS__OVERSAMPLING_RATE_4 ... DPS__OVERSAMPLING_RATE_128 + * + * @return 0 normally or -1 on fail + */ + virtual int16_t configTemp(uint8_t temp_mr, uint8_t temp_osr); + + /** + * Configures pressure measurement + * + * @param prs_mr: DPS__MEASUREMENT_RATE_1, DPS__MEASUREMENT_RATE_2,DPS__MEASUREMENT_RATE_4 ... DPS__MEASUREMENT_RATE_128 + * @param prs_osr: DPS__OVERSAMPLING_RATE_1, DPS__OVERSAMPLING_RATE_2, DPS__OVERSAMPLING_RATE_4 ... DPS__OVERSAMPLING_RATE_128 + * @return 0 normally or + * -1 on fail + */ + virtual int16_t configPressure(uint8_t prs_mr, uint8_t prs_osr); + + virtual int16_t flushFIFO() = 0; + + virtual float calcTemp(int32_t raw) = 0; + + virtual float calcPressure(int32_t raw) = 0; + + int16_t enableFIFO(); + + int16_t disableFIFO(); + + /** + * calculates the time that the sensor needs for 2^mr measurements with an oversampling rate of 2^osr (see table "pressure measurement time (ms) versus oversampling rate") + * Note that the total measurement time for temperature and pressure must not be more than 1 second. + * Timing behavior of pressure and temperature sensors can be considered the same. + * + * @param mr: DPS__MEASUREMENT_RATE_1, DPS__MEASUREMENT_RATE_2,DPS__MEASUREMENT_RATE_4 ... DPS__MEASUREMENT_RATE_128 + * @param osr: DPS__OVERSAMPLING_RATE_1, DPS__OVERSAMPLING_RATE_2, DPS__OVERSAMPLING_RATE_4 ... DPS__OVERSAMPLING_RATE_128 + * @return time that the sensor needs for this measurement + */ + uint16_t calcBusyTime(uint16_t temp_rate, uint16_t temp_osr); + + /** + * reads the next raw value from the FIFO + * + * @param value: the raw pressure or temperature value read from the pressure register blocks, where the LSB of PRS_B0 marks whether the value is a temperature or a pressure. + * + * @return -1 on fail + * 0 if result is a temperature raw value + * 1 if result is a pressure raw value + */ + int16_t getFIFOvalue(int32_t *value); + + /** + * Gets the results from continuous measurements and writes them to given arrays + * + * @param *tempBuffer: The start address of the buffer where the temperature results are written + * If this is NULL, no temperature results will be written out + * @param &tempCount: The size of the buffer for temperature results. + * When the function ends, it will contain the number of bytes written to the buffer. + * @param *prsBuffer: The start address of the buffer where the pressure results are written + * If this is NULL, no pressure results will be written out + * @param &prsCount: The size of the buffer for pressure results. + * When the function ends, it will contain the number of bytes written to the buffer. + * @param reg The FIFO empty register field; needed since this field is different for each sensor + * @return status code + */ + int16_t getContResults(float *tempBuffer, uint8_t &tempCount, float *prsBuffer, uint8_t &prsCount, RegMask_t reg); + + /** + * reads a byte from the sensor + * + * @param regAddress: Address that has to be read + * @return register content or -1 on fail + */ + int16_t readByte(uint8_t regAddress); + +#ifndef DPS_DISABLESPI + /** + * reads a byte from the sensor via SPI + * this function is automatically called by readByte + * if the sensor is connected via SPI + * + * @param regAddress: Address that has to be read + * @return register content or -1 on fail + */ + int16_t readByteSPI(uint8_t regAddress); +#endif + /** + * reads a block from the sensor + * + * @param regAddress: Address that has to be read + * @param length: Length of data block + * @param buffer: Buffer where data will be stored + * @return number of bytes that have been read successfully, which might not always equal to length due to rx-Buffer overflow etc. + */ + int16_t readBlock(RegBlock_t regBlock, uint8_t *buffer); + +#ifndef DPS_DISABLESPI + /** + * reads a block from the sensor via SPI + * + * @param regAddress: Address that has to be read + * @param length: Length of data block + * @param readBuffer: Buffer where data will be stored + * @return number of bytes that have been read successfully, which might not always equal to length due to rx-Buffer overflow etc. + */ + int16_t readBlockSPI(RegBlock_t regBlock, uint8_t *readBuffer); +#endif + /** + * writes a byte to a given register of the sensor without checking + * + * @param regAddress: Address of the register that has to be updated + * @param data: Byte that will be written to the register + * @return 0 if byte was written successfully or + * -1 on fail + */ + int16_t writeByte(uint8_t regAddress, uint8_t data); + + /** + * writes a byte to a register of the sensor + * + * @param regAddress: Address of the register that has to be updated + * @param data: Byte that will be written to the register + * @param check: If this is true, register content will be read after writing + * to check if update was successful + * @return 0 if byte was written successfully or + * -1 on fail + */ + int16_t writeByte(uint8_t regAddress, uint8_t data, uint8_t check); + +#ifndef DPS_DISABLESPI + /** + * writes a byte to a register of the sensor via SPI + * + * @param regAddress: Address of the register that has to be updated + * @param data: Byte that will be written to the register + * @param check: If this is true, register content will be read after writing + * to check if update was successful + * @return 0 if byte was written successfully or + * -1 on fail + */ + int16_t writeByteSpi(uint8_t regAddress, uint8_t data, uint8_t check); +#endif + + /** + * updates a bit field of the sensor without checking + * + * @param regMask: Mask of the register that has to be updated + * @param data: BitValues that will be written to the register + * @return 0 if byte was written successfully or + * -1 on fail + */ + int16_t writeByteBitfield(uint8_t data, RegMask_t regMask); + + /** + * updates a bit field of the sensor + * + * regMask: Mask of the register that has to be updated + * data: BitValues that will be written to the register + * check: enables/disables check after writing; 0 disables check. + * if check fails, -1 will be returned + * @return 0 if byte was written successfully or + * -1 on fail + */ + int16_t writeByteBitfield(uint8_t data, uint8_t regAddress, uint8_t mask, uint8_t shift, uint8_t check); + + /** + * reads a bit field from the sensor + * regMask: Mask of the register that has to be updated + * data: BitValues that will be written to the register + * @return read and processed bits or -1 on fail + */ + int16_t readByteBitfield(RegMask_t regMask); + + /** + * @brief converts non-32-bit negative numbers to 32-bit negative numbers with 2's complement + * + * @param raw The raw number of less than 32 bits + * @param length The bit length + */ + void getTwosComplement(int32_t *raw, uint8_t length); + + /** + * @brief Get a raw result from a given register block + * + * @param raw The address where the raw value is to be written + * @param reg The register block to be read from + * @return status code + */ + int16_t getRawResult(int32_t *raw, RegBlock_t reg); +}; + +#endif // DPSCLASS_H_INCLUDED diff --git a/src/bar/dps3/src/util/DpsRegister.h b/src/bar/dps3/src/util/DpsRegister.h new file mode 100644 index 0000000..6cfb964 --- /dev/null +++ b/src/bar/dps3/src/util/DpsRegister.h @@ -0,0 +1,19 @@ +#ifndef DPSREGISTER_H_INCLUDED +#define DPSREGISTER_H_INCLUDED + +#include + +typedef struct +{ + uint8_t regAddress; + uint8_t mask; + uint8_t shift; +} RegMask_t; + +typedef struct +{ + uint8_t regAddress; + uint8_t length; +} RegBlock_t; + +#endif \ No newline at end of file diff --git a/src/bar/dps3/src/util/dps3xx_config.h b/src/bar/dps3/src/util/dps3xx_config.h new file mode 100644 index 0000000..9dd93b8 --- /dev/null +++ b/src/bar/dps3/src/util/dps3xx_config.h @@ -0,0 +1,49 @@ +#ifndef DPS3xx_CONFIG_H_ +#define DPS3xx_CONFIG_H_ + +#define DPS3xx_NUM_OF_REGMASKS 16 + +enum Interrupt_source_3xx_e +{ + DPS3xx_NO_INTR = 0, + DPS3xx_PRS_INTR = 1, + DPS3xx_TEMP_INTR = 2, + DPS3xx_BOTH_INTR = 3, + DPS3xx_FIFO_FULL_INTR = 4, +}; + +namespace dps3xx +{ + + enum Registers_e + { + PROD_ID = 0, + REV_ID, + TEMP_SENSOR, // internal vs external + TEMP_SENSORREC, // temperature sensor recommendation + TEMP_SE, // temperature shift enable (if temp_osr>3) + PRS_SE, // pressure shift enable (if prs_osr>3) + FIFO_FL, // FIFO flush + FIFO_EMPTY, // FIFO empty + FIFO_FULL, // FIFO full + INT_HL, + INT_SEL, // interrupt select + }; + + const RegMask_t registers[DPS3xx_NUM_OF_REGMASKS] = { + {0x0D, 0x0F, 0}, // PROD_ID + {0x0D, 0xF0, 4}, // REV_ID + {0x07, 0x80, 7}, // TEMP_SENSOR + {0x28, 0x80, 7}, // TEMP_SENSORREC + {0x09, 0x08, 3}, // TEMP_SE + {0x09, 0x04, 2}, // PRS_SE + {0x0C, 0x80, 7}, // FIFO_FL + {0x0B, 0x01, 0}, // FIFO_EMPTY + {0x0B, 0x02, 1}, // FIFO_FULL + {0x09, 0x80, 7}, // INT_HL + {0x09, 0x70, 4}, // INT_SEL + }; + + const RegBlock_t coeffBlock = {0x10, 18}; +} // namespace dps3xx +#endif \ No newline at end of file diff --git a/src/bar/dps3/src/util/dps_config.h b/src/bar/dps3/src/util/dps_config.h new file mode 100644 index 0000000..2f97117 --- /dev/null +++ b/src/bar/dps3/src/util/dps_config.h @@ -0,0 +1,127 @@ + +#ifndef DPS_CONSTS_H_ +#define DPS_CONSTS_H_ +#include "DpsRegister.h" + +/////////// DPS3xx /////////// +#define DPS3xx__PROD_ID 0x00 +#define DPS3xx__SPI_WRITE_CMD 0x00U +#define DPS3xx__SPI_READ_CMD 0x80U +#define DPS3xx__SPI_RW_MASK 0x80U +#define DPS3xx__SPI_MAX_FREQ 1000000U + +#define DPS3xx__OSR_SE 3U + +// DPS3xx has 10 milliseconds of spare time for each synchronous measurement / per second for asynchronous measurements +// this is for error prevention on friday-afternoon-products :D +// you can set it to 0 if you dare, but there is no warranty that it will still work +#define DPS3xx__BUSYTIME_FAILSAFE 10U +#define DPS3xx__MAX_BUSYTIME ((1000U - DPS3xx__BUSYTIME_FAILSAFE) * DPS__BUSYTIME_SCALING) + +#define DPS3xx__REG_ADR_SPI3W 0x09U +#define DPS3xx__REG_CONTENT_SPI3W 0x01U + +/////////// common /////////// + +// slave address same for 3xx +#define DPS__FIFO_SIZE 32 +#define DPS__STD_SLAVE_ADDRESS 0x77U +#define DPS__RESULT_BLOCK_LENGTH 3 +#define NUM_OF_COMMON_REGMASKS 16 + +#define DPS__MEASUREMENT_RATE_1 0 +#define DPS__MEASUREMENT_RATE_2 1 +#define DPS__MEASUREMENT_RATE_4 2 +#define DPS__MEASUREMENT_RATE_8 3 +#define DPS__MEASUREMENT_RATE_16 4 +#define DPS__MEASUREMENT_RATE_32 5 +#define DPS__MEASUREMENT_RATE_64 6 +#define DPS__MEASUREMENT_RATE_128 7 + +#define DPS__OVERSAMPLING_RATE_1 DPS__MEASUREMENT_RATE_1 +#define DPS__OVERSAMPLING_RATE_2 DPS__MEASUREMENT_RATE_2 +#define DPS__OVERSAMPLING_RATE_4 DPS__MEASUREMENT_RATE_4 +#define DPS__OVERSAMPLING_RATE_8 DPS__MEASUREMENT_RATE_8 +#define DPS__OVERSAMPLING_RATE_16 DPS__MEASUREMENT_RATE_16 +#define DPS__OVERSAMPLING_RATE_32 DPS__MEASUREMENT_RATE_32 +#define DPS__OVERSAMPLING_RATE_64 DPS__MEASUREMENT_RATE_64 +#define DPS__OVERSAMPLING_RATE_128 DPS__MEASUREMENT_RATE_128 + +// we use 0.1 ms units for time calculations, so 10 units are one millisecond +#define DPS__BUSYTIME_SCALING 10U + +#define DPS__NUM_OF_SCAL_FACTS 8 + +// status code +#define DPS__SUCCEEDED 0 +#define DPS__FAIL_UNKNOWN -1 +#define DPS__FAIL_INIT_FAILED -2 +#define DPS__FAIL_TOOBUSY -3 +#define DPS__FAIL_UNFINISHED -4 + +namespace dps +{ + + /** + * @brief Operating mode. + * + */ + enum Mode + { + IDLE = 0x00, + CMD_PRS = 0x01, + CMD_TEMP = 0x02, + CMD_BOTH = 0x03, // only for DPS422 + CONT_PRS = 0x05, + CONT_TMP = 0x06, + CONT_BOTH = 0x07 + }; + + enum RegisterBlocks_e + { + PRS = 0, // pressure value + TEMP, // temperature value + }; + + const RegBlock_t registerBlocks[2] = { + {0x00, 3}, + {0x03, 3}, + }; + + /** + * @brief registers for configuration and flags; these are the same for both 3xx and 422, might need to be adapted for future sensors + * + */ + enum Config_Registers_e + { + TEMP_MR = 0, // temperature measure rate + TEMP_OSR, // temperature measurement resolution + PRS_MR, // pressure measure rate + PRS_OSR, // pressure measurement resolution + MSR_CTRL, // measurement control + FIFO_EN, + + TEMP_RDY, + PRS_RDY, + INT_FLAG_FIFO, + INT_FLAG_TEMP, + INT_FLAG_PRS, + }; + + const RegMask_t config_registers[NUM_OF_COMMON_REGMASKS] = { + {0x07, 0x70, 4}, // TEMP_MR + {0x07, 0x07, 0}, // TEMP_OSR + {0x06, 0x70, 4}, // PRS_MR + {0x06, 0x07, 0}, // PRS_OSR + {0x08, 0x07, 0}, // MSR_CTRL + {0x09, 0x02, 1}, // FIFO_EN + + {0x08, 0x20, 5}, // TEMP_RDY + {0x08, 0x10, 4}, // PRS_RDY + {0x0A, 0x04, 2}, // INT_FLAG_FIFO + {0x0A, 0x02, 1}, // INT_FLAG_TEMP + {0x0A, 0x01, 0}, // INT_FLAG_PRS + }; + +} // namespace dps +#endif /* DPS_CONSTS_H_ */ diff --git a/src/cfg/cfg.h b/src/cfg/cfg.h index 0b4c35d..049a530 100644 --- a/src/cfg/cfg.h +++ b/src/cfg/cfg.h @@ -183,7 +183,7 @@ SOFTWARE. MF_PARAM( ahr_gizmo, 0, int32_t, 'e', mf_MAHONY,mf_MAHONY_BF,mf_MADGWICK,mf_VQF) \ \ /*BAR - Barometer*/ \ - MF_PARAM( bar_gizmo, 0, int32_t, 'e', mf_NONE,mf_BMP280,mf_BMP388,mf_BMP390,mf_MS5611,mf_HP203B,mf_BMP580) \ + MF_PARAM( bar_gizmo, 0, int32_t, 'e', mf_NONE,mf_BMP280,mf_BMP388,mf_BMP390,mf_MS5611,mf_HP203B,mf_BMP580,mf_DPS310) \ MF_PARAM( bar_i2c_bus, -1, int32_t, 'i') \ MF_PARAM( bar_i2c_adr, 0, int32_t, 'i') \ MF_PARAM( bar_rate, 100, float, 'f') /*Barometer sample rate in Hz (default 100)*/ \ From 52c7436c5e130a1061b07b33e9cf00ab9b0da3a9 Mon Sep 17 00:00:00 2001 From: Enzo PB Date: Mon, 15 Sep 2025 13:14:05 +0200 Subject: [PATCH 2/4] Fix #78 compilation error in MF_Serial::read method for STM32 --- src/hal/MF_Serial.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/hal/MF_Serial.h b/src/hal/MF_Serial.h index c80143d..97e3c4f 100644 --- a/src/hal/MF_Serial.h +++ b/src/hal/MF_Serial.h @@ -32,7 +32,8 @@ class MF_SerialPtrWrapper : public MF_Serial { void begin(int baud) override { _serial->begin(baud); } int available() override { return _serial->available(); } int availableForWrite() override { return _serial->availableForWrite(); } - int read(uint8_t *buf, int len) override { return _serial->readBytes(buf, len); } + // cast buf to char*, because STM USBSerial doesn't support uint8 for readBytes + int read(uint8_t *buf, int len) override { return _serial->readBytes((char *) buf, len); } int write(uint8_t *buf, int len) override { return _serial->write(buf, len); } }; From ec5f5c685df06cdbcd58906a2c5cfc317597e8c4 Mon Sep 17 00:00:00 2001 From: EnzoPB Date: Fri, 19 Sep 2025 11:19:31 +0200 Subject: [PATCH 3/4] Fix #79 imu interrupt not firing on STM32 --- src/hal/STM32/hal_STM32_cpp.h | 24 ++++++++++++++++++++++++ src/imu/imu_cpp.h | 5 +++++ 2 files changed, 29 insertions(+) diff --git a/src/hal/STM32/hal_STM32_cpp.h b/src/hal/STM32/hal_STM32_cpp.h index f1540e7..5c5fdb7 100644 --- a/src/hal/STM32/hal_STM32_cpp.h +++ b/src/hal/STM32/hal_STM32_cpp.h @@ -204,6 +204,30 @@ void hal_print_pin_name(int pinnum) { } } +IRQn_Type hal_get_irqn_from_pin(int pin) { + // converts an arduino pin number to a STM32 HAL interrupt id + // taken from https://github.com/stm32duino/Arduino_Core_STM32/blob/main/libraries/SrcWrapper/src/stm32/interrupt.cpp#L158 + PinName pinName = digitalPinToPinName(pin); + uint16_t gpioPin = STM_GPIO_PIN(pinName); + uint8_t id = 0; + + while (gpioPin != 0x0001) { + gpioPin = gpioPin >> 1; + id++; + } + + switch (id) { + case 0: return EXTI0_IRQn; + case 1: return EXTI1_IRQn; + case 2: return EXTI2_IRQn; + case 3: return EXTI3_IRQn; + case 4: return EXTI4_IRQn; + case 5: case 6: case 7: case 8: case 9: + return EXTI9_5_IRQn; + default: + return EXTI15_10_IRQn; + } +} MF_Serial* hal_get_ser_bus(int bus_id, int baud, MF_SerialMode mode, bool invert) { if(bus_id < 0 || bus_id >= HAL_SER_NUM) return nullptr; diff --git a/src/imu/imu_cpp.h b/src/imu/imu_cpp.h index 22db354..680ef44 100644 --- a/src/imu/imu_cpp.h +++ b/src/imu/imu_cpp.h @@ -388,6 +388,11 @@ void _imu_ll_interrupt_handler(); void _imu_ll_interrupt_setup(int interrupt_pin) { Serial.println(MF_MOD ": IMU_EXEC_IRQ"); attachInterrupt(digitalPinToInterrupt(interrupt_pin), _imu_ll_interrupt_handler, RISING); + #ifdef ARDUINO_ARCH_STM32 + // (#79) on stm32, increase the interrupt priority, otherwise it does not get called + // configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY is the maximum priority allowed for interrupts that needs to call FreeRTOS api + HAL_NVIC_SetPriority(hal_get_irqn_from_pin(interrupt_pin), 0, configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY); + #endif } inline void _imu_ll_interrupt_handler2() { From 9dbf1607ffaed0ccf85b813b3435a9468daa61f7 Mon Sep 17 00:00:00 2001 From: Enzo PB Date: Fri, 19 Sep 2025 21:21:05 +0200 Subject: [PATCH 4/4] make bar measurement continuous (wip) --- src/bar/BarGizmoDPS310.h | 26 ++++++++++++-------------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/src/bar/BarGizmoDPS310.h b/src/bar/BarGizmoDPS310.h index b64e3f8..2f20ffb 100644 --- a/src/bar/BarGizmoDPS310.h +++ b/src/bar/BarGizmoDPS310.h @@ -10,24 +10,22 @@ class BarGizmoDPS310: public BarGizmo { public: BarGizmoDPS310(MF_I2C *i2c, int8_t i2c_adr, uint32_t sampleRate) { if (i2c_adr == 0) { - i2c_adr = 0x77; + i2c_adr = 0x77; // default i2c address for the DPS310 } pressureSensor.begin(*i2c, i2c_adr); - } - - bool update(float *press, float *temp) override { - int16_t ret; - - ret = pressureSensor.measurePressureOnce(*press); + int16_t ret = pressureSensor.startMeasureBothCont( + DPS__MEASUREMENT_RATE_128, DPS__OVERSAMPLING_RATE_2, + DPS__MEASUREMENT_RATE_128, DPS__OVERSAMPLING_RATE_2 + ); + // TODO: find a way to make gizmo check fail (bar.cpp line 90) if (ret != 0) { - return false; - } - - ret = pressureSensor.measureTempOnce(*temp); - if (ret != 0) { - return false; + Serial.println("bar init start measure cont failed"); + while (1); } + } - return true; + bool update(float *press, float *temp) override { + uint8_t measureCount = 1; + return pressureSensor.getContResults(temp, measureCount, press, measureCount) == 0; } };