openXsensor/oXs_ms5611.cpp

   1 #include "oXs_ms5611.h"
   2
   3 #if defined(SENSOR_IS_MS5611)
   4
   5 #ifdef DEBUG
   6 //#define DEBUGI2CMS5611
   7 //#define DEBUG_VARIO_DATA
   8 //#define DEBUGVARIOI2C
   9 //#define DEBUGVARIO
  10 //#define DEBUG_VARIO_TIME
  11 #endif
  12
  13 //extern unsigned long micros( void ) ;
  14 //extern unsigned long millis( void ) ;
  15 //extern void delay(unsigned long ms) ;
  16
  17 uint16_t i2cPressureError ;
  18 uint16_t i2cTemperatureError ;
  19 uint16_t i2cReadCount ;
  20
  21
  22
  23 //long result ;
  24
  25
  26 #ifdef DEBUG
  27 OXS_MS5611::OXS_MS5611(uint8_t addr, HardwareSerial &print)
  28 #else
  29 OXS_MS5611::OXS_MS5611(uint8_t addr)
  30 #endif
  31 {
  32   // constructor
  33   _addr=addr;
  34   varioData.SensorState = 0 ;
  35 #ifdef DEBUG
  36   printer = &print; //operate on the address of print
  37 //  printer->begin(115200);
  38 //  printer->print("Vario Sensor:MS5611 I2C Addr=");
  39 //  printer->println(addr,HEX);
  40 #endif
  41 }
  42
  43
  44 // **************** Setup the MS5611 sensor *********************
  45 void OXS_MS5611::setup() {
  46   varioData.absoluteAlt.available = false ;
  47   varioData.relativeAlt.available = false ;
  48   varioData.climbRate.available = false ;
  49   varioData.sensitivity.available = false ;
  50   varioData.vSpeed10SecAvailable = false ;
  51 //  varioData.newClimbRateAvailable = false;
  52 //  varioData.sensitivityPpm = 0 ;
  53 //  varioData.idxPrevAlt = 0 ;
  54 //  D1 = 0;
  55 //  D2 = 0;
  56 //  D2Prev = 0;
  57 //  D2Apply = 0 ;
  58 //  dT = 0 ;
  59 //  TEMP = 0 ;
  60 //  rawAltitude = 0 ;
  61
  62 //  altitude = 0 ;
  63 //  altitudeLowPass = 0 ;
  64 //  altitudeHighPass = 0 ;
  65   sensitivityMin = SENSITIVITY_MIN ; // set the min smoothing to the default value
  66   varioData.delaySmooth = 20000 ; // delay between 2 altitude calculation = 20msec = 20000 usec
  67   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)
  68 //  nextAverageAltMillis =  nextAltMillis ;  // in msec ; save when AverageAltitude has to be calculated
  69 //  nextAverageAltMillis =  nextAltMillis ;
  70  //   static long lastResultPressure = 0 ; // used to replace a new pressure by an oldone if collected pressure is wrong (at some time we got wring values)
  71  //   static long lastResultTemp = 0 ; // idem for temperature
  72
  73 //  climbRate2AltFloat = 0 ;
  74 //  climbRateFloat = 0 ;
  75
  76 #ifdef ALT_TEMP_COMPENSATION
  77   alt_temp_compensation = ALT_TEMP_COMPENSATION ;
  78 #endif
  79
  80
  81 #ifdef DEBUGVARIO
  82   printer->print(F("Vario Sensor:MS5611 I2C Addr="));
  83   printer->println(_addr,HEX);
  84   printer->print(F(" milli="));
  85   printer->println(millis());
  86
  87 #endif
  88   I2c.begin() ;
  89   I2c.timeOut(80); //initialise the time out in order to avoid infinite loop
  90 #ifdef DEBUGI2CMS5611
  91   printer->println(F("begin I2C scan"));
  92   I2c.scan() ;
  93   printer->print(F("last I2C scan adr: "));
  94   printer->println( I2c.scanAdr , HEX  );
  95 #endif
  96   errorI2C = I2c.write( _addr,0x1e) ;
  97     errorCalibration = false ;
  98   if (errorI2C > 0 ) {
  99 #ifdef DEBUGVARIO
 100     printer->print(F("error code in setup I2CWrite: "));
 101     printer->println( errorI2C );
 102 #endif
 103     errorCalibration = true ;
 104   } else {
 105     delay(100);
 106     for (byte i = 1; i <=6; i++) {
 107        errorI2C =  I2c.read( _addr, 0xa0 + i*2, 2 ) ; //read 2 bytes from the device after sending the command A0 + xx depending on the register to be read
 108        if ( errorI2C > 0 ) {
 109 #ifdef DEBUGVARIO
 110             printer->print(F("error code in setup I2CRead: "));
 111             printer->println( errorI2C );
 112 #endif
 113             errorCalibration = true ;
 114         } else {
 115             high = I2c.receive() ;
 116             low = I2c.receive() ;
 117             _calibrationData[i] = high<<8 | low;
 118         }
 119 #ifdef DEBUGVARIO
 120         printer->print(F("calibration data #"));
 121         printer->print(i);
 122         printer->print(F(" = "));
 123         printer->print( _calibrationData[i] );
 124         printer->print(F(" error= "));
 125         printer->println( errorI2C );
 126 #endif
 127     } // End for
 128   } // End if else
 129
 130 #ifdef DEBUGVARIO
 131   printer->println(F("setup vario done."));
 132 #endif
 133
 134 //  resetValues(); // not used anymore (min, max, ...)
 135 }  //end of setup
 136
 137
 138 //********************************************************************************************
 139 //***                            read the sensor                                           ***
 140 //********************************************************************************************
 141 #define WAIT_I2C_TIME 9000 // normally we have to wait 9000 usec
 142
 143 bool OXS_MS5611::readSensor() {   // read sensor performs a read from sensor if the delay since previous conversion command is enlapsed, else calculates once if data are available.
 144 #ifdef  DEBUGVARIOI2C
 145     printer->print(F("sensorState= "));
 146     printer->println(varioData.SensorState);
 147 #endif
 148 #ifdef DEBUG_VARIO_TIME
 149   unsigned long varioEnter = micros() ;
 150 #endif
 151     bool newVSpeedCalculated = false ;
 152     if ( ( micros()  - varioData.lastCommandMicros )> WAIT_I2C_TIME )   { // wait 9 msec at least before asking for reading the pressure
 153         long result = 0 ;
 154         if(  ! I2c.read( _addr, 0, 3 )) { ; //read 3 bytes from the device after sending a command "00";
 155           result = I2c.receive() ;
 156           result <<= 8 ;
 157           result |= I2c.receive() ;
 158           result <<= 8 ;
 159           result |= I2c.receive() ;
 160         } // result remain 0 if I2C is wrong
 161         if (varioData.SensorState==0) { // ========================= We got the temperature
 162           D2=result;
 163           varioData.SensorState++ ;
 164           I2c.write( _addr,0x48) ;                    // ask a conversion of Pressure
 165         } else {                      // ========================= We got the Pressure
 166           D1=result;
 167           pressureMicros = micros();                  // pressureMicros is the timestamp to calculate climbrate between 2 pressuresD2=result;
 168           varioData.SensorState = 2 ;
 169           I2c.write( _addr,0x58) ;                   // ask a conversion of Temperature
 170         }
 171         varioData.lastCommandMicros = micros()  ;    // save time of last command
 172     } else {
 173       if  ( varioData.SensorState == 2 ) {                   // 2 means that previous call to read sensor got a Pressure
 174           varioData.SensorState = 0 ;                         // reset state to 0 to allow reading the temperature
 175           if ( (D1 > 0) && (D2 > 0) &&( millis() > 1000) ) {   // we have all data and we can calculate
 176               calculateVario() ;
 177               newVSpeedCalculated = true ;
 178           }
 179       } // end ( varioData.SensorState == 2 )
 180     }
 181 #ifdef DEBUG_VARIO_TIME
 182   varioEnter = micros() - varioEnter ;
 183   if ( varioEnter > 10 ) {
 184     Serial.print("Vario ") ; Serial.println(varioEnter) ;                          // when I2C works at 400 khz, it takes about 520 usec to read the temperature or the pressure and 450 usec to perform the calculation
 185   }
 186 #endif
 187   return newVSpeedCalculated ;                                   // return true if a new Vspeed is available
 188 }  // end read sensor
 189
 190 void OXS_MS5611::calculateVario() {
 191         if (D2Prev == 0) D2Prev = D2 ;
 192         D2Apply = (D2 + D2Prev ) >> 1 ;
 193         D2Prev = D2 ;
 194         dT = D2Apply - ((long)_calibrationData[5] << 8);
 195 //      TEMP = (2000 + (((int64_t)dT * (int64_t)_calibrationData[6]) >> 23)) / (float) 1.0 ;
 196         varioData.temperature = (2000 + (((int64_t)dT * (int64_t)_calibrationData[6]) >> 23)) ;
 197 //      varioData.temperature= TEMP;
 198 //      OFF  = (((int64_t)_calibrationData[2]) << 16) + ( (_calibrationData[4] * dT) >> 7);
 199         OFF  = (((int64_t)_calibrationData[2]) << 16) + ( ( (_calibrationData[4] - alt_temp_compensation ) * dT) >> 7);
 200         SENS = (((int64_t)_calibrationData[1]) << 15) + ((_calibrationData[3] * dT) >> 8);
 201         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).
 202
 203       // altitude = 44330 * (1.0 - pow(pressure /sealevelPressure,0.1903));
 204       // other alternative (faster) = 1013.25 = 0 m , 954.61 = 500m , etc...
 205       //      Pressure  Alt (m) Ratio
 206       //      101325  0 0.08526603
 207       //      95461 500 0.089525515
 208       //      89876 1000  0.094732853
 209       //      84598 1500  0.098039216
 210       //      79498 2000  0.103906899
 211       //      74686 2500  0.109313511
 212       //      70112 3000  0.115101289
 213       //      65768 3500  0.121270919
 214       //      61645 4000  0.127811861
 215       //      57733 4500  0.134843581
 216       //      54025 5000
 217       if ( varioData.rawPressure > 954610000) {
 218         varioData.rawAltitude = ( 1013250000 - varioData.rawPressure ) * 0.08526603 ; // = 500 / (101325 - 95461)  // returned value 1234567 means 123,4567 m (temp is fixed to 15 degree celcius)
 219       } else if ( varioData.rawPressure > 898760000) {
 220         varioData.rawAltitude = 5000000 + ( 954610000 - varioData.rawPressure ) * 0.089525515  ;
 221       } else if ( varioData.rawPressure > 845980000) {
 222         varioData.rawAltitude = 10000000 + ( 898760000 - varioData.rawPressure ) * 0.094732853  ;
 223       } else if ( varioData.rawPressure > 794980000) {
 224         varioData.rawAltitude = 15000000 + ( 845980000 - varioData.rawPressure ) *  0.098039216 ;
 225       } else if ( varioData.rawPressure > 746860000) {
 226         varioData.rawAltitude = 20000000 + ( 794980000 - varioData.rawPressure ) *  0.103906899 ;
 227       } else if ( varioData.rawPressure > 701120000) {
 228         varioData.rawAltitude = 25000000 + ( 746860000 - varioData.rawPressure ) *  0.109313511 ;
 229       } else if ( varioData.rawPressure > 657680000) {
 230         varioData.rawAltitude = 30000000 + ( 701120000 - varioData.rawPressure ) *  0.115101289 ;
 231       } else if ( varioData.rawPressure > 616450000) {
 232         varioData.rawAltitude = 35000000 + ( 657680000 - varioData.rawPressure ) *  0.121270919 ;
 233       } else if ( varioData.rawPressure > 577330000) {
 234         varioData.rawAltitude = 40000000 + ( 616450000 - varioData.rawPressure ) *  0.127811861 ;
 235       } else {    varioData.rawAltitude = 45000000 + ( 577330000 - varioData.rawPressure ) *  0.134843581 ;
 236       }
 237
 238 // here the classical way to calculate Vspeed with high and low pass filter
 239       if (altitude == 0) {
 240         altitudeLowPass = altitudeHighPass = altitude = varioData.rawAltitude ;
 241       }
 242       altitude += 0.04 * (varioData.rawAltitude - altitude) ;
 243 //      varioData.altitudeAt20MsecAvailable = true ; // inform openxsens.ino that calculation of dTE can be performed
 244
 245       altitudeLowPass += 0.085 * ( varioData.rawAltitude - altitudeLowPass) ;
 246       altitudeHighPass += 0.1  * ( varioData.rawAltitude - altitudeHighPass) ;
 247       climbRate2AltFloat = ((altitudeHighPass - altitudeLowPass )  * 5666.685 ) / 20000 ;
 248
 249       abs_deltaClimbRate =  abs(climbRate2AltFloat - varioData.climbRateFloat) ;
 250       if ( varioData.sensitivityPpm  > 0) sensitivityMin =   varioData.sensitivityPpm ;
 251       if ( (abs_deltaClimbRate <= SENSITIVITY_MIN_AT) || (sensitivityMin >= SENSITIVITY_MAX) ) {
 252          varioData.sensitivity.value = sensitivityMin ;
 253       } else if (abs_deltaClimbRate >= SENSITIVITY_MAX_AT)  {
 254          varioData.sensitivity.value = SENSITIVITY_MAX ;
 255       } else {
 256          varioData.sensitivity.value = sensitivityMin + ( SENSITIVITY_MAX - sensitivityMin ) * (abs_deltaClimbRate - SENSITIVITY_MIN_AT) / (SENSITIVITY_MAX_AT - SENSITIVITY_MIN_AT) ;
 257       }
 258       varioData.climbRateFloat += varioData.sensitivity.value * (climbRate2AltFloat - varioData.climbRateFloat)  * 0.001 ; // sensitivity is an integer and must be divided by 1000
 259
 260       if ( abs((int32_t)  varioData.climbRateFloat - varioData.climbRate.value) > VARIOHYSTERESIS ) {
 261           //varioData.climbRate = (int32_t)  varioData.climbRateFloat  ;
 262           varioData.climbRate.value = (int32_t)  varioData.climbRateFloat  ;
 263       }
 264       //varioData.climbRateAvailable=true; // allows SPORT protocol to transmit the value
 265       varioData.climbRate.available=true; // allows SPORT protocol to transmit the value
 266 //      varioData.switchClimbRateAvailable = true ; // inform readsensors() that a switchable vspeed is available
 267 //      varioData.averageClimbRateAvailable = true ; // inform readsensors() that a vspeed is available to calculate the average
 268       // AltitudeAvailable is set to true only once every 100 msec in order to give priority to climb rate on SPORT
 269       altMillis = millis() ;
 270       if (altMillis > nextAltMillis){
 271         nextAltMillis = altMillis + 100 ;
 272         varioData.absoluteAlt.value = altitude / 100 ; // altitude is in m *10000 and AbsoluteAlt must be in m * 100 (= cm)
 273         varioData.absoluteAlt.available=true ;  // Altitude is considered as available only after several loop in order to reduce number of transmission on Sport.
 274         varioData.sensitivity.available = true ;
 275         if (varioData.altOffset == 0) varioData.altOffset = varioData.absoluteAlt.value ;
 276         varioData.relativeAlt.value = varioData.absoluteAlt.value - varioData.altOffset ;
 277         varioData.relativeAlt.available = true ;
 278         if ( varioData.relativeAlt.value > varioData.relativeAltMax ) varioData.relativeAltMax = varioData.relativeAlt.value ;
 279         varioData.relativeAltMaxAvailable = true ;
 280
 281 //        if ( altMillis > nextAverageAltMillis ){ // calculation of the difference of altitude (in m) between the 10 last sec
 282 //            nextAverageAltMillis = altMillis + 500 ; // calculate only once every 500 msec
 283 //            varioData.vSpeed10Sec = (varioData.absoluteAlt - varioData.prevAlt[varioData.idxPrevAlt]) /100 ;
 284 //            varioData.prevAlt[varioData.idxPrevAlt] = varioData.absoluteAlt ;
 285 //            varioData.idxPrevAlt++ ;
 286 //            if ( varioData.idxPrevAlt >= 20 ) varioData.idxPrevAlt = 0 ;
 287 //            if ( altMillis > 15000) {  // make the data avalaible only after 15 sec)
 288 //                varioData.vSpeed10SecAvailable = true ;
 289 //            }
 290 //        }
 291
 292       } // end If (altMillis > nextAltMillis)
 293 #ifdef DEBUG_VARIO_DATA
 294       static bool firstPrintAlt = true ;
 295       if (firstPrintAlt == true) {
 296           firstPrintAlt = false ;
 297 //          printer->println(F( "T,Ra,Sm,A,NC,DS,AHP,ALP,CR2, Temp" )) ;
 298           Serial.println(F( "T,Ra,Alt,vpsd, Alt2, rawVspd, vspd2 , smoothAlt, smoothVspd" )) ;
 299       }
 300             Serial.print( varioData.temperature ) ; printer->print(",");
 301             Serial.print(  (float) varioData.rawAltitude  ) ; Serial.print(","); // alt is displayed in CM with 2 decimal
 302  //           printer->print(  expoSmooth ) ;             printer->print(" ,");
 303             Serial.print( (float) altitude  ) ;             Serial.print(" ,");
 304             Serial.print( varioData.climbRate.value ) ;            Serial.print(" ,");
 305  //           printer->print( delaySmooth ) ;            printer->print(" ,");
 306  //           printer->print( altitudeHighPass ) ;             printer->print(" ,");
 307  //           printer->print( altitudeLowPass ) ;            printer->print(" ,");
 308  //           printer->print( climbRate2AltFloat ) ;             printer->print(" ,");
 309  //           printer->print( varioData.temperature ) ;
 310  //           printer->print( smoothAltitude ) ;            printer->print(" ,");
 311  //           printer->print( rawRateVSpeed ) ;            printer->print(" ,");
 312  //           printer->print( smoothRateVSpeed ) ;            printer->print(" ,");
 313  //           printer->print( expoSmooth5611_alt_auto * 1000 ) ;            printer->print(" ,");
 314  //           printer->print( expoSmooth5611_vSpeed_auto * 1000 ) ;            printer->print(" ,");
 315             Serial.println( ) ;
 316
 317 #endif
 318
 319
 320       pressureMicrosPrev2 = pressureMicrosPrev1 ;
 321
 322 } // End of calculateVario
 323
 324 #endif // en of #if defined(SENSOR_IS_MS5611)
 325
 326 /*
 327 void OXS_MS5611::readSensor() {
 328    long result = 0;
 329 #ifdef  DEBUGVARIOI2C
 330     printer->print(F("sensorState= "));
 331     printer->println(varioData.SensorState);
 332 #endif
 333   if (varioData.SensorState==1) { // ========================= Read the pressure
 334     extended2Micros = micros() >> 1 ;
 335     if (extended2Micros < varioData.lastCommand2Micros) extended2Micros = extended2Micros | 0x80000000 ;
 336     if ( extended2Micros  > (varioData.lastCommand2Micros + ( WAIT_I2C_TIME / 2 ) ) ) { // wait 9 msec at least before asking for reading the pressure
 337 //        long result = 0;
 338               if(  ! I2c.read( _addr, 0, 3 )) { ; //read 3 bytes from the device after sending a command "00"; keep previous value in case of error
 339                 result = I2c.receive() ;
 340                 result <<= 8 ;
 341                 result |= I2c.receive() ;
 342                 result <<= 8 ;
 343                 result |= I2c.receive() ;
 344                 D1=result;
 345         } else {
 346               D1 = 0 ; // D1 value are not processed to calculate Alt.
 347               i2cPressureError++ ; // for debugging, count the number of read errors on pressure
 348         }
 349         i2cReadCount++ ;  // for debugging I2C count the number of Pressure and Temperature reads
 350         I2c.write( _addr,0x58) ; // ask a conversion of Temperature
 351         varioData.lastCommand2Micros = (micros() >>1 );
 352         varioData.SensorState = 2;
 353     } // end of delay of 9 ms
 354   } // end of SensorState == 1
 355   else if (varioData.SensorState==2){ // =========================
 356     extended2Micros = micros() >> 1 ;
 357     if (extended2Micros < varioData.lastCommand2Micros) extended2Micros = extended2Micros | 0x80000000 ;
 358     if ( extended2Micros > ( varioData.lastCommand2Micros + (WAIT_I2C_TIME / 2) )) { // wait 9000 usec to get Temp with high precision
 359           if ( ! I2c.read( _addr, 0, 3 )) { ; //read 3 bytes from the device; keep previous value in case of error
 360                 result = I2c.receive() ;
 361                 result <<= 8 ;
 362                 result |= I2c.receive() ;
 363                 result <<= 8 ;
 364                 result |= I2c.receive() ;
 365                 D2=result;
 366           } else {
 367           i2cTemperatureError++ ; // for debugging, count the number of read errors on temperature
 368           }
 369           varioData.SensorState=0; //
 370     }  // End of process if temperature can be read
 371
 372   } // End of process if SensorState was 1 or 2
 373   if (varioData.SensorState==0) {    // ========================== new Pressure and (new or old) Temp are known so Request Pressure immediately and calculate altitude
 374     I2c.write( _addr,0x48) ;// ask a conversion of Pressure
 375     pressureMicrosPrev1 = pressureMicros ;
 376     pressureMicros = micros(); // pressureMicros is the timestamp to calculate climbrate between 2 pressures
 377     varioData.lastCommand2Micros = pressureMicros >> 1 ;
 378     varioData.SensorState=1;
 379     if ((D1 > 0) & (millis() > 1000) ) { // If D1 has been read in a previous loop and if sensor is started since more than 1 sec, then calculate pressure etc...
 380       if (D2Prev == 0) D2Prev = D2 ;
 381       D2Apply = (D2 + D2Prev ) / 2 ;
 382       D2Prev = D2 ;
 383       dT = D2Apply - ((long)_calibrationData[5] << 8);
 384 //      TEMP = (2000 + (((int64_t)dT * (int64_t)_calibrationData[6]) >> 23)) / (float) 1.0 ;
 385       varioData.temperature = (2000 + (((int64_t)dT * (int64_t)_calibrationData[6]) >> 23)) ;
 386 //      varioData.temperature= TEMP;
 387 //      OFF  = (((int64_t)_calibrationData[2]) << 16) + ( (_calibrationData[4] * dT) >> 7);
 388       OFF  = (((int64_t)_calibrationData[2]) << 16) + ( ( (_calibrationData[4] - alt_temp_compensation ) * dT) >> 7);
 389       SENS = (((int64_t)_calibrationData[1]) << 15) + ((_calibrationData[3] * dT) >> 8);
 390       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).
 391
 392       // altitude = 44330 * (1.0 - pow(pressure /sealevelPressure,0.1903));
 393       // other alternative (faster) = 1013.25 = 0 m , 954.61 = 500m , etc...
 394       //      Pressure  Alt (m) Ratio
 395       //      101325    0       0.08526603
 396       //      95461     500     0.089525515
 397       //      89876     1000    0.094732853
 398       //      84598     1500    0.098039216
 399       //      79498     2000    0.103906899
 400       //      74686     2500    0.109313511
 401       //      70112     3000    0.115101289
 402       //      65768     3500    0.121270919
 403       //      61645     4000    0.127811861
 404       //      57733     4500    0.134843581
 405       //      54025     5000
 406
 407       if ( varioData.rawPressure > 954610000) {
 408         varioData.rawAltitude = ( 1013250000 - varioData.rawPressure ) * 0.08526603 ; // = 500 / (101325 - 95461)  // returned value 1234567 means 123,4567 m (temp is fixed to 15 degree celcius)
 409       } else if ( varioData.rawPressure > 898760000) {
 410         varioData.rawAltitude = 5000000 + ( 954610000 - varioData.rawPressure ) * 0.089525515  ;
 411       } else if ( varioData.rawPressure > 845980000) {
 412         varioData.rawAltitude = 10000000 + ( 898760000 - varioData.rawPressure ) * 0.094732853  ;
 413       } else if ( varioData.rawPressure > 794980000) {
 414         varioData.rawAltitude = 15000000 + ( 845980000 - varioData.rawPressure ) *  0.098039216 ;
 415       } else if ( varioData.rawPressure > 746860000) {
 416         varioData.rawAltitude = 20000000 + ( 794980000 - varioData.rawPressure ) *  0.103906899 ;
 417       } else if ( varioData.rawPressure > 701120000) {
 418         varioData.rawAltitude = 25000000 + ( 746860000 - varioData.rawPressure ) *  0.109313511 ;
 419       } else if ( varioData.rawPressure > 657680000) {
 420         varioData.rawAltitude = 30000000 + ( 701120000 - varioData.rawPressure ) *  0.115101289 ;
 421       } else if ( varioData.rawPressure > 616450000) {
 422         varioData.rawAltitude = 35000000 + ( 657680000 - varioData.rawPressure ) *  0.121270919 ;
 423       } else if ( varioData.rawPressure > 577330000) {
 424         varioData.rawAltitude = 40000000 + ( 616450000 - varioData.rawPressure ) *  0.127811861 ;
 425       } else {    varioData.rawAltitude = 45000000 + ( 577330000 - varioData.rawPressure ) *  0.134843581 ;
 426       }
 427
 428 // here the classical way to calculate Vspeed with high and low pass filter
 429       if (altitude == 0) {
 430         altitudeLowPass = altitudeHighPass = altitude = varioData.rawAltitude ;
 431         pressureMicrosPrev2 = pressureMicrosPrev1 - 20000 ;
 432       }
 433       altitude += 0.04 * (varioData.rawAltitude - altitude) ;
 434       varioData.altitudeAt20MsecAvailable = true ; // inform openxsens.ino that calculation of dTE can be performed
 435
 436       altitudeLowPass += 0.085 * ( varioData.rawAltitude - altitudeLowPass) ;
 437       altitudeHighPass += 0.1 * ( varioData.rawAltitude - altitudeHighPass) ;
 438       if (pressureMicrosPrev1 > pressureMicrosPrev2 ) varioData.delaySmooth += 0.1 * ( pressureMicrosPrev1 -  pressureMicrosPrev2  - varioData.delaySmooth ) ; //delay between 2 measures  only if there is no overflow of pressureMicos
 439       climbRate2AltFloat = ((altitudeHighPass - altitudeLowPass )  * 5666.685 ) / varioData.delaySmooth;
 440
 441       abs_deltaClimbRate =  abs(climbRate2AltFloat - varioData.climbRateFloat) ;
 442       if ( varioData.sensitivityPpm  > 0) sensitivityMin =   varioData.sensitivityPpm ;
 443       if ( (abs_deltaClimbRate <= SENSITIVITY_MIN_AT) || (sensitivityMin >= SENSITIVITY_MAX) ) {
 444          varioData.sensitivity = sensitivityMin ;
 445       } else if (abs_deltaClimbRate >= SENSITIVITY_MAX_AT)  {
 446          varioData.sensitivity = SENSITIVITY_MAX ;
 447       } else {
 448          varioData.sensitivity = sensitivityMin + ( SENSITIVITY_MAX - sensitivityMin ) * (abs_deltaClimbRate - SENSITIVITY_MIN_AT) / (SENSITIVITY_MAX_AT - SENSITIVITY_MIN_AT) ;
 449       }
 450       varioData.climbRateFloat += varioData.sensitivity * (climbRate2AltFloat - varioData.climbRateFloat)  * 0.001 ; // sensitivity is an integer and must be divided by 1000
 451
 452       if ( abs((int32_t)  varioData.climbRateFloat - varioData.climbRate) > VARIOHYSTERESIS ) {
 453           varioData.climbRate = (int32_t)  varioData.climbRateFloat  ;
 454       }
 455       varioData.climbRateAvailable=true; // allows SPORT protocol to transmit the value
 456       varioData.switchClimbRateAvailable = true ; // inform readsensors() that a switchable vspeed is available
 457       varioData.averageClimbRateAvailable = true ; // inform readsensors() that a vspeed is available to calculate the average
 458       // AltitudeAvailable is set to true only once every 100 msec in order to give priority to climb rate on SPORT
 459       altMillis = millis() ;
 460       if (altMillis > nextAltMillis){
 461         nextAltMillis = altMillis + 100 ;
 462         varioData.absoluteAlt = altitude / 100 ; // altitude is in m *10000 and AbsoluteAlt must be in m * 100
 463         varioData.absoluteAltAvailable=true ;  // Altitude is considered as available only after several loop in order to reduce number of transmission on Sport.
 464         varioData.sensitivityAvailable = true ;
 465         if (varioData.altOffset == 0) varioData.altOffset = varioData.absoluteAlt ;
 466         varioData.relativeAlt = varioData.absoluteAlt - varioData.altOffset ;
 467         varioData.relativeAltAvailable = true ;
 468         if ( varioData.relativeAlt > varioData.relativeAltMax ) varioData.relativeAltMax = varioData.relativeAlt ;
 469         varioData.relativeAltMaxAvailable = true ;
 470
 471 //        if ( altMillis > nextAverageAltMillis ){ // calculation of the difference of altitude (in m) between the 10 last sec
 472 //            nextAverageAltMillis = altMillis + 500 ; // calculate only once every 500 msec
 473 //            varioData.vSpeed10Sec = (varioData.absoluteAlt - varioData.prevAlt[varioData.idxPrevAlt]) /100 ;
 474 //            varioData.prevAlt[varioData.idxPrevAlt] = varioData.absoluteAlt ;
 475 //            varioData.idxPrevAlt++ ;
 476 //            if ( varioData.idxPrevAlt >= 20 ) varioData.idxPrevAlt = 0 ;
 477 //            if ( altMillis > 15000) {  // make the data avalaible only after 15 sec)
 478 //                varioData.vSpeed10SecAvailable = true ;
 479 //            }
 480 //        }
 481
 482       } // end If (altMillis > nextAltMillis)
 483 #ifdef DEBUGDATA
 484       static bool firstPrintAlt = true ;
 485       if (firstPrintAlt == true) {
 486           firstPrintAlt = false ;
 487 //          printer->println(F( "T,Ra,Sm,A,NC,DS,AHP,ALP,CR2, Temp" )) ;
 488           printer->println(F( "T,Ra,Alt,vpsd, Alt2, rawVspd, vspd2 , smoothAlt, smoothVspd" )) ;
 489       }
 490             printer->print(  pressureMicrosPrev1 ) ; printer->print(",");
 491             printer->print(  (float) varioData.rawAltitude  ) ; printer->print(","); // alt is displayed in CM with 2 decimal
 492  //           printer->print(  expoSmooth ) ;             printer->print(" ,");
 493             printer->print( (float) altitude  ) ;             printer->print(" ,");
 494             printer->print( varioData.climbRate ) ;            printer->print(" ,");
 495  //           printer->print( delaySmooth ) ;            printer->print(" ,");
 496  //           printer->print( altitudeHighPass ) ;             printer->print(" ,");
 497  //           printer->print( altitudeLowPass ) ;            printer->print(" ,");
 498  //           printer->print( climbRate2AltFloat ) ;             printer->print(" ,");
 499  //           printer->print( varioData.temperature ) ;
 500  //           printer->print( smoothAltitude ) ;            printer->print(" ,");
 501  //           printer->print( rawRateVSpeed ) ;            printer->print(" ,");
 502  //           printer->print( smoothRateVSpeed ) ;            printer->print(" ,");
 503  //           printer->print( expoSmooth5611_alt_auto * 1000 ) ;            printer->print(" ,");
 504  //           printer->print( expoSmooth5611_vSpeed_auto * 1000 ) ;            printer->print(" ,");
 505             printer->println( ) ;
 506
 507 #endif
 508
 509
 510       pressureMicrosPrev2 = pressureMicrosPrev1 ;
 511       }  // end of D1 > 0 & time > 3 sec
 512   } // End of process if SensorState was 0
 513 } // End of readSensor
 514
 515 */
 516
 517
 518
 519