openXsensor/oXs_bmp280.cpp

   1 #include "oXs_bmp280.h"
   2
   3 #if defined(SENSOR_IS_BMP280)
   4
   5 #ifdef DEBUG
   6 //#define DEBUGI2CBMP280
   7 //#define DEBUGDATA
   8 //#define DEBUGVARIOI2C
   9 #endif
  10
  11 extern unsigned long micros( void ) ;
  12 extern unsigned long millis( void ) ;
  13 extern void delay(unsigned long ms) ;
  14
  15 static BMP280_CALIB_DATA _bmp280_coeffs;   // Last read calibration data will be available here
  16 //static uint8_t           _bmp085Mode;
  17
  18
  19 #ifdef DEBUG
  20 OXS_BMP280::OXS_BMP280( HardwareSerial &print)
  21 #else
  22 OXS_BMP280::OXS_BMP280(void)
  23 #endif
  24 {
  25   // constructor
  26   //_addr=addr;
  27   _addr = BMP280_ADR ;
  28   varioData.SensorState = 0 ;
  29 #ifdef DEBUG
  30   printer = &print; //operate on the address of print
  31   printer->begin(115200);
  32   printer->print("Vario Sensor:BMP280 I2C Addr=");
  33   printer->println(_addr,HEX);
  34 #endif
  35 }
  36
  37
  38 // **************** Setup the BMP280 sensor *********************
  39 void OXS_BMP280::setup() {
  40   unsigned int _calibrationData[13]; // The factory calibration data of the BMP280
  41   varioData.absoluteAlt.available = false ;
  42   varioData.relativeAlt.available = false ;
  43   varioData.climbRate.available = false ;
  44   varioData.sensitivity.available = false ;
  45 //  varioData.vSpeed10SecAvailable = false ;
  46   sensitivityMin = SENSITIVITY_MIN ; // set the min smoothing to the default value
  47   varioData.delaySmooth = 20000 ; // delay between 2 altitude calculation = 20msec = 20000 usec
  48   nextAltMillis  =  5000 ;  // in msec; save when Altitude has to be calculated; altitude is available only after some delay in order to get a stable value (less temperature drift)
  49 //  nextAverageAltMillis =  nextAltMillis ;  // in msec ; save when AverageAltitude has to be calculated
  50 //  nextAverageAltMillis =  nextAltMillis ;
  51
  52 //#ifdef ALT_TEMP_COMPENSATION
  53 //  alt_temp_compensation = ALT_TEMP_COMPENSATION ;
  54 //#endif
  55
  56
  57 #ifdef DEBUG
  58   printer->print(F("Vario Sensor:BMP280 "));
  59   printer->println(" ");
  60   printer->print(F(" milli="));
  61   printer->println(millis());
  62
  63 #endif
  64
  65   I2c.begin() ;
  66   I2c.timeOut( 80); //initialise the time out in order to avoid infinite loop
  67 #ifdef DEBUGI2CBMP280
  68   I2c.scan() ;
  69   printer->print(F("last I2C scan adr: "));
  70   printer->println( I2c.scanAdr , HEX  );
  71 #endif
  72
  73 // write in register 0xF4 value 0x33 (it means oversampling temperature 1 X , oversampling pressure 8 X and normal mode = continue )
  74   errorI2C = I2c.write( _addr , (uint8_t) 0xF4 , (uint8_t) 0x33 ) ;
  75 // write in register 0xF5 value 0x00 (it means 0.5msec between sampling, no filter, I2C protocol )
  76   errorI2C = I2c.write( _addr , (uint8_t) 0xF5 , (uint8_t) 0x00 ) ;
  77
  78   errorCalibration = false ;
  79   for (byte i = 1; i <=12; i++) {
  80     errorI2C =  I2c.read( _addr , 0x86 + i*2, 2 ) ; //read 2 bytes from the device after sending the register to be read (first register = 0x86 (=register AC1)
  81     if ( errorI2C > 0 ) {
  82 #ifdef DEBUG
  83       printer->print(F("error code in setup I2CRead: "));
  84       printer->println( errorI2C );
  85 #endif
  86       errorCalibration = true ;
  87     } else {
  88       low = I2c.receive() ;
  89       high = I2c.receive() ;
  90       _calibrationData[i] = high<<8 | low;
  91     }
  92 #ifdef DEBUG
  93     printer->print(F("calibration data #"));
  94     printer->print(i);
  95     printer->print(F(" = "));
  96     printer->print( _calibrationData[i] );
  97     printer->print(F(" error= "));
  98     printer->println( errorI2C );
  99 #endif
 100   } // End for
 101
 102   _bmp280_coeffs.dig_T1 = _calibrationData[1];
 103   _bmp280_coeffs.dig_T2 = _calibrationData[2];
 104   _bmp280_coeffs.dig_T3 = _calibrationData[3];
 105   _bmp280_coeffs.dig_P1 = _calibrationData[4];
 106   _bmp280_coeffs.dig_P2 = _calibrationData[5];
 107   _bmp280_coeffs.dig_P3 = _calibrationData[6];
 108   _bmp280_coeffs.dig_P4  = _calibrationData[7];
 109   _bmp280_coeffs.dig_P5  = _calibrationData[8];
 110   _bmp280_coeffs.dig_P6  = _calibrationData[9];
 111   _bmp280_coeffs.dig_P7  = _calibrationData[10];
 112   _bmp280_coeffs.dig_P8  = _calibrationData[11];
 113   _bmp280_coeffs.dig_P9  = _calibrationData[12];
 114 //  _bmp280Mode        = 1; // perform an average of 2 pressure reads
 115
 116
 117
 118
 119 #ifdef DEBUG
 120   printer->println(F("setup vario done."));
 121 #endif
 122
 123 }  //end of setup
 124
 125
 126 //********************************************************************************************
 127 //***                            read the sensor                                           ***
 128 //********************************************************************************************
 129 bool OXS_BMP280::readSensor() {
 130   //static uint32_t lastBMP280ReadMicro ;
 131   bool newVSpeedCalculated  = false ;
 132   if ( ( micros() - varioData.lastCommandMicros ) > 20000)  {
 133     int32_t adc_T = 0 ;
 134     int32_t adc_P = 0 ;
 135     int32_t t_fine; // t_fine carries fine temperature as global value
 136     int32_t var1 ;
 137     int32_t var2 ;
 138     int32_t T; // Returns temperature in DegC, resolution is 0.01 DegC. Output value of “5123” equals 51.23 DegC.
 139     uint32_t p; // pressure in pascal
 140
 141     errorI2C = I2c.read( _addr , (uint8_t) 0xF7 , (uint8_t) 6) ; // read 6 bytes starting from register F7
 142     adc_P = I2c.receive() ;
 143     adc_P <<= 8 ;
 144     adc_P |= I2c.receive() ;
 145     adc_P <<= 8 ;
 146     adc_P |= I2c.receive();
 147     adc_P = adc_P >> 4;
 148     adc_T = I2c.receive();
 149     adc_T <<= 8 ;
 150     adc_T |= I2c.receive();
 151     adc_T <<= 8 ;
 152     adc_T |= I2c.receive();
 153     adc_T = adc_T >> 4 ;
 154     varioData.lastCommandMicros = micros() ;
 155
 156     var1 = ((((adc_T>>3) - ((int32_t)_bmp280_coeffs.dig_T1<<1))) * ((int32_t)_bmp280_coeffs.dig_T2)) >> 11;
 157     var2 = (((((adc_T>>4) - ((int32_t)_bmp280_coeffs.dig_T1)) * ((adc_T>>4) - ((int32_t)_bmp280_coeffs.dig_T1))) >> 12) * ((int32_t)_bmp280_coeffs.dig_T3)) >> 14;
 158     t_fine = var1 + var2;
 159     varioData.temperature = (t_fine * 5 + 128) >> 8;
 160
 161     var1 = (((int32_t)t_fine)>>1) - (int32_t)64000;
 162     var2 = (((var1>>2) * (var1>>2)) >> 11 ) * ((int32_t)_bmp280_coeffs.dig_P6);
 163     var2 = var2 + ((var1*((int32_t)_bmp280_coeffs.dig_P5))<<1);
 164     var2 = (var2>>2)+(((int32_t)_bmp280_coeffs.dig_P4)<<16);
 165     var1 = (((_bmp280_coeffs.dig_P3 * (((var1>>2) * (var1>>2)) >> 13 )) >> 3) + ((((int32_t)_bmp280_coeffs.dig_P2) * var1)>>1))>>18;
 166     var1 =((((32768+var1))*((int32_t)_bmp280_coeffs.dig_P1))>>15);
 167     if (var1 == 0) {
 168       varioData.rawPressure = 0 ;
 169     } else {
 170       p = (((uint32_t)(((int32_t)1048576) - adc_P)-(var2>>12)))*3125;
 171       if (p < 0x80000000)   {
 172         p = (p << 1) / ((uint32_t)var1);
 173       }   else   {
 174         p = (p / (uint32_t)var1) * 2;
 175       }
 176       var1 = (((int32_t)_bmp280_coeffs.dig_P9) * ((int32_t)(((p>>3) * (p>>3))>>13)))>>12;
 177       var2 = (((int32_t)(p>>2)) * ((int32_t)_bmp280_coeffs.dig_P8))>>13;
 178       varioData.rawPressure = (uint32_t)((int32_t)p + ((var1 + var2 + _bmp280_coeffs.dig_P7) >> 4))*  10000 ;
 179     }
 180     if ( (errorI2C == 0 ) & (millis() > 1000) ) { // If no I2c error  and if sensor is started since more than 1 sec, then calculate pressure etc...
 181       calculateVario() ;
 182       newVSpeedCalculated = true ;
 183     }
 184   } //   end check on 20 msec
 185   return newVSpeedCalculated ;                             // return true if a new Vspeed is available
 186 }  // end read sensor
 187
 188
 189
 190 void OXS_BMP280::calculateVario() {
 191
 192   // altitude = 44330 * (1.0 - pow(pressure /sealevelPressure,0.1903));
 193   // other alternative (faster) = 1013.25 = 0 m , 954.61 = 500m , etc...
 194   //      Pressure      Alt (m) Ratio
 195   //      101325        0       0.08526603
 196   //      95461 500     0.089525515
 197   //      89876 1000    0.094732853
 198   //      84598 1500    0.098039216
 199   //      79498 2000    0.103906899
 200   //      74686 2500    0.109313511
 201   //      70112 3000    0.115101289
 202   //      65768 3500    0.121270919
 203   //      61645 4000    0.127811861
 204   //      57733 4500    0.134843581
 205   //      54025 5000
 206
 207   if ( varioData.rawPressure > 954610000) {
 208     varioData.rawAltitude = ( 1013250000 - varioData.rawPressure ) * 0.08526603 ; // = 500 / (101325 - 95461)  // returned value 1234567 means 123,4567 m (temp is fixed to 15 degree celcius)
 209   } else if ( varioData.rawPressure > 898760000) {
 210     varioData.rawAltitude = 5000000 + ( 954610000 - varioData.rawPressure ) * 0.089525515  ;
 211   } else if ( varioData.rawPressure > 845980000) {
 212     varioData.rawAltitude = 10000000 + ( 898760000 - varioData.rawPressure ) * 0.094732853  ;
 213   } else if ( varioData.rawPressure > 794980000) {
 214     varioData.rawAltitude = 15000000 + ( 845980000 - varioData.rawPressure ) *  0.098039216 ;
 215   } else if ( varioData.rawPressure > 746860000) {
 216     varioData.rawAltitude = 20000000 + ( 794980000 - varioData.rawPressure ) *  0.103906899 ;
 217   } else if ( varioData.rawPressure > 701120000) {
 218     varioData.rawAltitude = 25000000 + ( 746860000 - varioData.rawPressure ) *  0.109313511 ;
 219   } else if ( varioData.rawPressure > 657680000) {
 220     varioData.rawAltitude = 30000000 + ( 701120000 - varioData.rawPressure ) *  0.115101289 ;
 221   } else if ( varioData.rawPressure > 616450000) {
 222     varioData.rawAltitude = 35000000 + ( 657680000 - varioData.rawPressure ) *  0.121270919 ;
 223   } else if ( varioData.rawPressure > 577330000) {
 224     varioData.rawAltitude = 40000000 + ( 616450000 - varioData.rawPressure ) *  0.127811861 ;
 225   } else {
 226     varioData.rawAltitude = 45000000 + ( 577330000 - varioData.rawPressure ) *  0.134843581 ;
 227   }
 228
 229 // here the classical way to calculate Vspeed with high and low pass filter
 230   if (altitude == 0) {
 231     altitudeLowPass = altitudeHighPass = altitude = varioData.rawAltitude ;
 232 //        pressureMicrosPrev2 = pressureMicrosPrev1 - 20000 ;
 233   }
 234   altitude += 0.04 * (varioData.rawAltitude - altitude) ;
 235 //      varioData.altitudeAt20MsecAvailable = true ; // inform openxsens.ino that calculation of dTE can be performed
 236
 237   altitudeLowPass += 0.085 * ( varioData.rawAltitude - altitudeLowPass) ;
 238   altitudeHighPass += 0.1 * ( varioData.rawAltitude - altitudeHighPass) ;
 239 //      if (pressureMicrosPrev1 > pressureMicrosPrev2 ) varioData.delaySmooth += 0.1 * ( pressureMicrosPrev1 -  pressureMicrosPrev2  - varioData.delaySmooth ) ; //delay between 2 measures  only if there is no overflow of pressureMicos
 240   climbRate2AltFloat = ((altitudeHighPass - altitudeLowPass )  * 5666.685 ) / 20000;
 241
 242   abs_deltaClimbRate =  abs(climbRate2AltFloat - varioData.climbRateFloat) ;
 243   if ( varioData.sensitivityPpm  > 0) {
 244     sensitivityMin =   varioData.sensitivityPpm ;
 245   }
 246   if ( (abs_deltaClimbRate <= SENSITIVITY_MIN_AT) || (sensitivityMin >= SENSITIVITY_MAX) ) {
 247     varioData.sensitivity.value = sensitivityMin ;
 248   } else if (abs_deltaClimbRate >= SENSITIVITY_MAX_AT)  {
 249     varioData.sensitivity.value = SENSITIVITY_MAX ;
 250   } else {
 251     varioData.sensitivity.value = sensitivityMin + ( SENSITIVITY_MAX - sensitivityMin ) * (abs_deltaClimbRate - SENSITIVITY_MIN_AT) / (SENSITIVITY_MAX_AT - SENSITIVITY_MIN_AT) ;
 252   }
 253   varioData.climbRateFloat += varioData.sensitivity.value * (climbRate2AltFloat - varioData.climbRateFloat)  * 0.001 ; // sensitivity is an integer and must be divided by 1000
 254
 255   if ( abs((int32_t)  varioData.climbRateFloat - varioData.climbRate.value) > VARIOHYSTERESIS ) {
 256     varioData.climbRate.value = (int32_t)  varioData.climbRateFloat  ;
 257   }
 258   varioData.climbRate.available=true; // allows SPORT protocol to transmit the value
 259 //      varioData.switchClimbRateAvailable = true ; // inform readsensors() that a switchable vspeed is available
 260 //      varioData.averageClimbRateAvailable = true ; // inform readsensors() that a vspeed is available to calculate the average
 261       // AltitudeAvailable is set to true only once every 100 msec in order to give priority to climb rate on SPORT
 262   altMillis = millis() ;
 263   if (altMillis > nextAltMillis){
 264     nextAltMillis = altMillis + 100 ;
 265     varioData.absoluteAlt.value = altitude / 100 ; // altitude is in m *10000 and AbsoluteAlt must be in m * 100
 266     varioData.absoluteAlt.available=true ;  // Altitude is considered as available only after several loop in order to reduce number of transmission on Sport.
 267     varioData.sensitivity.available = true ;
 268     if (varioData.altOffset == 0) {
 269       varioData.altOffset = varioData.absoluteAlt.value ;
 270     }
 271     varioData.relativeAlt.value = varioData.absoluteAlt.value - varioData.altOffset ;
 272     varioData.relativeAlt.available = true ;
 273     if ( varioData.relativeAlt.value > varioData.relativeAltMax ) {
 274       varioData.relativeAltMax = varioData.relativeAlt.value ;
 275     }
 276     varioData.relativeAltMaxAvailable = true ;
 277 //        if ( altMillis > nextAverageAltMillis ){ // calculation of the difference of altitude (in m) between the 10 last sec
 278 //            nextAverageAltMillis = altMillis + 500 ; // calculate only once every 500 msec
 279 //            varioData.vSpeed10Sec = (varioData.absoluteAlt.value - varioData.prevAlt[varioData.idxPrevAlt]) /100 ;
 280 //            varioData.prevAlt[varioData.idxPrevAlt] = varioData.absoluteAlt.value ;
 281 //            varioData.idxPrevAlt++ ;
 282 //            if ( varioData.idxPrevAlt >= 20 ) varioData.idxPrevAlt = 0 ;
 283 //            if ( altMillis > 15000) {  // make the data avalaible only after 15 sec)
 284 //                varioData.vSpeed10SecAvailable = true ;
 285 //            }
 286 //        }
 287
 288   } // end If (altMillis > nextAltMillis)
 289 #ifdef DEBUGDATA
 290   static bool firstPrintAlt = true ;
 291   if (firstPrintAlt == true) {
 292     firstPrintAlt = false ;
 293 //    printer->println(F( "T,Ra,Sm,A,NC,DS,AHP,ALP,CR2, Temp" )) ;
 294     printer->println(F( "T,Ra,Alt,vpsd, Alt2, rawVspd, vspd2 , smoothAlt, smoothVspd" )) ;
 295   }
 296   printer->print(  pressureMicrosPrev1 ) ;
 297   printer->print(",");
 298   printer->print(  (float) varioData.rawAltitude  ) ; printer->print(","); // alt is displayed in CM with 2 decimal
 299 //  printer->print(  expoSmooth ) ;
 300 //  printer->print(" ,");
 301   printer->print( (float) altitude  ) ;
 302   printer->print(" ,");
 303   printer->print( varioData.climbRate ) ;
 304   printer->print(" ,");
 305 //  printer->print( delaySmooth );
 306 //  printer->print(" ,");
 307 //  printer->print( altitudeHighPass );
 308 //  printer->print(" ,");
 309 //  printer->print( altitudeLowPass );
 310 //  printer->print(" ,");
 311 //  printer->print( climbRate2AltFloat );
 312 //  printer->print(" ,");
 313 //  printer->print( varioData.temperature );
 314 //  printer->print( smoothAltitude );
 315 //  printer->print(" ,");
 316 //  printer->print( rawRateVSpeed );
 317 //  printer->print(" ,");
 318 //  printer->print( smoothRateVSpeed );
 319 //  printer->print(" ,");
 320 //  printer->print( expoSmooth5611_alt_auto * 1000 );
 321 //  printer->print(" ,");
 322 //  printer->print( expoSmooth5611_vSpeed_auto * 1000 ) ;
 323 //  printer->print(" ,");
 324   printer->println( ) ;
 325
 326 #endif
 327
 328   pressureMicrosPrev2 = pressureMicrosPrev1 ;
 329 } // End of calculate Vario
 330
 331 #endif // end of #if defined(SENSOR_IS_BMP280)