openXsensor/oXs_sdp3x.cpp

   1 #include "oXs_sdp3x.h"
   2
   3 #ifdef DEBUG
   4     #define DEBUG_SDP3X_SCAN
   5     #define DEBUG_SDP3X_ERRORCODE
   6     #define DEBUG_SDP3X_RAWDATA
   7     //#define DEBUG_SDP3X_SIMULATION
   8 #endif
   9
  10
  11
  12 extern unsigned long micros( void ) ;
  13 extern unsigned long millis( void ) ;
  14 extern void delay(unsigned long ms) ;
  15
  16 #ifdef DEBUG
  17 OXS_SDP3X::OXS_SDP3X(uint8_t addr, HardwareSerial &print)
  18 #else
  19 OXS_SDP3X::OXS_SDP3X(uint8_t addr)
  20 #endif
  21 {  // constructor
  22   _addr=addr;
  23 #ifdef DEBUG
  24   printer = &print;
  25 #endif
  26 }
  27
  28
  29 // **************** Setup the SDP3X sensor *********************
  30 void OXS_SDP3X::setup() {
  31   airSpeedData.airSpeed.available = false ;
  32 #ifdef DEBUG
  33   printer->print(F("AirSpeed Sensor:SDP3X I2C Addr="));
  34   printer->println(_addr,HEX);
  35 #endif
  36   I2c.begin() ;
  37   I2c.timeOut( 80); //initialise the time out in order to avoid infinite loop
  38 #ifdef DEBUG_SDP3X_SCAN
  39   I2c.scan() ; // scan all I2C address
  40   printer->print(F("I2C scan adr: "));
  41   printer->println( I2c.scanAdr , HEX  );
  42 #endif
  43 #ifdef DEBUG_SDP3X_SIMULATION // use dummy setup in case of simulation
  44   nextPressureReadMillis = millis() + 2;    //
  45   nextAirSpeedMillis  = nextPressureReadMillis + 200 ;
  46   airSpeedData.temperature4525 = 23.4  ;
  47   temperatureKelvinSdp3x = 273.15 + airSpeedData.temperature4525 ; // in Kelvin
  48   dpScaleSdp3x =  966.0 / 1013.0 / 60 ; //60=SDP31 , 240=SDP32
  49 #else // not a simulation
  50   // set the sensor in continous mode with averaging (send a command 0X3615)
  51   I2c.write(_addr, (uint8_t) 0X36 , (uint8_t) 0X15);
  52   delay(20); // wait 20msec in order to get the first data (datasheet says 8 msec)
  53   // read the sensor to get the initial temperature and the scaling factor
  54   I2CErrorCodeSdp3x = I2c.read( _addr,  9 ) ; //read 9 bytes from the device;
  55   nextPressureReadMillis = millis() + 2;    //
  56   nextAirSpeedMillis  = nextPressureReadMillis + 200 ;
  57   if (I2CErrorCodeSdp3x == 0) {
  58       data[0] = I2c.receive() ; // discard diffPressure
  59       data[1] = I2c.receive() ; // discard diffPressure
  60       //int16_t dp_raw   = (int16_t)data[0] << 8 | data[1];
  61       data[0] = I2c.receive() ; // discard this byte = CRC
  62       data[0] = I2c.receive() ;
  63       data[1] = I2c.receive() ;
  64       airSpeedData.temperature4525 = ((int16_t)data[0] << 8 | data[1]) / 200.0  ; // sdp3x use a scale factor of 200 for temp
  65       temperatureKelvinSdp3x = 273.15 + airSpeedData.temperature4525 ; // in Kelvin
  66       data[0] = I2c.receive() ; // discard this byte = CRC
  67       data[0] = I2c.receive() ;
  68       data[1] = I2c.receive() ;
  69       dpScaleSdp3x =  966.0 / 1013.0 / ((int16_t)data[0] << 8 | data[1]);
  70       // datasheet says that we have to apply a correction of 966/actual pressure in mbar; it is estimated with 1013
  71   }
  72 #ifdef DEBUG
  73     printer->print(F("Set up SDP3X done. I2C Error code= "));
  74     printer->println(I2CErrorCodeSdp3x);
  75     printer->print(F(" milli="));
  76     printer->println(millis());
  77 #endif
  78 #endif // endif for simulation
  79 }  //end of setup
  80
  81
  82 /****************************************************************************/
  83 /* readSensor - Read differential pressure + temperature from the 4525DO    */
  84 /****************************************************************************/
  85 void OXS_SDP3X::readSensor() {
  86     unsigned long airSpeedMillis = millis() ;
  87     if ( airSpeedMillis >= nextPressureReadMillis){  // do not read the sensor if there is less than 0.5 msec since previous read
  88         nextPressureReadMillis = airSpeedMillis + 2 ;
  89 #ifdef DEBUG_SDP3X_SIMULATION // use dummy read in case of simulation
  90         if (true){ // added to keep the same if level as when no simulation
  91             airSpeedData.difPressureAdc_zero =  0x7FFF * dpScaleSdp3x     ;  // diffPressure in pa
  92 #else // not in simulation
  93         I2CErrorCodeSdp3x = I2c.read( _addr,  2 ) ; //read 2 bytes from the device;
  94 #ifdef DEBUG_SDP3X_ERRORCODE
  95         if ( I2CErrorCodeSdp3x) {
  96             printer->print(F("Read SDP3X done. I2C Error code= "));
  97             printer->println(I2CErrorCodeSdp3x);
  98         }
  99 #endif
 100         if( I2CErrorCodeSdp3x == 0) { // when no read error, we calculate
 101             data[0] = I2c.receive() ;
 102             data[1] = I2c.receive() ;
 103             airSpeedData.difPressureAdc_zero =  ((int16_t) (data[0] << 8) + data[1] ) * dpScaleSdp3x     ;  // diffPressure in pa
 104 #endif // end of simulation
 105 #define FILTERING_SDP3X_MIN        0.01   //
 106 #define FILTERING_SDP3X_MAX        0.1 //
 107 #define FILTERING_SDP3X_MIN_AT       10 // when abs is less than MIN_AT , apply MIN
 108 #define FILTERING_SDP3X_MAX_AT       100 // when abs is more than MAX_AT , apply MAX (interpolation in between)
 109             float abs_deltaDifPressure =  abs(airSpeedData.difPressureAdc_zero - smoothDifPressure) ;
 110             if (abs_deltaDifPressure <= FILTERING_SDP3X_MIN_AT) {
 111                 expoSmooth_auto = FILTERING_SDP3X_MIN ;
 112             } else if (abs_deltaDifPressure >= FILTERING_SDP3X_MAX_AT)  {
 113                 expoSmooth_auto = FILTERING_SDP3X_MAX ;
 114             } else {
 115                 expoSmooth_auto = FILTERING_SDP3X_MIN + ( FILTERING_SDP3X_MAX - FILTERING_SDP3X_MIN) * (abs_deltaDifPressure - FILTERING_SDP3X_MIN_AT) / (FILTERING_SDP3X_MAX_AT - FILTERING_SDP3X_MIN_AT) ;
 116             }
 117             smoothDifPressure += expoSmooth_auto * ( airSpeedData.difPressureAdc_zero - smoothDifPressure ) ; //
 118
 119                // calculate airspeed based on pressure, altitude and temperature
 120                // airspeed (m/sec) = sqr(2 * differential_pressure_in_Pa / air_mass_kg_per_m3)
 121                // air_mass_kg_per_m3 = pressure_in_pa / (287.05 * (Temp celcius + 273.15))
 122                // so airspeed m/sec =sqr( 2 * 287.05 * differential_pressure_pa * (temperature Celsius + 273.15) / pressure_in_pa )
 123                // so at 15° and 1013hpa in cm/sec = 127.79 (=sqr(2*287.05*288.15/101300))
 124                // or rawAirSpeed cm/sec =  2396 * sqrt( (float) abs(smoothDifPressureAdc) * temperatureKelvin  /  actualPressure) ); // in cm/sec ; actual pressure must be in pa (so 101325 about at sea level)
 125 #ifdef AIRSPEED_AT_SEA_LEVEL_AND_15C
 126             airSpeedData.smoothAirSpeed =  127.79 * sqrt( (float) ( abs(smoothDifPressure) ) ); // indicated airspeed is calculated at 15 Celsius and 101325 pascal
 127 #else
 128             airSpeedData.smoothAirSpeed =  2396.0 * sqrt( (float) ( abs(smoothDifPressure) * temperatureKelvinSdp3x  /  actualPressure) ); // in cm/sec ; actual pressure must be in pa (so 101325 about at sea level)
 129 #endif
 130             if ( smoothDifPressure < 0 ) airSpeedData.smoothAirSpeed = - airSpeedData.smoothAirSpeed ; // apply the sign
 131
 132 #ifdef DEBUG_SDP3X_RAWDATA
 133             static bool firstRawData = true ;
 134             if ( firstRawData ) {
 135                 printer->println(F("at,  difPressure , expoSmooth_auto ,  smoothDifPressure, smoothAirSpeed, ")) ;
 136                 firstRawData = false ;
 137             } else {
 138                 printer->print( airSpeedMillis ); printer->print(F(" , "));
 139                 printer->print(  airSpeedData.difPressureAdc_zero); printer->print(F(" , "));
 140                 printer->print( expoSmooth_auto ); printer->print(F(" , "));
 141                 printer->print( smoothDifPressure ); printer->print(F(" , "));
 142
 143                 printer->print( airSpeedData.smoothAirSpeed * 3.6 / 100); printer->print(F(" , "));
 144                 printer->println(" ") ;
 145             }
 146 #endif
 147         } // end no error on I2C
 148
 149         if (airSpeedMillis > nextAirSpeedMillis) { // publish airspeed only once every xx ms
 150               nextAirSpeedMillis = airSpeedMillis + 200 ;
 151               if ( airSpeedData.smoothAirSpeed >  0) {  // normally send only if positive and greater than 300 cm/sec , otherwise send 0 but for test we keep all values to check for drift
 152 #ifdef AIRSPEED_IN_KMH  // uncomment this line if AIR speed has to be in knot instead of km/h
 153                   airSpeedData.airSpeed.value = airSpeedData.smoothAirSpeed * 0.36 ; // from cm/sec to 1/10 km/h
 154 #else
 155                   airSpeedData.airSpeed.value = airSpeedData.smoothAirSpeed * 0.1943844492 ; // from cm/sec to 1/10 knot/h
 156 #endif
 157               } else {
 158                   airSpeedData.airSpeed.value = 0 ;
 159               }
 160               airSpeedData.airSpeed.available = true ;
 161         }  // end of process every xx millis
 162     } // end of process every 2 millis
 163 } // End of readSensor
 164
 165