Skip to content

Commit

Permalink
drivers: add SPA06
Browse files Browse the repository at this point in the history
  • Loading branch information
@julianoes
julianoes committed Nov 18, 2024
1 parent cba0e83 commit 04ac761
Show file tree
Hide file tree
Showing 4 changed files with 57 additions and 43 deletions.
7 changes: 7 additions & 0 deletions ROMFS/px4fmu_common/init.d/rc.sensors
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
53 changes: 32 additions & 21 deletions src/drivers/barometer/goertek/spa06/SPA06.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -158,7 +168,7 @@ SPA06::init()
}

if (tries < 0) {
PX4_DEBUG("spa06 cal failed");
PX4_DEBUG("spa06 sensor or coef not ready");
return -EIO;
}

Expand All @@ -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();
Expand Down Expand Up @@ -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;
Expand Down
27 changes: 12 additions & 15 deletions src/drivers/barometer/goertek/spa06/SPA06.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,36 +76,33 @@ class SPA06 : public I2CSPIDriver<SPA06>

// 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
// oversampling (times): single | 2 | 4 | 8 | 16 | 32 | 64 | 128
// 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;
Expand All @@ -115,6 +112,6 @@ class SPA06 : public I2CSPIDriver<SPA06>
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};
};
13 changes: 6 additions & 7 deletions src/drivers/barometer/goertek/spa06/spa06.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@
/**
* @file spa06.h
*
* Shared defines for the spa06 driver.
* Shared defines for the SPA06 driver.
*/
#pragma once

Expand All @@ -51,7 +51,7 @@


#define SPA06_VALUE_RESET 9
#define SPA06_VALUE_ID 0x10
#define SPA06_VALUE_ID 0x11

namespace spa06
{
Expand All @@ -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;
Expand All @@ -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
Expand All @@ -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
Expand Down

0 comments on commit 04ac761

Please sign in to comment.