src/main/drivers/barometer/barometer_spl06.c

   1 /*
   2  * This file is part of INAV.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <math.h>
  19 #include <stdbool.h>
  20 #include <stdint.h>
  21
  22 #include <platform.h>
  23 #include "build/build_config.h"
  24 #include "common/utils.h"
  25
  26 #include "drivers/time.h"
  27 #include "drivers/io.h"
  28 #include "drivers/bus.h"
  29 #include "drivers/barometer/barometer.h"
  30 #include "drivers/barometer/barometer_spl06.h"
  31
  32 #if defined(USE_BARO_SPL06)
  33
  34 // SPL06, address 0x76
  35
  36 typedef struct {
  37     int16_t c0;
  38     int16_t c1;
  39     int32_t c00;
  40     int32_t c10;
  41     int16_t c01;
  42     int16_t c11;
  43     int16_t c20;
  44     int16_t c21;
  45     int16_t c30;
  46 } spl06_coeffs_t;
  47
  48
  49 spl06_coeffs_t spl06_cal;
  50 // uncompensated pressure and temperature
  51 static int32_t spl06_pressure_raw = 0;
  52 static int32_t spl06_temperature_raw = 0;
  53
  54 static int8_t spl06_samples_to_cfg_reg_value(uint8_t sample_rate)
  55 {
  56     switch(sample_rate)
  57     {
  58         case 1: return 0;
  59         case 2: return 1;
  60         case 4: return 2;
  61         case 8: return 3;
  62         case 16: return 4;
  63         case 32: return 5;
  64         case 64: return 6;
  65         case 128: return 7;
  66         default: return -1; // invalid
  67     }
  68 }
  69
  70 static int32_t spl06_raw_value_scale_factor(uint8_t oversampling_rate)
  71 {
  72     switch(oversampling_rate)
  73     {
  74         case 1: return 524288;
  75         case 2: return 1572864;
  76         case 4: return 3670016;
  77         case 8: return 7864320;
  78         case 16: return 253952;
  79         case 32: return 516096;
  80         case 64: return 1040384;
  81         case 128: return 2088960;
  82         default: return -1; // invalid
  83     }
  84 }
  85
  86 static bool spl06_start_temperature_measurement(baroDev_t * baro)
  87 {
  88     return busWrite(baro->busDev, SPL06_MODE_AND_STATUS_REG, SPL06_MEAS_TEMPERATURE);
  89 }
  90
  91 static bool spl06_read_temperature(baroDev_t * baro)
  92 {
  93     uint8_t data[SPL06_TEMPERATURE_LEN];
  94     int32_t spl06_temperature;
  95
  96     bool ack = busReadBuf(baro->busDev, SPL06_TEMPERATURE_START_REG, data, SPL06_TEMPERATURE_LEN);
  97
  98     if (ack) {
  99         spl06_temperature = (int32_t)((data[0] & 0x80 ? 0xFF000000 : 0) | (((uint32_t)(data[0])) << 16) | (((uint32_t)(data[1])) << 8) | ((uint32_t)data[2]));
 100         spl06_temperature_raw = spl06_temperature;
 101     }
 102
 103     return ack;
 104 }
 105
 106 static bool spl06_start_pressure_measurement(baroDev_t * baro)
 107 {
 108     return busWrite(baro->busDev, SPL06_MODE_AND_STATUS_REG, SPL06_MEAS_PRESSURE);
 109 }
 110
 111 static bool spl06_read_pressure(baroDev_t * baro)
 112 {
 113     uint8_t data[SPL06_PRESSURE_LEN];
 114     int32_t spl06_pressure;
 115
 116     bool ack = busReadBuf(baro->busDev, SPL06_PRESSURE_START_REG, data, SPL06_PRESSURE_LEN);
 117
 118     if (ack) {
 119         spl06_pressure = (int32_t)((data[0] & 0x80 ? 0xFF000000 : 0) | (((uint32_t)(data[0])) << 16) | (((uint32_t)(data[1])) << 8) | ((uint32_t)data[2]));
 120         spl06_pressure_raw = spl06_pressure;
 121     }
 122
 123     return ack;
 124 }
 125
 126 // Returns temperature in degrees centigrade
 127 static float spl06_compensate_temperature(int32_t temperature_raw)
 128 {
 129     const float t_raw_sc = (float)temperature_raw / spl06_raw_value_scale_factor(SPL06_TEMPERATURE_OVERSAMPLING);
 130     const float temp_comp = (float)spl06_cal.c0 / 2 + t_raw_sc * spl06_cal.c1;
 131     return temp_comp;
 132 }
 133
 134 // Returns pressure in Pascal
 135 static float spl06_compensate_pressure(int32_t pressure_raw, int32_t temperature_raw)
 136 {
 137     const float p_raw_sc = (float)pressure_raw / spl06_raw_value_scale_factor(SPL06_PRESSURE_OVERSAMPLING);
 138     const float t_raw_sc = (float)temperature_raw / spl06_raw_value_scale_factor(SPL06_TEMPERATURE_OVERSAMPLING);
 139
 140     const float pressure_cal = (float)spl06_cal.c00 + p_raw_sc * ((float)spl06_cal.c10 + p_raw_sc * ((float)spl06_cal.c20 + p_raw_sc * spl06_cal.c30));
 141     const float p_temp_comp = t_raw_sc * ((float)spl06_cal.c01 + p_raw_sc * ((float)spl06_cal.c11 + p_raw_sc * spl06_cal.c21));
 142
 143     return pressure_cal + p_temp_comp;
 144 }
 145
 146 bool spl06_calculate(baroDev_t * baro, int32_t * pressure, int32_t * temperature)
 147 {
 148     UNUSED(baro);
 149
 150     if (pressure) {
 151         *pressure = lrintf(spl06_compensate_pressure(spl06_pressure_raw, spl06_temperature_raw));
 152     }
 153
 154     if (temperature) {
 155         *temperature = lrintf(spl06_compensate_temperature(spl06_temperature_raw) * 100);
 156     }
 157
 158     return true;
 159 }
 160
 161 #define DETECTION_MAX_RETRY_COUNT   5
 162 static bool deviceDetect(busDevice_t * busDev)
 163 {
 164     uint8_t chipId;
 165     for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT; retry++) {
 166         delay(100);
 167         bool ack = busRead(busDev, SPL06_CHIP_ID_REG, &chipId);
 168         if (ack && chipId == SPL06_DEFAULT_CHIP_ID) {
 169             return true;
 170         }
 171     };
 172
 173     return false;
 174 }
 175
 176 static bool read_calibration_coefficients(baroDev_t *baro) {
 177     uint8_t sstatus;
 178     if (!(busRead(baro->busDev, SPL06_MODE_AND_STATUS_REG, &sstatus) && (sstatus & SPL06_MEAS_CFG_COEFFS_RDY)))
 179         return false;   // error reading status or coefficients not ready
 180
 181     uint8_t caldata[SPL06_CALIB_COEFFS_LEN];
 182
 183     if (!busReadBuf(baro->busDev, SPL06_CALIB_COEFFS_START, (uint8_t *)&caldata, SPL06_CALIB_COEFFS_LEN)) {
 184         return false;
 185     }
 186
 187     spl06_cal.c0 = (caldata[0] & 0x80 ? 0xF000 : 0) | ((uint16_t)caldata[0] << 4) | (((uint16_t)caldata[1] & 0xF0) >> 4);
 188     spl06_cal.c1 = ((caldata[1] & 0x8 ? 0xF000 : 0) | ((uint16_t)caldata[1] & 0x0F) << 8) | (uint16_t)caldata[2];
 189     spl06_cal.c00 = (caldata[3] & 0x80 ? 0xFFF00000 : 0) | ((uint32_t)caldata[3] << 12) | ((uint32_t)caldata[4] << 4) | (((uint32_t)caldata[5] & 0xF0) >> 4);
 190     spl06_cal.c10 = (caldata[5] & 0x8 ? 0xFFF00000 : 0) | (((uint32_t)caldata[5] & 0x0F) << 16) | ((uint32_t)caldata[6] << 8) | (uint32_t)caldata[7];
 191     spl06_cal.c01 = ((uint16_t)caldata[8] << 8) | ((uint16_t)caldata[9]);
 192     spl06_cal.c11 = ((uint16_t)caldata[10] << 8) | (uint16_t)caldata[11];
 193     spl06_cal.c20 = ((uint16_t)caldata[12] << 8) | (uint16_t)caldata[13];
 194     spl06_cal.c21 = ((uint16_t)caldata[14] << 8) | (uint16_t)caldata[15];
 195     spl06_cal.c30 = ((uint16_t)caldata[16] << 8) | (uint16_t)caldata[17];
 196
 197     return true;
 198 }
 199
 200 static bool spl06_configure_measurements(baroDev_t *baro)
 201 {
 202     uint8_t reg_value;
 203
 204     reg_value = SPL06_TEMP_USE_EXT_SENSOR | spl06_samples_to_cfg_reg_value(SPL06_TEMPERATURE_OVERSAMPLING);
 205     if (!busWrite(baro->busDev, SPL06_TEMPERATURE_CFG_REG, reg_value)) {
 206         return false;
 207     }
 208
 209     reg_value = spl06_samples_to_cfg_reg_value(SPL06_PRESSURE_OVERSAMPLING);
 210     if (!busWrite(baro->busDev, SPL06_PRESSURE_CFG_REG, reg_value)) {
 211         return false;
 212     }
 213
 214     reg_value = 0;
 215     if (SPL06_TEMPERATURE_OVERSAMPLING > 8) {
 216         reg_value |= SPL06_TEMPERATURE_RESULT_BIT_SHIFT;
 217     }
 218     if (SPL06_PRESSURE_OVERSAMPLING > 8) {
 219         reg_value |= SPL06_PRESSURE_RESULT_BIT_SHIFT;
 220     }
 221     if (!busWrite(baro->busDev, SPL06_INT_AND_FIFO_CFG_REG, reg_value)) {
 222         return false;
 223     }
 224
 225     return true;
 226 }
 227
 228 bool spl06Detect(baroDev_t *baro)
 229 {
 230     baro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_SPL06, 0, OWNER_BARO);
 231     if (baro->busDev == NULL) {
 232         return false;
 233     }
 234
 235     busSetSpeed(baro->busDev, BUS_SPEED_STANDARD);
 236
 237     if (!(deviceDetect(baro->busDev) && read_calibration_coefficients(baro) && spl06_configure_measurements(baro))) {
 238         busDeviceDeInit(baro->busDev);
 239         return false;
 240     }
 241
 242     baro->ut_delay = SPL06_MEASUREMENT_TIME(SPL06_TEMPERATURE_OVERSAMPLING) * 1000;
 243     baro->get_ut = spl06_read_temperature;
 244     baro->start_ut = spl06_start_temperature_measurement;
 245
 246     baro->up_delay = SPL06_MEASUREMENT_TIME(SPL06_PRESSURE_OVERSAMPLING) * 1000;
 247     baro->start_up = spl06_start_pressure_measurement;
 248     baro->get_up = spl06_read_pressure;
 249
 250     baro->calculate = spl06_calculate;
 251
 252     return true;
 253 }
 254
 255 #endif