src/main/drivers/barometer/barometer_ms56xx.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 "build/build_config.h"
  24 #include "common/utils.h"
  25
  26 #include "drivers/io.h"
  27 #include "drivers/bus.h"
  28 #include "drivers/time.h"
  29 #include "drivers/barometer/barometer.h"
  30 #include "drivers/barometer/barometer_ms56xx.h"
  31
  32 #if defined(USE_BARO_MS5607) || defined(USE_BARO_MS5611)
  33
  34 // MS56xx, Standard address 0x77
  35 #define MS56XX_ADDR             0x77
  36
  37 #define CMD_RESET               0x1E // ADC reset command
  38 #define CMD_ADC_READ            0x00 // ADC read command
  39 #define CMD_ADC_CONV            0x40 // ADC conversion command
  40 #define CMD_ADC_D1              0x00 // ADC D1 conversion
  41 #define CMD_ADC_D2              0x10 // ADC D2 conversion
  42 #define CMD_ADC_256             0x00 // ADC OSR=256
  43 #define CMD_ADC_512             0x02 // ADC OSR=512
  44 #define CMD_ADC_1024            0x04 // ADC OSR=1024
  45 #define CMD_ADC_2048            0x06 // ADC OSR=2048
  46 #define CMD_ADC_4096            0x08 // ADC OSR=4096
  47 #define CMD_PROM_RD             0xA0 // Prom read command
  48 #define PROM_NB                 8
  49
  50 STATIC_UNIT_TESTED uint32_t ms56xx_ut;  // static result of temperature measurement
  51 STATIC_UNIT_TESTED uint32_t ms56xx_up;  // static result of pressure measurement
  52 STATIC_UNIT_TESTED uint16_t ms56xx_c[PROM_NB];  // on-chip ROM
  53 static uint8_t ms56xx_osr = CMD_ADC_4096;
  54
  55 STATIC_UNIT_TESTED int8_t ms56xx_crc(uint16_t *prom)
  56 {
  57     int32_t i, j;
  58     uint32_t res = 0;
  59     uint8_t crc = prom[7] & 0xF;
  60     prom[7] &= 0xFF00;
  61
  62     bool blankEeprom = true;
  63
  64     for (i = 0; i < 16; i++) {
  65         if (prom[i >> 1]) {
  66             blankEeprom = false;
  67         }
  68         if (i & 1)
  69             res ^= ((prom[i >> 1]) & 0x00FF);
  70         else
  71             res ^= (prom[i >> 1] >> 8);
  72         for (j = 8; j > 0; j--) {
  73             if (res & 0x8000)
  74                 res ^= 0x1800;
  75             res <<= 1;
  76         }
  77     }
  78     prom[7] |= crc;
  79     if (!blankEeprom && crc == ((res >> 12) & 0xF))
  80         return 0;
  81
  82     return -1;
  83 }
  84
  85 static uint32_t ms56xx_read_adc(baroDev_t *baro)
  86 {
  87     uint8_t rxbuf[3];
  88     busReadBuf(baro->busDev, CMD_ADC_READ, rxbuf, 3);
  89     return (rxbuf[0] << 16) | (rxbuf[1] << 8) | rxbuf[2];
  90 }
  91
  92 static bool ms56xx_start_ut(baroDev_t *baro)
  93 {
  94     return busWrite(baro->busDev, CMD_ADC_CONV + CMD_ADC_D2 + ms56xx_osr, 1);
  95 }
  96
  97 static bool ms56xx_get_ut(baroDev_t *baro)
  98 {
  99     ms56xx_ut = ms56xx_read_adc(baro);
 100     return true;
 101 }
 102
 103 static bool ms56xx_start_up(baroDev_t *baro)
 104 {
 105     return busWrite(baro->busDev, CMD_ADC_CONV + CMD_ADC_D1 + ms56xx_osr, 1);
 106 }
 107
 108 static bool ms56xx_get_up(baroDev_t *baro)
 109 {
 110     ms56xx_up = ms56xx_read_adc(baro);
 111     return true;
 112 }
 113
 114 #ifdef USE_BARO_MS5611
 115 STATIC_UNIT_TESTED bool ms5611_calculate(baroDev_t *baro, int32_t *pressure, int32_t *temperature)
 116 {
 117     UNUSED(baro);
 118     uint32_t press;
 119     int64_t temp;
 120     int64_t delt;
 121     int64_t dT = (int64_t)ms56xx_ut - ((uint64_t)ms56xx_c[5] * 256);
 122     int64_t off = ((int64_t)ms56xx_c[2] << 16) + (((int64_t)ms56xx_c[4] * dT) >> 7);
 123     int64_t sens = ((int64_t)ms56xx_c[1] << 15) + (((int64_t)ms56xx_c[3] * dT) >> 8);
 124     temp = 2000 + ((dT * (int64_t)ms56xx_c[6]) >> 23);
 125
 126     if (temp < 2000) { // temperature lower than 20degC
 127         delt = temp - 2000;
 128         delt = 5 * delt * delt;
 129         off -= delt >> 1;
 130         sens -= delt >> 2;
 131         if (temp < -1500) { // temperature lower than -15degC
 132             delt = temp + 1500;
 133             delt = delt * delt;
 134             off -= 7 * delt;
 135             sens -= (11 * delt) >> 1;
 136         }
 137     temp -= ((dT * dT) >> 31);
 138     }
 139     press = ((((int64_t)ms56xx_up * sens) >> 21) - off) >> 15;
 140
 141     if (pressure)
 142         *pressure = press;
 143     if (temperature)
 144         *temperature = temp;
 145
 146     return true;
 147 }
 148 #endif
 149
 150 #ifdef USE_BARO_MS5607
 151 STATIC_UNIT_TESTED bool ms5607_calculate(baroDev_t *baro, int32_t *pressure, int32_t *temperature)
 152 {
 153     UNUSED(baro);
 154     uint32_t press;
 155     int64_t temp;
 156     int64_t delt;
 157     int64_t dT = (int64_t)ms56xx_ut - ((uint64_t)ms56xx_c[5] << 8);
 158     int64_t off = ((int64_t)ms56xx_c[2] << 17) + (((int64_t)ms56xx_c[4] * dT) >> 6);
 159     int64_t sens = ((int64_t)ms56xx_c[1] << 16) + (((int64_t)ms56xx_c[3] * dT) >> 7);
 160     temp = 2000 + ((dT * (int64_t)ms56xx_c[6]) >> 23);
 161
 162     if (temp < 2000) { // temperature lower than 20degC
 163         delt = temp - 2000;
 164         delt = delt * delt;
 165         off -= (61 * delt) >> 4;
 166         sens -= 2 * delt;
 167         if (temp < -1500) { // temperature lower than -15degC
 168             delt = temp + 1500;
 169             delt = delt * delt;
 170             off -= 15 * delt;
 171             sens -= 8 * delt;
 172         }
 173         temp -= ((dT * dT) >> 31);
 174     }
 175     press = ((((int64_t)ms56xx_up * sens) >> 21) - off) >> 15;
 176
 177     if (pressure)
 178         *pressure = press;
 179     if (temperature)
 180         *temperature = temp;
 181
 182     return true;
 183 }
 184 #endif
 185
 186 #define DETECTION_MAX_RETRY_COUNT   5
 187 static bool deviceDetect(busDevice_t * dev)
 188 {
 189     for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT; retry++) {
 190         uint8_t sig = 0;
 191
 192         delay(10);
 193
 194         bool ack = busRead(dev, CMD_PROM_RD, &sig);
 195         if (ack && sig != 0xFF) {
 196             return true;
 197         }
 198     };
 199
 200     return false;
 201 }
 202
 203 static bool deviceInit(baroDev_t *baro)
 204 {
 205     busSetSpeed(baro->busDev, BUS_SPEED_STANDARD);
 206
 207     busWrite(baro->busDev, CMD_RESET, 1);
 208     delay(5);
 209
 210     // read all coefficients
 211     for (int i = 0; i < PROM_NB; i++) {
 212         uint8_t rxbuf[2] = { 0, 0 };
 213         busReadBuf(baro->busDev, CMD_PROM_RD + i * 2, rxbuf, 2);
 214         ms56xx_c[i] = (rxbuf[0] << 8 | rxbuf[1]);
 215     }
 216
 217     // check crc, bail out if wrong - we are probably talking to BMP085 w/o XCLR line!
 218     if (ms56xx_crc(ms56xx_c) != 0) {
 219         return false;
 220     }
 221
 222     baro->ut_delay = 10000;
 223     baro->up_delay = 10000;
 224     baro->start_ut = ms56xx_start_ut;
 225     baro->get_ut = ms56xx_get_ut;
 226     baro->start_up = ms56xx_start_up;
 227     baro->get_up = ms56xx_get_up;
 228
 229     return true;
 230 }
 231
 232 #ifdef USE_BARO_MS5607
 233 bool ms5607Detect(baroDev_t *baro)
 234 {
 235     baro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_MS5607, 0, OWNER_BARO);
 236     if (baro->busDev == NULL) {
 237         return false;
 238     }
 239
 240     if (!deviceDetect(baro->busDev)) {
 241         busDeviceDeInit(baro->busDev);
 242         return false;
 243     }
 244
 245     if (!deviceInit(baro)) {
 246         busDeviceDeInit(baro->busDev);
 247         return false;
 248     }
 249
 250     baro->calculate = ms5607_calculate;
 251
 252     return true;
 253 }
 254 #endif
 255
 256 #ifdef USE_BARO_MS5611
 257 bool ms5611Detect(baroDev_t *baro)
 258 {
 259     baro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_MS5611, 0, OWNER_BARO);
 260     if (baro->busDev == NULL) {
 261         return false;
 262     }
 263
 264     if (!deviceDetect(baro->busDev)) {
 265         busDeviceDeInit(baro->busDev);
 266         return false;
 267     }
 268
 269     if (!deviceInit(baro)) {
 270         busDeviceDeInit(baro->busDev);
 271         return false;
 272     }
 273
 274     baro->calculate = ms5611_calculate;
 275
 276     return true;
 277 }
 278 #endif
 279
 280 #endif