src/lib/Baro/baro_spl06.cpp

   1 #include <Wire.h>
   2
   3 /****
   4  * Calculations used in this code taken from iNav's SPL006 (sic) implementation
   5  * https://github.com/iNavFlight/inav/pull/5028
   6  *
   7  * Code reused under GPL License, see the iNav project for full license information
   8  * https://github.com/iNavFlight/inav
   9  ****/
  10
  11 #include "baro_spl06.h"
  12
  13 uint8_t SPL06::oversampleToRegVal(const uint8_t oversamples) const
  14 {
  15     switch (oversamples)
  16     {
  17         default: // fallthrough
  18         case 1: return 0;
  19         case 2: return 1;
  20         case 4: return 2;
  21         case 8: return 3;
  22         case 16: return 4;
  23         case 32: return 5;
  24         case 64: return 6;
  25         case 128: return 7;
  26     }
  27 }
  28
  29 int32_t SPL06::oversampleToScaleFactor(const uint8_t oversamples) const
  30 {
  31     switch (oversamples)
  32     {
  33         default: // falthrough
  34         case 1: return 524288;
  35         case 2: return 1572864;
  36         case 4: return 3670016;
  37         case 8: return 7864320;
  38         case 16: return 253952;
  39         case 32: return 516096;
  40         case 64: return 1040384;
  41         case 128: return 2088960;
  42     }
  43 }
  44
  45 void SPL06::initialize()
  46 {
  47     if (m_initialized)
  48         return;
  49
  50     // Step 1: Load calibration data
  51     uint8_t status = 0;
  52     readRegister(SPL06_MODE_AND_STATUS_REG, &status, sizeof(status));
  53     if (((status & SPL06_MEAS_CFG_COEFFS_RDY) == 0) || ((status & SPL06_MEAS_CFG_SENSOR_RDY) == 0))
  54         return;
  55
  56     uint8_t caldata[SPL06_CALIB_COEFFS_LEN];
  57     readRegister(SPL06_CALIB_COEFFS_START, caldata, sizeof(caldata));
  58
  59     m_calib.c0 = (caldata[0] & 0x80 ? 0xF000 : 0) | ((uint16_t)caldata[0] << 4) | (((uint16_t)caldata[1] & 0xF0) >> 4);
  60     m_calib.c1 = ((caldata[1] & 0x8 ? 0xF000 : 0) | ((uint16_t)caldata[1] & 0x0F) << 8) | (uint16_t)caldata[2];
  61     m_calib.c00 = (caldata[3] & 0x80 ? 0xFFF00000 : 0) | ((uint32_t)caldata[3] << 12) | ((uint32_t)caldata[4] << 4) | (((uint32_t)caldata[5] & 0xF0) >> 4);
  62     m_calib.c10 = (caldata[5] & 0x8 ? 0xFFF00000 : 0) | (((uint32_t)caldata[5] & 0x0F) << 16) | ((uint32_t)caldata[6] << 8) | (uint32_t)caldata[7];
  63     m_calib.c01 = ((uint16_t)caldata[8] << 8) | ((uint16_t)caldata[9]);
  64     m_calib.c11 = ((uint16_t)caldata[10] << 8) | (uint16_t)caldata[11];
  65     m_calib.c20 = ((uint16_t)caldata[12] << 8) | (uint16_t)caldata[13];
  66     m_calib.c21 = ((uint16_t)caldata[14] << 8) | (uint16_t)caldata[15];
  67     m_calib.c30 = ((uint16_t)caldata[16] << 8) | (uint16_t)caldata[17];
  68
  69     // Step 2: Set up oversampling and FIFO
  70     uint8_t reg_value;
  71     reg_value = SPL06_TEMP_USE_EXT_SENSOR | oversampleToRegVal(OVERSAMPLING_TEMPERATURE);
  72     writeRegister(SPL06_TEMPERATURE_CFG_REG, &reg_value, sizeof(reg_value));
  73
  74     reg_value = oversampleToRegVal(OVERSAMPLING_PRESSURE);
  75     writeRegister(SPL06_PRESSURE_CFG_REG, &reg_value, sizeof(reg_value));
  76
  77     reg_value = 0;
  78     if (OVERSAMPLING_TEMPERATURE > 8)
  79         reg_value |= SPL06_TEMPERATURE_RESULT_BIT_SHIFT;
  80     if (OVERSAMPLING_PRESSURE > 8)
  81         reg_value |= SPL06_PRESSURE_RESULT_BIT_SHIFT;
  82     writeRegister(SPL06_INT_AND_FIFO_CFG_REG, &reg_value, sizeof(reg_value));
  83
  84     // Done!
  85     m_initialized = true;
  86 }
  87
  88 uint8_t SPL06::getTemperatureDuration()
  89 {
  90     return SPL06_MEASUREMENT_TIME(OVERSAMPLING_TEMPERATURE);
  91 }
  92
  93 void SPL06::startTemperature()
  94 {
  95     uint8_t reg_value = SPL06_MEAS_TEMPERATURE;
  96     writeRegister(SPL06_MODE_AND_STATUS_REG, &reg_value, sizeof(reg_value));
  97 }
  98
  99 int32_t SPL06::getTemperature()
 100 {
 101     uint8_t status;
 102     readRegister(SPL06_MODE_AND_STATUS_REG, &status, sizeof(status));
 103     if ((status & SPL06_MEAS_CFG_TEMPERATURE_RDY) == 0)
 104         return TEMPERATURE_INVALID;
 105
 106     uint8_t data[SPL06_TEMPERATURE_LEN];
 107     readRegister(SPL06_TEMPERATURE_START_REG, data, sizeof(data));
 108
 109     // Unpack and descale
 110     int32_t uncorr_temp = (int32_t)((data[0] & 0x80 ? 0xFF000000 : 0) | (((uint32_t)(data[0])) << 16) | (((uint32_t)(data[1])) << 8) | ((uint32_t)data[2]));
 111     m_temperatureLast = (float)uncorr_temp / oversampleToScaleFactor(OVERSAMPLING_TEMPERATURE);
 112
 113     // Adjust for calibration
 114     const float temp_comp = (float)m_calib.c0 / 2 + m_temperatureLast * m_calib.c1;
 115
 116     return temp_comp * 100;
 117 }
 118
 119 uint8_t SPL06::getPressureDuration()
 120 {
 121     return SPL06_MEASUREMENT_TIME(OVERSAMPLING_PRESSURE);
 122 }
 123
 124 void SPL06::startPressure()
 125 {
 126     uint8_t reg_value = SPL06_MEAS_PRESSURE;
 127     writeRegister(SPL06_MODE_AND_STATUS_REG, &reg_value, sizeof(reg_value));
 128 }
 129
 130 uint32_t SPL06::getPressure()
 131 {
 132     uint8_t status;
 133     readRegister(SPL06_MODE_AND_STATUS_REG, &status, sizeof(status));
 134     if ((status & SPL06_MEAS_CFG_PRESSURE_RDY) == 0)
 135         return PRESSURE_INVALID;
 136
 137     uint8_t data[SPL06_PRESSURE_LEN];
 138     readRegister(SPL06_PRESSURE_START_REG, data, sizeof(data));
 139
 140     // Unpack and descale
 141     int32_t uncorr_press = (int32_t)((data[0] & 0x80 ? 0xFF000000 : 0) | (((uint32_t)(data[0])) << 16) | (((uint32_t)(data[1])) << 8) | ((uint32_t)data[2]));
 142     const float p_raw_sc = (float)uncorr_press / oversampleToScaleFactor(OVERSAMPLING_PRESSURE);
 143
 144     // Adjust for calibration and temperature
 145     const float pressure_cal = (float)m_calib.c00 + p_raw_sc * ((float)m_calib.c10 + p_raw_sc * ((float)m_calib.c20 + p_raw_sc * m_calib.c30));
 146     const float p_temp_comp = m_temperatureLast * ((float)m_calib.c01 + p_raw_sc * ((float)m_calib.c11 + p_raw_sc * m_calib.c21));
 147
 148     return (pressure_cal + p_temp_comp) * 10.0;
 149 }
 150
 151 bool SPL06::detect()
 152 {
 153     // Assumes Wire.begin() has already been called
 154     uint8_t chipid = 0;
 155
 156     // SPL06 can have two addresses based on the SDO pin.
 157     // check primary address
 158     m_address = SPL06_I2C_ADDR;
 159     readRegister(SPL06_CHIP_ID_REG, &chipid, sizeof(chipid));
 160     if (chipid == SPL06_DEFAULT_CHIP_ID)
 161         return true;
 162
 163     // check alternate address
 164     m_address = SPL06_I2C_ADDR_ALT;
 165     readRegister(SPL06_CHIP_ID_REG, &chipid, sizeof(chipid));
 166     return chipid == SPL06_DEFAULT_CHIP_ID;
 167 }