src/main/drivers/barometer_ms5611.c

   1 /*
   2  * This file is part of Cleanflight.
   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 <stdbool.h>
  19 #include <stdint.h>
  20
  21 #include <platform.h>
  22
  23 #include "barometer.h"
  24
  25 #include "gpio.h"
  26 #include "system.h"
  27 #include "bus_i2c.h"
  28
  29 #include "build_config.h"
  30
  31 // MS5611, Standard address 0x77
  32 #define MS5611_ADDR                 0x77
  33
  34 #define CMD_RESET               0x1E // ADC reset command
  35 #define CMD_ADC_READ            0x00 // ADC read command
  36 #define CMD_ADC_CONV            0x40 // ADC conversion command
  37 #define CMD_ADC_D1              0x00 // ADC D1 conversion
  38 #define CMD_ADC_D2              0x10 // ADC D2 conversion
  39 #define CMD_ADC_256             0x00 // ADC OSR=256
  40 #define CMD_ADC_512             0x02 // ADC OSR=512
  41 #define CMD_ADC_1024            0x04 // ADC OSR=1024
  42 #define CMD_ADC_2048            0x06 // ADC OSR=2048
  43 #define CMD_ADC_4096            0x08 // ADC OSR=4096
  44 #define CMD_PROM_RD             0xA0 // Prom read command
  45 #define PROM_NB                 8
  46
  47 static void ms5611_reset(void);
  48 static uint16_t ms5611_prom(int8_t coef_num);
  49 STATIC_UNIT_TESTED int8_t ms5611_crc(uint16_t *prom);
  50 static uint32_t ms5611_read_adc(void);
  51 static void ms5611_start_ut(void);
  52 static void ms5611_get_ut(void);
  53 static void ms5611_start_up(void);
  54 static void ms5611_get_up(void);
  55 STATIC_UNIT_TESTED void ms5611_calculate(int32_t *pressure, int32_t *temperature);
  56
  57 STATIC_UNIT_TESTED uint32_t ms5611_ut;  // static result of temperature measurement
  58 STATIC_UNIT_TESTED uint32_t ms5611_up;  // static result of pressure measurement
  59 STATIC_UNIT_TESTED uint16_t ms5611_c[PROM_NB];  // on-chip ROM
  60 static uint8_t ms5611_osr = CMD_ADC_4096;
  61
  62 bool ms5611Detect(baro_t *baro)
  63 {
  64     bool ack = false;
  65     uint8_t sig;
  66     int i;
  67
  68     delay(10); // No idea how long the chip takes to power-up, but let's make it 10ms
  69
  70     ack = i2cRead(MS5611_ADDR, CMD_PROM_RD, 1, &sig);
  71     if (!ack)
  72         return false;
  73
  74     ms5611_reset();
  75     // read all coefficients
  76     for (i = 0; i < PROM_NB; i++)
  77         ms5611_c[i] = ms5611_prom(i);
  78     // check crc, bail out if wrong - we are probably talking to BMP085 w/o XCLR line!
  79     if (ms5611_crc(ms5611_c) != 0)
  80         return false;
  81
  82     // TODO prom + CRC
  83     baro->ut_delay = 10000;
  84     baro->up_delay = 10000;
  85     baro->start_ut = ms5611_start_ut;
  86     baro->get_ut = ms5611_get_ut;
  87     baro->start_up = ms5611_start_up;
  88     baro->get_up = ms5611_get_up;
  89     baro->calculate = ms5611_calculate;
  90
  91     return true;
  92 }
  93
  94 static void ms5611_reset(void)
  95 {
  96     i2cWrite(MS5611_ADDR, CMD_RESET, 1);
  97     delayMicroseconds(2800);
  98 }
  99
 100 static uint16_t ms5611_prom(int8_t coef_num)
 101 {
 102     uint8_t rxbuf[2] = { 0, 0 };
 103     i2cRead(MS5611_ADDR, CMD_PROM_RD + coef_num * 2, 2, rxbuf); // send PROM READ command
 104     return rxbuf[0] << 8 | rxbuf[1];
 105 }
 106
 107 STATIC_UNIT_TESTED int8_t ms5611_crc(uint16_t *prom)
 108 {
 109     int32_t i, j;
 110     uint32_t res = 0;
 111     uint8_t crc = prom[7] & 0xF;
 112     prom[7] &= 0xFF00;
 113
 114     bool blankEeprom = true;
 115
 116     for (i = 0; i < 16; i++) {
 117         if (prom[i >> 1]) {
 118             blankEeprom = false;
 119         }
 120         if (i & 1)
 121             res ^= ((prom[i >> 1]) & 0x00FF);
 122         else
 123             res ^= (prom[i >> 1] >> 8);
 124         for (j = 8; j > 0; j--) {
 125             if (res & 0x8000)
 126                 res ^= 0x1800;
 127             res <<= 1;
 128         }
 129     }
 130     prom[7] |= crc;
 131     if (!blankEeprom && crc == ((res >> 12) & 0xF))
 132         return 0;
 133
 134     return -1;
 135 }
 136
 137 static uint32_t ms5611_read_adc(void)
 138 {
 139     uint8_t rxbuf[3];
 140     i2cRead(MS5611_ADDR, CMD_ADC_READ, 3, rxbuf); // read ADC
 141     return (rxbuf[0] << 16) | (rxbuf[1] << 8) | rxbuf[2];
 142 }
 143
 144 static void ms5611_start_ut(void)
 145 {
 146     i2cWrite(MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D2 + ms5611_osr, 1); // D2 (temperature) conversion start!
 147 }
 148
 149 static void ms5611_get_ut(void)
 150 {
 151     ms5611_ut = ms5611_read_adc();
 152 }
 153
 154 static void ms5611_start_up(void)
 155 {
 156     i2cWrite(MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D1 + ms5611_osr, 1); // D1 (pressure) conversion start!
 157 }
 158
 159 static void ms5611_get_up(void)
 160 {
 161     ms5611_up = ms5611_read_adc();
 162 }
 163
 164 STATIC_UNIT_TESTED void ms5611_calculate(int32_t *pressure, int32_t *temperature)
 165 {
 166     uint32_t press;
 167     int64_t temp;
 168     int64_t delt;
 169     int64_t dT = (int64_t)ms5611_ut - ((uint64_t)ms5611_c[5] * 256);
 170     int64_t off = ((int64_t)ms5611_c[2] << 16) + (((int64_t)ms5611_c[4] * dT) >> 7);
 171     int64_t sens = ((int64_t)ms5611_c[1] << 15) + (((int64_t)ms5611_c[3] * dT) >> 8);
 172     temp = 2000 + ((dT * (int64_t)ms5611_c[6]) >> 23);
 173
 174     if (temp < 2000) { // temperature lower than 20degC
 175         delt = temp - 2000;
 176         delt = 5 * delt * delt;
 177         off -= delt >> 1;
 178         sens -= delt >> 2;
 179         if (temp < -1500) { // temperature lower than -15degC
 180             delt = temp + 1500;
 181             delt = delt * delt;
 182             off -= 7 * delt;
 183             sens -= (11 * delt) >> 1;
 184         }
 185     temp -= ((dT * dT) >> 31);
 186     }
 187     press = ((((int64_t)ms5611_up * sens) >> 21) - off) >> 15;
 188
 189
 190     if (pressure)
 191         *pressure = press;
 192     if (temperature)
 193         *temperature = temp;
 194 }