openXsensor/oXs_4525.cpp

   1 #include "oXs_4525.h"
   2
   3 #ifdef DEBUG
   4 //#define DEBUG4525ERRORCODE
   5 //#define DEBUG4525HEX
   6 //#define DEBUG4525RAWDATA
   7 //#define DEBUG4525LASTDATA
   8 //#define DEBUG4525READDELAY
   9 //#define DEBUG4525READINOUTDELAY
  10 //#define DEBUGMAXPRESSURE
  11 #endif
  12
  13
  14
  15 extern unsigned long micros( void ) ;
  16 extern unsigned long millis( void ) ;
  17 extern void delay(unsigned long ms) ;
  18
  19 #ifdef DEBUG
  20 OXS_4525::OXS_4525(uint8_t addr, HardwareSerial &print)
  21 #else
  22 OXS_4525::OXS_4525(uint8_t addr)
  23 #endif
  24 {  // constructor
  25   _addr=addr;
  26 #ifdef DEBUG
  27   printer = &print;
  28 #endif
  29 }
  30
  31
  32 // **************** Setup the 4525DO sensor *********************
  33 void OXS_4525::setup() {
  34   //airSpeedData.available=false;
  35   calibrated4525 = false ;
  36 //  calibrateCount4525 = 0 ;
  37   airSpeedData.airSpeed.available = false ;
  38 //  airSpeedData.compensationAvailable =false ;
  39
  40   airSpeedData.airspeedReset = true ; // set on true to force a reset the first time the 100 ms part is entered
  41 //  airSpeedData.sensitivity4525Ppm = 0 ;
  42 //  smoothDifPressureAdc = 0 ;
  43 //  offset4525 = 0 ;
  44 //  difPressureSum = 0 ;
  45 //  temperature4525 =0 ;
  46 //  rawAirSpeed = 0 ;
  47 //  smoothAirSpeed = 0 ;
  48   nextAirSpeedMillis  =  3200 ;  // save when AirSpeed has to be calculated; Airspeed is available only after 3200 in order to get a stable value (less temperature drift)
  49 #ifdef DEBUG
  50   printer->print(F("AirSpeed Sensor:4525 I2C Addr="));
  51   printer->println(_addr,HEX);
  52 #endif
  53   I2c.begin() ;
  54   I2c.timeOut( 80); //initialise the time out in order to avoid infinite loop
  55 #ifdef DEBUG4525SETUP
  56   I2c.scan() ; // scan all I2C address
  57   printer->print(F("I2C scan adr: "));
  58   printer->println( I2c.scanAdr , HEX  );
  59 #endif
  60 // read the sensor to get the initial temperature
  61   I2CErrorCode4525 = I2c.read( _addr,  4 ) ; //read 4 bytes from the device;
  62   data[0] = I2c.receive() ;
  63   data[1] = I2c.receive() ;
  64   data[2] = I2c.receive() ;
  65   data[3] = I2c.receive() ;
  66   if ( ( data[0] & 0xC0 ) == 0) {
  67          temperature4525Adc =    (data[2] << 8) + data[3] ;
  68          temperature4525Adc = (0xFFE0 & temperature4525Adc) >> 5;
  69          airSpeedData.temperature4525 = (0.097703957f * temperature4525Adc)  + 223 ; // in kelvin
  70   } else {
  71           airSpeedData.temperature4525 = 300 ;
  72   }
  73 #ifdef DEBUG
  74   printer->print(F("Set up 4525 done. I2C Error code= "));
  75   printer->println(I2CErrorCode4525);
  76   printer->print(F(" milli="));
  77   printer->println(millis());
  78 #endif
  79 }  //end of setup
  80
  81
  82 /****************************************************************************/
  83 /* readSensor - Read differential pressure + temperature from the 4525DO    */
  84 /****************************************************************************/
  85 void OXS_4525::readSensor() {
  86          static int32_t difPressureAdc4Values[4] = { 0, 0, 0, 0 };
  87
  88          static int32_t difPressureAdcSum4Values;
  89          static int8_t countAverage ;
  90
  91 #ifdef DEBUG4525READINOUTDELAY
  92          static unsigned long airSpeedMicrosDebug ;
  93          static unsigned long airSpeedMicrosDebugMid1 ;
  94          static unsigned long airSpeedMicrosDebugMid2 ;
  95          static unsigned long airSpeedMicrosDebugMid3 ;
  96
  97          airSpeedMicrosDebug = micros() ;
  98 #endif
  99 //        I2CErrorCode4525 = I2c.read( _addr,  4 ) ; //read 4 bytes from the device;
 100         I2CErrorCode4525 = I2c.read( _addr,  2 ) ; //read 2 bytes from the device;
 101 #ifdef DEBUG4525ERRORCODE
 102         printer->print(F("Read 4525 done. I2C Error code= "));
 103         printer->println(I2CErrorCode4525);
 104 #endif
 105         if( I2CErrorCode4525 == 0) {
 106 #ifdef DEBUG4525READDELAY
 107             static unsigned long airSpeedMicrosDebugPrev = airSpeedMicros ;
 108             printer->print(F("Delay 4525  between 2 read= "));
 109             printer->println(airSpeedMicros - airSpeedMicrosDebugPrev);
 110             airSpeedMicrosDebugPrev = airSpeedMicros ;
 111 #endif
 112             data[0] = I2c.receive() ;
 113             data[1] = I2c.receive() ;
 114 #ifdef DEBUG4525HEX
 115             printer->print(F("Data from 4525= "));  printer->print(data[0], HEX);  printer->print(F(" "));  printer->print(data[1], HEX);
 116             printer->print(F(" "));  printer->print(data[2], HEX);   printer->print(F(" "));  printer->println(data[3], HEX);
 117 #endif
 118 #ifdef DEBUG4525READINOUTDELAY
 119          airSpeedMicrosDebugMid1 = micros() ;
 120 #endif
 121
 122            if ( ( data[0] & 0xC0 ) == 0) {
 123                difPressureAdc =  ( ( (data[0] << 8) + data[1] ) & 0x3FFF) - 0x2000   ; // substract in order to have a zero value
 124 //               difPressureAdc = 14745 - 8192 ; // test should give 1 psi = 6894 pa = 105 m/sec = 370 km/h
 125                //difPressureAdc = 1638 - 8192 ; // test should give -1 psi = 6894 pa
 126                if ( calibrated4525 == false) {
 127                    calibrateCount4525++ ;
 128                    if (calibrateCount4525 == 256 ) { // after 256 reading , we can calculate the offset
 129                      offset4525 =  (  (float) difPressureSum / 128.0 ) ; //there has been 128 reading (256-128)
 130                      calibrated4525 = true ;
 131                    } else if  (calibrateCount4525 >= 128  ){ // after 128 reading, we can start cummulate the ADC values in order to calculate the offset
 132                       difPressureSum += difPressureAdc ;
 133                    } // end calibration
 134               }  else { // sensor is calibrated
 135                     difPressureAdc_0 = difPressureAdc - offset4525 ;
 136  // calculate a moving average on 4 values ( used only for vspeed compensation )
 137                     difPressureAdcSum4Values += difPressureAdc - difPressureAdc4Values[countAverage] ;
 138                     difPressureAdc4Values[countAverage] = difPressureAdc ;
 139                     if( (++countAverage) >= 4 ) countAverage = 0 ;
 140                     airSpeedData.difPressureAdc_zero = (float) difPressureAdcSum4Values * 0.25 - offset4525 ;
 141
 142
 143 #define FILTERING4525_ADC_MIN        0.001   //
 144 #define FILTERING4525_ADC_MAX        0.01 //
 145 #define FILTERING4525_ADC_MIN_AT       10 // when abs(delta between ADC and current value) is less than MIN_AT , apply MIN
 146 #define FILTERING4525_ADC_MAX_AT       100 // when abs(delta between ADC and current value) is more than MAX_AT , apply MAX (interpolation in between)
 147                     abs_deltaDifPressureAdc =  abs(difPressureAdc_0 - smoothDifPressureAdc) ;
 148                     if (abs_deltaDifPressureAdc <= FILTERING4525_ADC_MIN_AT) {
 149                        expoSmooth4525_adc_auto = FILTERING4525_ADC_MIN ;
 150                     } else if (abs_deltaDifPressureAdc >= FILTERING4525_ADC_MAX_AT)  {
 151                        expoSmooth4525_adc_auto = FILTERING4525_ADC_MAX ;
 152                     } else {
 153                        expoSmooth4525_adc_auto = FILTERING4525_ADC_MIN + ( FILTERING4525_ADC_MAX - FILTERING4525_ADC_MIN) * (abs_deltaDifPressureAdc - FILTERING4525_ADC_MIN_AT) / (FILTERING4525_ADC_MAX_AT - FILTERING4525_ADC_MIN_AT) ;
 154                     }
 155                     smoothDifPressureAdc += expoSmooth4525_adc_auto * ( difPressureAdc_0 - smoothDifPressureAdc ) ; //
 156               }
 157                // calculate airspeed based on pressure, altitude and temperature
 158                // airspeed (m/sec) = sqr(2 * differential_pressure_in_Pa / air_mass_kg_per_m3)
 159                // air_mass_kg_per_m3 = pressure_in_pa / (287.05 * (Temp celcius + 273.15))
 160                // and differantial_pressure_Pa =  ((smoothDifPressureAdc  ) * 1.052) ;  // with MS4525DO_001 a range of 2 PSI gives 80% of 16383 (= max of 14bits); 1 PSI = 6894,76 Pascal ; so 1 unit of ADC = 2/ (80% * 16383) * 6894,76)
 161                // so airspeed m/sec =sqr( 2 * 287.05 * 1.052 * differential_pressure_pa * (temperature Celsius + 273.15) / pressure_in_pa )
 162                // rawAirSpeed cm/sec =  24,58 * 100 * sqrt( (float) abs(smoothDifPressureAdc) * temperature4525  /  actualPressure) ); // in cm/sec ; actual pressure must be in pa (so 101325 about at sea level)
 163 #ifdef AIRSPEED_AT_SEA_LEVEL_AND_15C
 164                airSpeedData.smoothAirSpeed =  131.06 * sqrt( (float) ( abs(smoothDifPressureAdc) ) ); // indicated airspeed is calculated at 15 Celsius and 101325 pascal
 165 #else
 166                airSpeedData.smoothAirSpeed =  2458 * sqrt( (float) ( abs(smoothDifPressureAdc) * airSpeedData.temperature4525  /  actualPressure) ); // in cm/sec ; actual pressure must be in pa (so 101325 about at sea level)
 167 #endif
 168              if ( smoothDifPressureAdc < 0 ) airSpeedData.smoothAirSpeed = - airSpeedData.smoothAirSpeed ; // apply the sign
 169
 170 #ifdef DEBUG4525RAWDATA
 171                   static bool firstRawData = true ;
 172                   if ( firstRawData ) {
 173                           printer->println(F("at,  difPressureAdc ,difPressADC_0 , countAverage , difPressureAdcSum4Values ,airSpeedData.difPressureAdc_zero , expoSmooth4525_adc_auto ,smoothDifPressureAdc ,  smoothAirSpeed, ")) ;
 174                         firstRawData = false ;
 175                   } else {
 176                         printer->print( airSpeedMicros); printer->print(F(" , "));
 177                         printer->print(  difPressureAdc); printer->print(F(" , "));
 178                         printer->print( difPressureAdc_0); printer->print(F(" , "));
 179                         printer->print( countAverage); printer->print(F(" , "));
 180                         printer->print( difPressureAdcSum4Values); printer->print(F(" , "));
 181                         printer->print( airSpeedData.difPressureAdc_zero); printer->print(F(" , "));
 182                         printer->print( expoSmooth4525_adc_auto * 1000); printer->print(F(" , "));
 183                         printer->print( airSpeedData.smoothDifPressureAdc); printer->print(F(" , "));
 184                          printer->print( airSpeedData.smoothAirSpeed * 3.6 / 100); printer->print(F(" , "));
 185
 186                         printer->println(" ") ;
 187                   }
 188 #endif
 189
 190            } // end if data[0] is valid
 191         } // end no error on I2C
 192         airSpeedMillis = millis() ;
 193 #ifdef DEBUG4525READINOUTDELAY
 194          airSpeedMicrosDebugMid2 = micros() ;
 195 #endif
 196
 197         if (airSpeedMillis > nextAirSpeedMillis){ // publish airspeed only once every xx ms
 198               nextAirSpeedMillis = airSpeedMillis + 200 ;
 199               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
 200 #ifdef AIRSPEED_IN_KMH  // uncomment this line if AIR speed has to be in knot instead of km/h
 201                   airSpeedData.airSpeed.value = airSpeedData.smoothAirSpeed * 0.36 ; // from cm/sec to 1/10 km/h
 202 #else
 203                   airSpeedData.airSpeed.value = airSpeedData.smoothAirSpeed * 0.1943844492 ; // from cm/sec to 1/10 knot/h
 204 #endif
 205               } else {
 206                   airSpeedData.airSpeed.value = 0 ;
 207               }
 208               airSpeedData.airSpeed.available = true ;
 209 // check if offset must be reset
 210               if (airSpeedData.airspeedReset) { // adjust the offset if a reset command is received from Tx
 211                     offset4525 =  offset4525  + smoothDifPressureAdc ;
 212                     airSpeedData.airspeedReset = false ; // avoid that offset is changed again and again if PPM do not send a command
 213               }
 214
 215
 216        }  // end of process every xx millis
 217
 218 #ifdef DEBUG4525READINOUTDELAY
 219          airSpeedMicrosDebugMid3 = micros() ;
 220          printer->print(F("at= "));
 221          printer->print(airSpeedMicrosDebug) ;
 222          printer->print(F(" delay in out Mid= "));
 223          airSpeedMicrosDebugMid1 =  airSpeedMicrosDebugMid1 - airSpeedMicrosDebug ;
 224          printer->print(airSpeedMicrosDebugMid1) ;
 225          printer->print(F(" delay in out 100ms= "));
 226          airSpeedMicrosDebugMid2 =  airSpeedMicrosDebugMid2 - airSpeedMicrosDebug ;
 227          printer->print(airSpeedMicrosDebugMid2) ;
 228          printer->print(F(" tot= "));
 229          airSpeedMicrosDebugMid3 = airSpeedMicrosDebugMid3 - airSpeedMicrosDebug ;
 230          printer->print(airSpeedMicrosDebugMid3) ;
 231           printer->println(F(" "));
 232 #endif
 233
 234 } // End of readSensor
 235
 236