// Basic software for HLCA from Sensortechnics
//www.basicairdata.eu (c) JLJ 2013
//Circuit description http://www.basicairdata.eu/integration.html
// From D/S, if you use different ranges or unidirectional sensor check and correct ranges
// Pressure  - Sensor output hex/dec
//12.5  mBar 1250 Pa - 6CCCx /27852d
//-12.5 mBar 1250 Pa - 0666x /1638d
// 0    mBar    0 Pa - 3999x/14745
//(27852-1638)/2500=10.4856 per Pascal

#include <Wire.h>

void setup()
{
  Wire.begin();                
  Serial.begin(57600);//must macth serial or xbee setting        
}
unsigned long timebase; //Arduino time base
float offset=0;//Offset to remove from readings [Pa]
int   reading ;///The reading from pressure sensor[Pa]
float rawdata; //Reading with offset
float lastdata;//reading after remove offset[Pa]
float lastdatafilter;//Last data sample from digital filter
float data[]={0,0,0}; //Array containing measure values, index is the delay respect actual sample time. sensorout=1/3*(data[0]+data[1]+data[2]) Moving average filter
float c=0.9965790;//Pitot calibration factor
float iasms;//[m/s]
int basesample=50;//[ms] set the time between two sensor readings
int samplemultiplier=3*1 ;//Report rate trough serial is basesample*samplemultiplier
//status variables
int what=1;//Keep trace of time

void loop()
{
  delay(basesample);                  
  Wire.beginTransmission(120); //Initialize correct I2C device number
  Wire.requestFrom(120, 2);
  if(2 <= Wire.available())   //When the two requested bytes are available proceed to data handling
  {
    reading = Wire.read();  //16 Bit reading, 1 byte per time
    reading = reading << 8;
    reading |= Wire.read();
    rawdata=(reading-1638)/10.4856-1250;
    lastdata=RemoveOffset(reading,offset);
//Equivalent code
/*    data[2]=data[1];
    data[1]=data[0];
    data[0]=lastdata;
    lastdatafilter=(data[0]+data[1]+data[2])/3;
    */
    lastdatafilter=Ma2ndorder(lastdata,data);
    if (what>samplemultiplier) {
      timebase=millis(); //Refresh data for timestamp
 //     Serial.print("Time :");    
      Serial.print(timebase);
      Serial.print(";");
//      Serial.print("Raw Reading decimal");    
      Serial.print(reading);
      Serial.print(";");
//      Serial.print("Raw Reading Pa:");    
      Serial.print(rawdata);
      Serial.print(";");
//      Serial.print("Reading Pa :");    
      Serial.print(lastdata);
      Serial.print(";");
//      Serial.print("Filtered Pa :");    
      Serial.print(lastdatafilter);
      Serial.print(";");
//Internal data debug
//      Serial.print(data[2]);
//      Serial.print(";");
//Internal data debug
//      Serial.print(data[1]);
//      Serial.print(";");
//Internal data debug
//      Serial.print(data[0]);
//      Serial.print(";");
//      Serial.print("IAS m/s :");    
      iasms=sqrt((float)(2*lastdatafilter*c/(float)1.225));
      Serial.println(iasms);

      what=1;//Init the counter

    }
    what++;
  }
      
}
//Second order moving average filter
float Ma2ndorder(float lastdata,float *pdata){
    *(pdata +2)=*(pdata +1);
    *(pdata+ 1)=*(pdata +0);
    *(pdata + 0)=lastdata;
  return ((*(pdata +0)+*(pdata +1)+*(pdata+2))/3);
}

//Function to remove know offset from readings, Offset will be supplied in Pascal.
float RemoveOffset(int reading, float offset){
  float output;
  output=(((reading-offset*10.48356)-1638)/10.4856)-1250;
  return output;
}