src/main/drivers/barometer/barometer_ms5611.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23
  24 #include "platform.h"
  25
  26 #if defined(USE_BARO) && (defined(USE_BARO_MS5611) || defined(USE_BARO_SPI_MS5611))
  27
  28 #include "build/build_config.h"
  29
  30 #include "barometer.h"
  31 #include "barometer_ms5611.h"
  32
  33 #include "drivers/bus_i2c.h"
  34 #include "drivers/bus_i2c_busdev.h"
  35 #include "drivers/bus_spi.h"
  36 #include "drivers/io.h"
  37 #include "drivers/time.h"
  38
  39 // 10 MHz max SPI frequency
  40 #define MS5611_MAX_SPI_CLK_HZ 10000000
  41
  42 // MS5611, Standard address 0x77
  43 #define MS5611_I2C_ADDR                 0x77
  44
  45 #define CMD_RESET               0x1E // ADC reset command
  46 #define CMD_ADC_READ            0x00 // ADC read command
  47 #define CMD_ADC_CONV            0x40 // ADC conversion command
  48 #define CMD_ADC_D1              0x00 // ADC D1 conversion
  49 #define CMD_ADC_D2              0x10 // ADC D2 conversion
  50 #define CMD_ADC_256             0x00 // ADC OSR=256
  51 #define CMD_ADC_512             0x02 // ADC OSR=512
  52 #define CMD_ADC_1024            0x04 // ADC OSR=1024
  53 #define CMD_ADC_2048            0x06 // ADC OSR=2048
  54 #define CMD_ADC_4096            0x08 // ADC OSR=4096
  55 #define CMD_PROM_RD             0xA0 // Prom read command
  56 #define PROM_NB                 8
  57
  58 STATIC_UNIT_TESTED uint32_t ms5611_ut;  // static result of temperature measurement
  59 STATIC_UNIT_TESTED uint32_t ms5611_up;  // static result of pressure measurement
  60 STATIC_UNIT_TESTED uint16_t ms5611_c[PROM_NB];  // on-chip ROM
  61 static uint8_t ms5611_osr = CMD_ADC_4096;
  62 #define MS5611_DATA_FRAME_SIZE 3
  63 static DMA_DATA_ZERO_INIT uint8_t sensor_data[MS5611_DATA_FRAME_SIZE];
  64
  65 void ms5611BusInit(const extDevice_t *dev)
  66 {
  67 #ifdef USE_BARO_SPI_MS5611
  68     if (dev->bus->busType == BUS_TYPE_SPI) {
  69         IOHi(dev->busType_u.spi.csnPin); // Disable
  70         IOInit(dev->busType_u.spi.csnPin, OWNER_BARO_CS, 0);
  71         IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_OUT_PP);
  72         spiSetClkDivisor(dev, spiCalculateDivider(MS5611_MAX_SPI_CLK_HZ));
  73     }
  74 #else
  75     UNUSED(dev);
  76 #endif
  77 }
  78
  79 void ms5611BusDeinit(const extDevice_t *dev)
  80 {
  81 #ifdef USE_BARO_SPI_MS5611
  82     if (dev->bus->busType == BUS_TYPE_SPI) {
  83         spiPreinitByIO(dev->busType_u.spi.csnPin);
  84     }
  85 #else
  86     UNUSED(dev);
  87 #endif
  88 }
  89
  90 static void ms5611Reset(const extDevice_t *dev)
  91 {
  92     busRawWriteRegister(dev, CMD_RESET, 1);
  93
  94     delayMicroseconds(2800);
  95 }
  96
  97 static uint16_t ms5611Prom(const extDevice_t *dev, int8_t coef_num)
  98 {
  99     uint8_t rxbuf[2] = { 0, 0 };
 100
 101     busRawReadRegisterBuffer(dev, CMD_PROM_RD + coef_num * 2, rxbuf, 2); // send PROM READ command
 102
 103     return rxbuf[0] << 8 | rxbuf[1];
 104 }
 105
 106 STATIC_UNIT_TESTED int8_t ms5611CRC(uint16_t *prom)
 107 {
 108     int32_t i, j;
 109     uint32_t res = 0;
 110     uint8_t crc = prom[7] & 0xF;
 111     prom[7] &= 0xFF00;
 112
 113     bool blankEeprom = true;
 114
 115     for (i = 0; i < 16; i++) {
 116         if (prom[i >> 1]) {
 117             blankEeprom = false;
 118         }
 119         if (i & 1)
 120             res ^= ((prom[i >> 1]) & 0x00FF);
 121         else
 122             res ^= (prom[i >> 1] >> 8);
 123         for (j = 8; j > 0; j--) {
 124             if (res & 0x8000)
 125                 res ^= 0x1800;
 126             res <<= 1;
 127         }
 128     }
 129     prom[7] |= crc;
 130     if (!blankEeprom && crc == ((res >> 12) & 0xF))
 131         return 0;
 132
 133     return -1;
 134 }
 135
 136 static void ms5611ReadAdc(const extDevice_t *dev)
 137 {
 138     busRawReadRegisterBufferStart(dev, CMD_ADC_READ, sensor_data, MS5611_DATA_FRAME_SIZE); // read ADC
 139 }
 140
 141 static bool ms5611StartUT(baroDev_t *baro)
 142 {
 143     return busRawWriteRegisterStart(&baro->dev, CMD_ADC_CONV + CMD_ADC_D2 + ms5611_osr, 1); // D2 (temperature) conversion start!
 144 }
 145
 146 static bool ms5611ReadUT(baroDev_t *baro)
 147 {
 148     if (busBusy(&baro->dev, NULL)) {
 149         return false;
 150     }
 151
 152     ms5611ReadAdc(&baro->dev);
 153
 154     return true;
 155 }
 156
 157 static bool ms5611GetUT(baroDev_t *baro)
 158 {
 159     if (busBusy(&baro->dev, NULL)) {
 160         return false;
 161     }
 162
 163     ms5611_ut = sensor_data[0] << 16 | sensor_data[1] << 8 | sensor_data[2];
 164
 165     return true;
 166 }
 167
 168 static bool ms5611StartUP(baroDev_t *baro)
 169 {
 170     return busRawWriteRegisterStart(&baro->dev, CMD_ADC_CONV + CMD_ADC_D1 + ms5611_osr, 1); // D1 (pressure) conversion start!
 171 }
 172
 173 static bool ms5611ReadUP(baroDev_t *baro)
 174 {
 175     if (busBusy(&baro->dev, NULL)) {
 176         return false;
 177     }
 178
 179     ms5611ReadAdc(&baro->dev);
 180
 181     return true;
 182 }
 183
 184 static bool ms5611GetUP(baroDev_t *baro)
 185 {
 186     if (busBusy(&baro->dev, NULL)) {
 187         return false;
 188     }
 189
 190     ms5611_up = sensor_data[0] << 16 | sensor_data[1] << 8 | sensor_data[2];
 191
 192     return true;
 193 }
 194
 195 STATIC_UNIT_TESTED void ms5611Calculate(int32_t *pressure, int32_t *temperature)
 196 {
 197     uint32_t press;
 198     int64_t temp;
 199     int64_t delt;
 200     int64_t dT = (int64_t)ms5611_ut - ((uint64_t)ms5611_c[5] * 256);
 201     int64_t off = ((int64_t)ms5611_c[2] << 16) + (((int64_t)ms5611_c[4] * dT) >> 7);
 202     int64_t sens = ((int64_t)ms5611_c[1] << 15) + (((int64_t)ms5611_c[3] * dT) >> 8);
 203     temp = 2000 + ((dT * (int64_t)ms5611_c[6]) >> 23);
 204
 205     if (temp < 2000) { // temperature lower than 20degC
 206         delt = temp - 2000;
 207         delt = 5 * delt * delt;
 208         off -= delt >> 1;
 209         sens -= delt >> 2;
 210         if (temp < -1500) { // temperature lower than -15degC
 211             delt = temp + 1500;
 212             delt = delt * delt;
 213             off -= 7 * delt;
 214             sens -= (11 * delt) >> 1;
 215         }
 216     temp -= ((dT * dT) >> 31);
 217     }
 218     press = ((((int64_t)ms5611_up * sens) >> 21) - off) >> 15;
 219
 220     if (pressure)
 221         *pressure = press;
 222     if (temperature)
 223         *temperature = temp;
 224 }
 225
 226 bool ms5611Detect(baroDev_t *baro)
 227 {
 228     uint8_t sig;
 229     int i;
 230     bool defaultAddressApplied = false;
 231
 232     delay(10); // No idea how long the chip takes to power-up, but let's make it 10ms
 233
 234     extDevice_t *dev = &baro->dev;
 235
 236     ms5611BusInit(dev);
 237
 238     if ((dev->bus->busType == BUS_TYPE_I2C) && (dev->busType_u.i2c.address == 0)) {
 239         // Default address for MS5611
 240         dev->busType_u.i2c.address = MS5611_I2C_ADDR;
 241         defaultAddressApplied = true;
 242     }
 243
 244     if (!busRawReadRegisterBuffer(dev, CMD_PROM_RD, &sig, 1) || sig == 0xFF) {
 245         goto fail;
 246     }
 247
 248     ms5611Reset(dev);
 249
 250     // read all coefficients
 251     for (i = 0; i < PROM_NB; i++)
 252         ms5611_c[i] = ms5611Prom(dev, i);
 253
 254     // check crc, bail out if wrong - we are probably talking to BMP085 w/o XCLR line!
 255     if (ms5611CRC(ms5611_c) != 0) {
 256         goto fail;
 257     }
 258
 259     busDeviceRegister(dev);
 260
 261     // TODO prom + CRC
 262     baro->ut_delay = 10000;
 263     baro->up_delay = 10000;
 264     baro->start_ut = ms5611StartUT;
 265     baro->read_ut = ms5611ReadUT;
 266     baro->get_ut = ms5611GetUT;
 267     baro->start_up = ms5611StartUP;
 268     baro->read_up = ms5611ReadUP;
 269     baro->get_up = ms5611GetUP;
 270     baro->calculate = ms5611Calculate;
 271
 272     return true;
 273
 274 fail:;
 275     ms5611BusDeinit(dev);
 276
 277     if (defaultAddressApplied) {
 278         dev->busType_u.i2c.address = 0;
 279     }
 280
 281     return false;
 282 }
 283 #endif