openXsensor/oXs_bmp180.cpp

   1 #include "oXs_bmp180.h"
   2
   3 #if defined(SENSOR_IS_BMP180)
   4
   5 #ifdef DEBUG
   6 //#define DEBUGI2CMS5611
   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 bmp085_calib_data _bmp085_coeffs;   // Last read accelerometer data will be available here
  16 static uint8_t           _bmp085Mode;
  17
  18
  19 //long result ;
  20
  21
  22 #ifdef DEBUG
  23 OXS_BMP180::OXS_BMP180( HardwareSerial &print)
  24 #else
  25 OXS_BMP180::OXS_BMP180(void)
  26 #endif
  27 {
  28   // constructor
  29 //  _addr=addr;
  30   varioData.SensorState = 0 ;
  31 #ifdef DEBUG
  32   printer = &print; //operate on the address of print
  33 //  printer->begin(115200);
  34 //  printer->print("Vario Sensor:MS5611 I2C Addr=");
  35 //  printer->println(addr,HEX);
  36 #endif
  37 }
  38
  39
  40 // **************** Setup the BMP180 sensor *********************
  41 void OXS_BMP180::setup() {
  42   unsigned int _calibrationData[12]; // The factory calibration data of the BMP180
  43   varioData.absoluteAlt.available = false ;
  44   varioData.relativeAlt.available = false ;
  45   varioData.climbRate.available = false ;
  46   varioData.sensitivity.available = false ;
  47 //  varioData.vSpeed10SecAvailable = false ;
  48   sensitivityMin = SENSITIVITY_MIN ; // set the min smoothing to the default value
  49   varioData.delaySmooth = 20000 ; // delay between 2 altitude calculation = 20msec = 20000 usec
  50   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)
  51 //  nextAverageAltMillis =  nextAltMillis ;  // in msec ; save when AverageAltitude has to be calculated
  52 //  nextAverageAltMillis =  nextAltMillis ;
  53
  54 #ifdef ALT_TEMP_COMPENSATION
  55   alt_temp_compensation = ALT_TEMP_COMPENSATION ;
  56 #endif
  57
  58
  59 #ifdef DEBUG
  60   printer->print(F("Vario Sensor:BMP180 "));
  61   printer->println(" ");
  62   printer->print(F(" milli="));
  63   printer->println(millis());
  64
  65 #endif
  66
  67   I2c.begin() ;
  68   I2c.timeOut( 80); //initialise the time out in order to avoid infinite loop
  69 #ifdef DEBUGI2CBMP180
  70   I2c.scan() ;
  71   printer->print(F("last I2C scan adr: "));
  72   printer->println( I2c.scanAdr , HEX  );
  73 #endif
  74     errorCalibration = false ;
  75     for (byte i = 1; i <=11; i++) {
  76        errorI2C =  I2c.read( BMP180_ADR, 0xA8 + i*2, 2 ) ; //read 2 bytes from the device after sending the register to be read (first register = 0xAA (=register AC1)
  77        if ( errorI2C > 0 ) {
  78 #ifdef DEBUG
  79             printer->print(F("error code in setup I2CRead: "));
  80             printer->println( errorI2C );
  81 #endif
  82             errorCalibration = true ;
  83         } else {
  84             high = I2c.receive() ;
  85             low = I2c.receive() ;
  86             _calibrationData[i] = high<<8 | low;
  87         }
  88 #ifdef DEBUG
  89         printer->print(F("calibration data #"));
  90         printer->print(i);
  91         printer->print(F(" = "));
  92         printer->print( _calibrationData[i] );
  93         printer->print(F(" error= "));
  94         printer->println( errorI2C );
  95 #endif
  96     } // End for
  97
  98     _bmp085_coeffs.ac1 = _calibrationData[1];
  99     _bmp085_coeffs.ac2 = _calibrationData[2];
 100     _bmp085_coeffs.ac3 = _calibrationData[3];
 101     _bmp085_coeffs.ac4 = _calibrationData[4];
 102     _bmp085_coeffs.ac5 = _calibrationData[5];
 103     _bmp085_coeffs.ac6 = _calibrationData[6];
 104     _bmp085_coeffs.b1  = _calibrationData[7];
 105     _bmp085_coeffs.b2  = _calibrationData[8];
 106     _bmp085_coeffs.mb  = _calibrationData[9];
 107     _bmp085_coeffs.mc  = _calibrationData[10];
 108     _bmp085_coeffs.md  = _calibrationData[11];
 109     _bmp085Mode        = 1; // perform an average of 2 pressure reads
 110
 111
 112
 113 #ifdef DEBUG
 114   printer->println(F("setup vario done."));
 115 #endif
 116   I2c.write( BMP180_ADR , 0xF4 , 0x74) ;// ask a conversion of Pressure sending 74 in register F4; 74 means an average of 2 reads and so a normal wait time of 7.5 msec
 117   varioData.lastCommandMicros = micros() ;
 118   varioData.SensorState=0; //
 119
 120 }  //end of setup
 121
 122
 123 //********************************************************************************************
 124 //***                            read the sensor                                           ***
 125 //********************************************************************************************
 126 bool OXS_BMP180::readSensor() {
 127 long result = 0 ;
 128 #ifdef  DEBUGVARIOI2C
 129     printer->print(F("sensorState= "));
 130     printer->println(varioData.SensorState);
 131 #endif
 132   bool newVSpeedCalculated  = false ;
 133   if (varioData.SensorState==0) { // ========================= Read the pressure
 134     if (  ( micros() - varioData.lastCommandMicros) > 9000){ // wait 9 msec at least before asking for reading the pressure
 135 //        long result = 0;
 136               if(  ! I2c.read( BMP180_ADR, 0xF6, 3 )) { ; //read 3 bytes from the device starting from register F6; keep previous value in case of error
 137                 result = I2c.receive() ;
 138                 result <<= 8 ;
 139                 result |= I2c.receive() ;
 140                 result <<= 8 ;
 141                 result |= I2c.receive() ;
 142                 D1=result >> 7; // divide by 2* (8- the parameter for number of averages)
 143         } else {
 144               D1 = 0 ; // D1 value are not processed to calculate Alt.
 145         }
 146         I2c.write( BMP180_ADR ,0xF4 , 0x2E ) ; // ask a conversion of Temperature sending 2E in register F4
 147         varioData.lastCommandMicros = micros();
 148         varioData.SensorState = 1;
 149     } // end of delay of 9 ms
 150   } // end of SensorState == 1
 151   else if (varioData.SensorState==1){ // =========================
 152     if ( ( micros() - varioData.lastCommandMicros ) > 9000 ) { // wait 9000 usec to get Temp with high precision
 153           if ( ! I2c.read( BMP180_ADR , 0xF6, 2 )) { ; //read 2 bytes from the device in register F6 ; keep previous value in case of error
 154                 result = I2c.receive() ;
 155                 result <<= 8 ;
 156                 result |= I2c.receive() ;
 157                 D2=result;
 158           }
 159           I2c.write( BMP180_ADR , 0xF4 , 0x74) ;// ask a conversion of Pressure sending 74 in register F4; 74 means an average of 2 reads and so a normal wait time of 7.5 msec
 160           varioData.lastCommandMicros = micros() ;
 161           varioData.SensorState=2; //
 162     }  // End of process if temperature can be read
 163   } // End of process if SensorState was 1
 164   else if (varioData.SensorState==2) {    // ========================== new Pressure and (new or old) Temp are known so Request Pressure immediately and calculate altitude
 165     varioData.SensorState=0;
 166     if ((D1 > 0) & (millis() > 3000) ) { // If D1 has been read in a previous loop and if sensor is started since more than 3 sec, then calculate pressure etc...
 167               calculateVario() ;
 168               newVSpeedCalculated = true ;
 169     }
 170   } //   end ( varioData.SensorState == 2 )
 171   return newVSpeedCalculated ;                             // return true if a new Vspeed is available
 172 }  // end read sensor
 173
 174
 175 void OXS_BMP180::calculateVario() {
 176       if (D2Prev == 0) D2Prev = D2 ;
 177       D2Apply = (D2 + D2Prev ) / 2 ;
 178       D2Prev = D2 ;
 179 #if BMP085_USE_DATASHEET_VALS
 180     _bmp085_coeffs.ac1 = 408;
 181     _bmp085_coeffs.ac2 = -72;
 182     _bmp085_coeffs.ac3 = -14383;
 183     _bmp085_coeffs.ac4 = 32741;
 184     _bmp085_coeffs.ac5 = 32757;
 185     _bmp085_coeffs.ac6 = 23153;
 186     _bmp085_coeffs.b1  = 6190;
 187     _bmp085_coeffs.b2  = 4;
 188     _bmp085_coeffs.mb  = -32768;
 189     _bmp085_coeffs.mc  = -8711;
 190     _bmp085_coeffs.md  = 2868;
 191     _bmp085Mode        = 0;
 192     D2Apply = 27898 ;
 193     D1 = 23843 ;
 194 #endif
 195       int32_t X1 = (D2Apply - (int32_t)_bmp085_coeffs.ac6) * ((int32_t)_bmp085_coeffs.ac5) >> 15;
 196       int32_t X2 = ((int32_t)_bmp085_coeffs.mc << 11) / (X1+(int32_t)_bmp085_coeffs.md);
 197       int32_t b5 = X1 + X2 ;
 198       varioData.temperature = ( b5 + 8 ) >> 4 ; // = Temperature
 199
 200       // calcul of pressure.
 201       int32_t p ;
 202       int32_t b6 = b5 - 4000;
 203       int32_t x1 = (_bmp085_coeffs.b2 * ((b6 * b6) >> 12)) >> 11;
 204       int32_t x2 = (_bmp085_coeffs.ac2 * b6) >> 11;
 205       int32_t x3 = x1 + x2;
 206       int32_t b3 = (((((int32_t) _bmp085_coeffs.ac1) * 4 + x3) << _bmp085Mode) + 2) >> 2;
 207       x1 = (_bmp085_coeffs.ac3 * b6) >> 13;
 208       x2 = (_bmp085_coeffs.b1 * ((b6 * b6) >> 12)) >> 16;
 209       x3 = ((x1 + x2) + 2) >> 2;
 210       uint32_t b4 = (_bmp085_coeffs.ac4 * (uint32_t) (x3 + 32768)) >> 15;
 211       uint32_t b7 = ((uint32_t) (D1 - b3) * (50000 >> _bmp085Mode));
 212       if (b7 < 0x80000000)
 213       {
 214         p = (b7 << 1) / b4;
 215       }
 216       else
 217       {
 218         p = (b7 / b4) << 1;
 219       }
 220       x1 = (p >> 8) * (p >> 8);
 221       x1 = (x1 * 3038) >> 16;
 222       x2 = (-7357 * p) >> 16;
 223       varioData.rawPressure = (p + ((x1 + x2 + 3791) >> 4)) * 10000;
 224
 225 //      varioData.temperature= TEMP;
 226 //      OFF  = (((int64_t)_calibrationData[2]) << 16) + ( (_calibrationData[4] * dT) >> 7);
 227 //      OFF  = (((int64_t)_calibrationData[2]) << 16) + ( ( (_calibrationData[4] - alt_temp_compensation ) * dT) >> 7);
 228 //      SENS = (((int64_t)_calibrationData[1]) << 15) + ((_calibrationData[3] * dT) >> 8);
 229 //      varioData.rawPressure= (((((((int64_t) D1) * (int64_t) SENS) >> 21) - OFF) * 10000 ) >> 15) ; // 1013.25 mb gives 1013250000 is a factor to keep higher precision (=1/100 cm).
 230
 231       // altitude = 44330 * (1.0 - pow(pressure /sealevelPressure,0.1903));
 232       // other alternative (faster) = 1013.25 = 0 m , 954.61 = 500m , etc...
 233       //      Pressure  Alt (m) Ratio
 234       //      101325    0       0.08526603
 235       //      95461     500     0.089525515
 236       //      89876     1000    0.094732853
 237       //      84598     1500    0.098039216
 238       //      79498     2000    0.103906899
 239       //      74686     2500    0.109313511
 240       //      70112     3000    0.115101289
 241       //      65768     3500    0.121270919
 242       //      61645     4000    0.127811861
 243       //      57733     4500    0.134843581
 244       //      54025     5000
 245
 246       if ( varioData.rawPressure > 954610000) {
 247         varioData.rawAltitude = ( 1013250000 - varioData.rawPressure ) * 0.08526603 ; // = 500 / (101325 - 95461)  // returned value 1234567 means 123,4567 m (temp is fixed to 15 degree celcius)
 248       } else if ( varioData.rawPressure > 898760000) {
 249         varioData.rawAltitude = 5000000 + ( 954610000 - varioData.rawPressure ) * 0.089525515  ;
 250       } else if ( varioData.rawPressure > 845980000) {
 251         varioData.rawAltitude = 10000000 + ( 898760000 - varioData.rawPressure ) * 0.094732853  ;
 252       } else if ( varioData.rawPressure > 794980000) {
 253         varioData.rawAltitude = 15000000 + ( 845980000 - varioData.rawPressure ) *  0.098039216 ;
 254       } else if ( varioData.rawPressure > 746860000) {
 255         varioData.rawAltitude = 20000000 + ( 794980000 - varioData.rawPressure ) *  0.103906899 ;
 256       } else if ( varioData.rawPressure > 701120000) {
 257         varioData.rawAltitude = 25000000 + ( 746860000 - varioData.rawPressure ) *  0.109313511 ;
 258       } else if ( varioData.rawPressure > 657680000) {
 259         varioData.rawAltitude = 30000000 + ( 701120000 - varioData.rawPressure ) *  0.115101289 ;
 260       } else if ( varioData.rawPressure > 616450000) {
 261         varioData.rawAltitude = 35000000 + ( 657680000 - varioData.rawPressure ) *  0.121270919 ;
 262       } else if ( varioData.rawPressure > 577330000) {
 263         varioData.rawAltitude = 40000000 + ( 616450000 - varioData.rawPressure ) *  0.127811861 ;
 264       } else {    varioData.rawAltitude = 45000000 + ( 577330000 - varioData.rawPressure ) *  0.134843581 ;
 265       }
 266
 267 // here the classical way to calculate Vspeed with high and low pass filter
 268       if (altitude == 0) {
 269         altitudeLowPass = altitudeHighPass = altitude = varioData.rawAltitude ;
 270 //        pressureMicrosPrev2 = pressureMicrosPrev1 - 20000 ;
 271       }
 272       altitude += 0.04 * (varioData.rawAltitude - altitude) ;
 273 //      varioData.altitudeAt20MsecAvailable = true ; // inform openxsens.ino that calculation of dTE can be performed
 274
 275       altitudeLowPass += 0.085 * ( varioData.rawAltitude - altitudeLowPass) ;
 276       altitudeHighPass += 0.1 * ( varioData.rawAltitude - altitudeHighPass) ;
 277 //      if (pressureMicrosPrev1 > pressureMicrosPrev2 ) varioData.delaySmooth += 0.1 * ( pressureMicrosPrev1 -  pressureMicrosPrev2  - varioData.delaySmooth ) ; //delay between 2 measures  only if there is no overflow of pressureMicos
 278       climbRate2AltFloat = ((altitudeHighPass - altitudeLowPass )  * 5666.685 ) / 20000;
 279
 280       abs_deltaClimbRate =  abs(climbRate2AltFloat - varioData.climbRateFloat) ;
 281       if ( varioData.sensitivityPpm  > 0) sensitivityMin =   varioData.sensitivityPpm ;
 282       if ( (abs_deltaClimbRate <= SENSITIVITY_MIN_AT) || (sensitivityMin >= SENSITIVITY_MAX) ) {
 283          varioData.sensitivity.value = sensitivityMin ;
 284       } else if (abs_deltaClimbRate >= SENSITIVITY_MAX_AT)  {
 285          varioData.sensitivity.value = SENSITIVITY_MAX ;
 286       } else {
 287          varioData.sensitivity.value = sensitivityMin + ( SENSITIVITY_MAX - sensitivityMin ) * (abs_deltaClimbRate - SENSITIVITY_MIN_AT) / (SENSITIVITY_MAX_AT - SENSITIVITY_MIN_AT) ;
 288       }
 289       varioData.climbRateFloat += varioData.sensitivity.value * (climbRate2AltFloat - varioData.climbRateFloat)  * 0.001 ; // sensitivity is an integer and must be divided by 1000
 290
 291       if ( abs((int32_t)  varioData.climbRateFloat - varioData.climbRate.value) > VARIOHYSTERESIS ) {
 292           varioData.climbRate.value = (int32_t)  varioData.climbRateFloat  ;
 293       }
 294       varioData.climbRate.available=true; // allows SPORT protocol to transmit the value
 295 //      varioData.switchClimbRateAvailable = true ; // inform readsensors() that a switchable vspeed is available
 296 //      varioData.averageClimbRateAvailable = true ; // inform readsensors() that a vspeed is available to calculate the average
 297       // AltitudeAvailable is set to true only once every 100 msec in order to give priority to climb rate on SPORT
 298       altMillis = millis() ;
 299       if (altMillis > nextAltMillis){
 300         nextAltMillis = altMillis + 100 ;
 301         varioData.absoluteAlt.value = altitude / 100 ; // altitude is in m *10000 and AbsoluteAlt must be in m * 100
 302         varioData.absoluteAlt.available=true ;  // Altitude is considered as available only after several loop in order to reduce number of transmission on Sport.
 303         varioData.sensitivity.available = true ;
 304         if (varioData.altOffset == 0) varioData.altOffset = varioData.absoluteAlt.value ;
 305         varioData.relativeAlt.value = varioData.absoluteAlt.value - varioData.altOffset ;
 306         varioData.relativeAlt.available = true ;
 307         if ( varioData.relativeAlt.value > varioData.relativeAltMax ) varioData.relativeAltMax = varioData.relativeAlt.value ;
 308         varioData.relativeAltMaxAvailable = true ;
 309 //        if ( altMillis > nextAverageAltMillis ){ // calculation of the difference of altitude (in m) between the 10 last sec
 310 //            nextAverageAltMillis = altMillis + 500 ; // calculate only once every 500 msec
 311 //            varioData.vSpeed10Sec = (varioData.absoluteAlt.value - varioData.prevAlt[varioData.idxPrevAlt]) /100 ;
 312 //            varioData.prevAlt[varioData.idxPrevAlt] = varioData.absoluteAlt.value ;
 313 //            varioData.idxPrevAlt++ ;
 314 //            if ( varioData.idxPrevAlt >= 20 ) varioData.idxPrevAlt = 0 ;
 315 //            if ( altMillis > 15000) {  // make the data avalaible only after 15 sec)
 316 //                varioData.vSpeed10SecAvailable = true ;
 317 //            }
 318 //        }
 319
 320       } // end If (altMillis > nextAltMillis)
 321 #ifdef DEBUGDATA
 322       static bool firstPrintAlt = true ;
 323       if (firstPrintAlt == true) {
 324           firstPrintAlt = false ;
 325 //          printer->println(F( "T,Ra,Sm,A,NC,DS,AHP,ALP,CR2, Temp" )) ;
 326           printer->println(F( "T,Ra,Alt,vpsd, Alt2, rawVspd, vspd2 , smoothAlt, smoothVspd" )) ;
 327       }
 328             printer->print(  pressureMicrosPrev1 ) ; printer->print(",");
 329             printer->print(  (float) varioData.rawAltitude  ) ; printer->print(","); // alt is displayed in CM with 2 decimal
 330  //           printer->print(  expoSmooth ) ;             printer->print(" ,");
 331             printer->print( (float) altitude  ) ;             printer->print(" ,");
 332             printer->print( varioData.climbRate ) ;            printer->print(" ,");
 333  //           printer->print( delaySmooth ) ;            printer->print(" ,");
 334  //           printer->print( altitudeHighPass ) ;             printer->print(" ,");
 335  //           printer->print( altitudeLowPass ) ;            printer->print(" ,");
 336  //           printer->print( climbRate2AltFloat ) ;             printer->print(" ,");
 337  //           printer->print( varioData.temperature ) ;
 338  //           printer->print( smoothAltitude ) ;            printer->print(" ,");
 339  //           printer->print( rawRateVSpeed ) ;            printer->print(" ,");
 340  //           printer->print( smoothRateVSpeed ) ;            printer->print(" ,");
 341  //           printer->print( expoSmooth5611_alt_auto * 1000 ) ;            printer->print(" ,");
 342  //           printer->print( expoSmooth5611_vSpeed_auto * 1000 ) ;            printer->print(" ,");
 343             printer->println( ) ;
 344
 345 #endif
 346
 347       pressureMicrosPrev2 = pressureMicrosPrev1 ;
 348 } // End of calculate Vario
 349
 350 #endif // end of #if defined(SENSOR_IS_BMP180)
 351
 352
 353
 354