From 04ac761303bd089e2a30780fdd7a0d0e24128f74 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 18 Nov 2024 15:02:32 +1300 Subject: [PATCH] drivers: add SPA06 --- ROMFS/px4fmu_common/init.d/rc.sensors | 7 +++ src/drivers/barometer/goertek/spa06/SPA06.cpp | 53 +++++++++++-------- src/drivers/barometer/goertek/spa06/SPA06.hpp | 27 +++++----- src/drivers/barometer/goertek/spa06/spa06.h | 13 +++-- 4 files changed, 57 insertions(+), 43 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index b85f36e09de3..7f479a3b1f02 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -211,6 +211,13 @@ then spl06 -X -a 0x77 start fi +# SPA06 sensor external I2C +if param compare -s SENS_EN_SPA06 1 +then + spa06 -X start + spa06 -X -a 0x77 start +fi + # PCF8583 counter (RPM sensor) if param compare -s SENS_EN_PCF8583 1 then diff --git a/src/drivers/barometer/goertek/spa06/SPA06.cpp b/src/drivers/barometer/goertek/spa06/SPA06.cpp index fb81db54825e..a1b1b4118fc5 100644 --- a/src/drivers/barometer/goertek/spa06/SPA06.cpp +++ b/src/drivers/barometer/goertek/spa06/SPA06.cpp @@ -103,34 +103,44 @@ SPA06::scale_factor(int oversampling_rate) int SPA06::calibrate() { - uint8_t buf[18]; + uint8_t buf[21]; _interface->read(SPA06_ADDR_CAL, buf, sizeof(buf)); - _cal.c0 = (uint16_t)buf[0] << 4 | (uint16_t)buf[1] >> 4; + _cal.c0 = (uint16_t)(buf[0]) << 4 | (uint16_t)(buf[1]) >> 4; + // If value is negative, we need to fill the missing bits. _cal.c0 = (_cal.c0 & 1 << 11) ? (0xf000 | _cal.c0) : _cal.c0; - _cal.c1 = (uint16_t)(buf[1] & 0x0f) << 8 | (uint16_t)buf[2]; + _cal.c1 = (uint16_t)(buf[1] & 0x0F) << 8 | buf[2]; _cal.c1 = (_cal.c1 & 1 << 11) ? (0xf000 | _cal.c1) : _cal.c1; - _cal.c00 = (uint32_t)buf[3] << 12 | (uint32_t)buf[4] << 4 | (uint16_t)buf[5] >> 4; + _cal.c00 = (uint32_t)(buf[3]) << 12 | (uint32_t)(buf[4]) << 4 | (buf[5]) >> 4; _cal.c00 = (_cal.c00 & 1 << 19) ? (0xfff00000 | _cal.c00) : _cal.c00; - _cal.c10 = (uint32_t)(buf[5] & 0x0f) << 16 | (uint32_t)buf[6] << 8 | (uint32_t)buf[7]; + _cal.c10 = (uint32_t)(buf[5] & 0x0F) << 16 | (uint32_t)(buf[6]) << 8 | buf[7]; _cal.c10 = (_cal.c10 & 1 << 19) ? (0xfff00000 | _cal.c10) : _cal.c10; - _cal.c01 = (uint16_t)buf[8] << 8 | buf[9]; - _cal.c11 = (uint16_t)buf[10] << 8 | buf[11]; - _cal.c20 = (uint16_t)buf[12] << 8 | buf[13]; - _cal.c21 = (uint16_t)buf[14] << 8 | buf[15]; - _cal.c30 = (uint16_t)buf[16] << 8 | buf[17]; - - // PX4_INFO("c0:%d \nc1:%d \nc00:%d \nc10:%d \nc01:%d \nc11:%d \nc20:%d \nc21:%d \nc30:%d\n", - // _cal.c0,_cal.c1, - // _cal.c00,_cal.c10, - // _cal.c01,_cal.c11,_cal.c20,_cal.c21,_cal.c30 - // ); - //PX4_DEBUG("c0:%f",_cal.c0); + _cal.c01 = (uint16_t)(buf[8]) << 8 | buf[9]; + + _cal.c11 = (uint16_t)(buf[10]) << 8 | buf[11]; + + _cal.c20 = (uint16_t)(buf[12]) << 8 | buf[13]; + + _cal.c21 = (uint16_t)(buf[14]) << 8 | buf[15]; + + _cal.c30 = (uint16_t)(buf[16]) << 8 | buf[17]; + + _cal.c31 = (uint16_t)(buf[18]) << 4 | (uint16_t)(buf[19] & 0xF0) >> 4; + _cal.c31 = (_cal.c31 & 1 << 11) ? (0xf000 | _cal.c31) : _cal.c31; + + _cal.c40 = (uint16_t)(buf[19] & 0x0F) << 8 | buf[20]; + _cal.c40 = (_cal.c40 & 1 << 11) ? (0xf000 | _cal.c40) : _cal.c40; + + PX4_DEBUG("c0:%d\nc1:%d\nc00:%ld\nc10:%ld\nc01:%d\nc11:%d\nc20:%d\nc21:%d\nc30:%d\nc31:%d\nc40:%d\n", + _cal.c0, _cal.c1, + _cal.c00, _cal.c10, + _cal.c01, _cal.c11, _cal.c20, _cal.c21, _cal.c30, _cal.c31, _cal.c40); + return OK; } int @@ -158,7 +168,7 @@ SPA06::init() } if (tries < 0) { - PX4_DEBUG("spa06 cal failed"); + PX4_DEBUG("spa06 sensor or coef not ready"); return -EIO; } @@ -171,8 +181,9 @@ SPA06::init() _interface->set_reg(_curr_tmp_cfg, SPA06_ADDR_TMP_CFG); kt = 524288.0f; - + // Enable FIFO _interface->set_reg(1 << 2, SPA06_ADDR_CFG_REG); + // Continuous pressure and temperature mesasurement. _interface->set_reg(7, SPA06_ADDR_MEAS_CFG); Start(); @@ -217,8 +228,8 @@ SPA06::collect() // calculate float ftsc = (float)temp_raw / kt; float fpsc = (float)press_raw / kp; - float qua2 = (float)_cal.c10 + fpsc * ((float)_cal.c20 + fpsc * (float)_cal.c30); - float qua3 = ftsc * fpsc * ((float)_cal.c11 + fpsc * (float)_cal.c21); + float qua2 = (float)_cal.c10 + fpsc * ((float)_cal.c20 + fpsc * ((float)_cal.c30) + fpsc * (float)_cal.c40); + float qua3 = ftsc * fpsc * ((float)_cal.c11 + fpsc * ((float)_cal.c21) + fpsc * (float)_cal.c31); float fp = (float)_cal.c00 + fpsc * qua2 + ftsc * (float)_cal.c01 + qua3; float temperature = (float)_cal.c0 * 0.5f + (float)_cal.c1 * ftsc; diff --git a/src/drivers/barometer/goertek/spa06/SPA06.hpp b/src/drivers/barometer/goertek/spa06/SPA06.hpp index 565e572a3e79..255da708195c 100644 --- a/src/drivers/barometer/goertek/spa06/SPA06.hpp +++ b/src/drivers/barometer/goertek/spa06/SPA06.hpp @@ -76,10 +76,8 @@ class SPA06 : public I2CSPIDriver // configuration of pressure measurement rate (PM_RATE) and resolution (PM_PRC) // - // bit[7]: reserved - // - // PM_RATE[6:4] : 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 - // measurement rate: 1 | 2 | 4 | 8 | 16 | 32 | 64 | 128 + // PM_RATE[7:4] : 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 + // measurement rate: 1 | 2 | 4 | 8 | 16 | 32 | 64 | 128 | 25/16 | 25/8 | 25/4 | 25/2 | 25 | 50 | 100 | 200 // note: applicable for measurements in background mode only // // PM_PRC[3:0] : 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 @@ -87,25 +85,24 @@ class SPA06 : public I2CSPIDriver // measurement time(ms): 3.6 | 5.2 | 8.4 | 14.8 | 27.6 | 53.2 | 104.4 | 206.8 // precision(PaRMS) : 5.0 | | 2.5 | | 1.2 | 0.9 | 0.5 | // note: use in combination with a bit shift when the oversampling rate is > 8 times. see CFG_REG(0x19) register - static constexpr uint8_t _curr_prs_cfg{4 << 4 | 4}; + // + // -> 32 measurements per second, 16 oversampling + static constexpr uint8_t _curr_prs_cfg{5 << 4 | 4}; // configuration of temperature measurment rate (TMP_RATE) and resolution (TMP_PRC) // // temperature measurement: internal sensor (in ASIC) | external sensor (in pressure sensor MEMS element) - // TMP_EXT[7] : 0 | 1 - // note: it is highly recommended to use the same temperature sensor as the source of the calibration coefficients wihch can be read from reg 0x28 - // - // TMP_RATE[6:4] : 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 - // measurement rate: 1 | 2 | 4 | 8 | 16 | 32 | 64 | 128 + // PM_RATE[7:4] : 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 + // measurement rate: 1 | 2 | 4 | 8 | 16 | 32 | 64 | 128 | 25/16 | 25/8 | 25/4 | 25/2 | 25 | 50 | 100 | 200 // note: applicable for measurements in background mode only // - // bit[3]: reserved - // // TMP_PRC[2:0] : 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 // oversampling (times): single | 2 | 4 | 8 | 16 | 32 | 64 | 128 // note: single(default) measurement time 3.6ms, other settings are optional, and may not be relevant // note: use in combination with a bit shift when the oversampling rate is > 8 times. see CFG_REG(0x19) register - static constexpr uint8_t _curr_tmp_cfg{1 << 7 | 4 << 4 | 0}; + + // -> 32 measurements per second, single oversampling + static constexpr uint8_t _curr_tmp_cfg{5 << 4 | 0}; bool _collect_phase{false}; float kp; @@ -115,6 +112,6 @@ class SPA06 : public I2CSPIDriver perf_counter_t _measure_perf; perf_counter_t _comms_errors; - static constexpr uint32_t _sample_rate{16}; - static constexpr uint32_t _measure_interval{1000000 / _sample_rate / 2}; + static constexpr uint32_t _sample_rate{32}; + static constexpr uint32_t _measure_interval{1000000 / _sample_rate}; }; diff --git a/src/drivers/barometer/goertek/spa06/spa06.h b/src/drivers/barometer/goertek/spa06/spa06.h index b31276af8761..4412974f351f 100644 --- a/src/drivers/barometer/goertek/spa06/spa06.h +++ b/src/drivers/barometer/goertek/spa06/spa06.h @@ -34,7 +34,7 @@ /** * @file spa06.h * - * Shared defines for the spa06 driver. + * Shared defines for the SPA06 driver. */ #pragma once @@ -51,7 +51,7 @@ #define SPA06_VALUE_RESET 9 -#define SPA06_VALUE_ID 0x10 +#define SPA06_VALUE_ID 0x11 namespace spa06 { @@ -60,8 +60,8 @@ namespace spa06 struct calibration_s { int16_t c0, c1; int32_t c00, c10; - int16_t c01, c11, c20, c21, c30; -}; //calibration data + int16_t c01, c11, c20, c21, c30, c31, c40; +}; struct data_s { uint8_t p_msb; @@ -71,7 +71,7 @@ struct data_s { uint8_t t_msb; uint8_t t_lsb; uint8_t t_xlsb; -}; // data +}; #pragma pack(pop) class ISPA06 @@ -96,10 +96,9 @@ class ISPA06 virtual uint8_t get_device_address() const = 0; }; -} /* namespace */ +} // namespace spa06 -/* interface factories */ #if defined(CONFIG_SPI) extern spa06::ISPA06 *spa06_spi_interface(uint8_t busnum, uint32_t device, int bus_frequency, spi_mode_e spi_mode); #endif // CONFIG_SPI