AMAT_DAS_2016.ino

  /*
    Altimeter + Drop Mech + Pitot Tube + Accelerometer
  */
  
  /*
   MPL3115A2 Altitude Sensor Example
   By: A.Weiss, 7/17/2012, changes Nathan Seidle Sept 23rd, 2013
   License: This code is public domain but you buy me a beer if you use this and we meet someday (Beerware license).
   
   Hardware Connections (Breakoutboard to Arduino):
   -VCC = 3.3V
   -SDA = A4
   -SCL = A5
   -INT pins can be left unconnected for this demo
   
   Usage:
   -Serial terminal at 9600bps
   -Prints altitude in meters, temperature in degrees C, with 1/16 resolution.
   -software enabled interrupt on new data, ~1Hz with full resolution
   
   During testing, GPS with 9 sattelites reported 5393ft, sensor reported 5360ft (delta of 33ft). Very close!
   
   */
  
  #include <Wire.h> // for IIC communication
  #include <Servo.h>  //Servo library
  #define STATUS     0x00
  #define OUT_P_MSB  0x01
  #define OUT_P_CSB  0x02
  #define OUT_P_LSB  0x03
  #define OUT_T_MSB  0x04
  #define OUT_T_LSB  0x05
  #define DR_STATUS  0x06
  #define OUT_P_DELTA_MSB  0x07
  #define OUT_P_DELTA_CSB  0x08
  #define OUT_P_DELTA_LSB  0x09
  #define OUT_T_DELTA_MSB  0x0A
  #define OUT_T_DELTA_LSB  0x0B
  #define WHO_AM_I   0x0C
  #define F_STATUS   0x0D
  #define F_DATA     0x0E
  #define F_SETUP    0x0F
  #define TIME_DLY   0x10
  #define SYSMOD     0x11
  #define INT_SOURCE 0x12
  #define PT_DATA_CFG 0x13
  #define BAR_IN_MSB 0x14
  #define BAR_IN_LSB 0x15
  #define P_TGT_MSB  0x16
  #define P_TGT_LSB  0x17
  #define T_TGT      0x18
  #define P_WND_MSB  0x19
  #define P_WND_LSB  0x1A
  #define T_WND      0x1B
  #define P_MIN_MSB  0x1C
  #define P_MIN_CSB  0x1D
  #define P_MIN_LSB  0x1E
  #define T_MIN_MSB  0x1F
  #define T_MIN_LSB  0x20
  #define P_MAX_MSB  0x21
  #define P_MAX_CSB  0x22
  #define P_MAX_LSB  0x23
  #define T_MAX_MSB  0x24
  #define T_MAX_LSB  0x25
  #define CTRL_REG1  0x26
  #define CTRL_REG2  0x27
  #define CTRL_REG3  0x28
  #define CTRL_REG4  0x29
  #define CTRL_REG5  0x2A
  #define OFF_P      0x2B
  #define OFF_T      0x2C
  #define OFF_H      0x2D
  
  #define MPL3115A2_ADDRESS 0x60 // 7-bit I2C address
  #define READ_ADDR 0xC1 // 8-bit Read address
  #define WRITE_ADDR 0xC0 // 8-bit Write address
  
  #define PIN A0    //Pitot Tube
  #define RED 8    //Red wing LED
  #define GREEN 9  //Green wing LED
  
  float sensorValue = 0;
  float outputValue;
  float rho = 1.275;
  float pressure;
  float velocity;
  float temperature;
  
  //accelerometer
  int xPin = A1;
  int yPin = A2;
  int zPin = A3;
  int xv = 0;
  int yv = 0;
  int zv = 0;
  float xg;
  float yg;
  float zg;
  
  float x_zero=1.65/0.3;
  float y_zero=1.65/0.3;
  float z_zero=1.65/0.3;

  float pitch;
  float roll;
  
  float *pointerPitch = NULL;
  float *pointerRoll = NULL;
  float *pointerXg = NULL;
  float *pointerYg = NULL;
  float *pointerZg = NULL;
  
  long startTime;
  float altitude;
  
  Servo myservo;
  int tempPin = A0;
  int pin = 10;  //Servo attached to pin 10
  int start = 90;   //Start at 0 degrees
  int finish = 20; //End at 90 degrees
  float reset = 0;  //Set altitude to 0
  float drop_alt = 0;  //Drop altitude
  float average = 0;  
  float alt_average;  //altimeter averaged
  float alt_array[10];  //create altimeter running average
  float vel_average;  //velocity average
  float vel_array[10];  //create velocity running average
  float gravity = 9.81;  //acceleration due to gravity
  float time;  //drop time
  //float y_velocity;  //vertical velocity
  float x_distance;  //horizontal distance travelled
  int count = 0;    //set a count for LED timer (counter for moving average)
  char key;  //one of 3 options (start, drop, reset)
  
  
  void setup()
  {
    Wire.begin();        // join i2c bus
    Serial.begin(57600);  // start serial for output
  
    if(IIC_Read(WHO_AM_I) == 196) 
      Serial.println("MPL3115A2 online!");
    else
      Serial.println("No response - check connections");
  
    // Configure the sensor
    setModeAltimeter(); // Measure altitude above sea level in meters
    //setModeBarometer(); // Measure pressure in Pascals from 20 to 110 kPa
  
    setOversampleRate(7); // Set Oversample to the recommended 128
    enableEventFlags(); // Enable all three pressure and temp event flags
    
    //Drop Mech Setup
    myservo.attach(pin);
    myservo.write(start);
    
    //Pitot Tube
    pinMode(PIN, INPUT);
    pinMode(RED, OUTPUT);
    pinMode(GREEN, OUTPUT);
    pinMode(tempPin,INPUT);
  }
  
  void loop()
  {
    count++;
    digitalWrite(RED, LOW);  //turn off RED LED
    digitalWrite(GREEN, LOW);  //turn off GREEN LED
    
    if (count % 6 == 0)
    {
      digitalWrite(RED, HIGH);  //flash RED LED
    }//end if
    
    if (count % 10 == 0)
    {
      digitalWrite(GREEN, HIGH);  //flash GREEN LED
      count = 0;  //reset count to 0
    }//end if
    
    key = 0; //Reset Key
    //myservo.write(start);
    if (Serial.available())
    {
      key = Serial.read();
//      Serial.print("Command = ");
//      Serial.println(key);
      
      if(key == '1')
      {
        myservo.write(start);
//        Serial.print("angle is: ");
        Serial.println(myservo.read());
      }//end if
    
      if(key == '2')
      {
//        Serial.println("Bombs Away!!");
        myservo.write(finish);
        drop_alt = alt_average;
  //      Serial.print("angle is: ");
  //      Serial.println(myservo.read());
      }//end if
      
      if(key == '3')
      {
        reset = readAltitudeFt();
      }//end if
      
      if(key == '4')
      {
        resetAccelero();
      }//end if
      
    }//end if
    
    //startTime = millis();
  
    //float altitude = readAltitude();
    //Serial.print("Altitude(m):");
    //Serial.print(altitude, 2);
  
    altitude = readAltitudeFt() - reset;
    
    alt_array[count] = altitude;  //put alt value in moving average
    average = 0;
    for(int i=0; i<9; i++)
    {
      average = average + alt_array[i];  //sum array
    }//end for
    alt_average = average/10;  //calculate average
    
    
  //  float pressure = readPressure();
  //  Serial.print("Pressure(Pa): ");
  //  Serial.println(pressure, 2);
  
    //float temperature = readTemp();
    //Serial.print(" Temp(c):");
    //Serial.print(temperature, 2);
  
  //  float temperature = readTempF();
  //  Serial.print("Temp(f): ");
  //  Serial.println(temperature, 2);
    
    //Serial.print(" time diff:");
    //Serial.print(millis() - startTime);
  
  /** Pitot Tube **/
    sensorValue = analogRead(PIN);
    //Serial.print("Sensorvalue = ");
    //Serial.print(sensorValue);
    //Serial.print("    ");
    sensorValue = sensorValue/1023*5;
    pressure = sensorValue - 2.4;
  //  Serial.print("Dynamic Pressure: ");
  //  Serial.println(pressure);
    velocity = sq((2*pressure/rho));
    vel_array[count] = velocity;  //put alt value in moving average
    average = 0;
    for(int i=0; i<9; i++)
    {
      average = average + vel_array[i];  //sum array
    }//end for
    vel_average = average/10;  //calculate average
    
    
    time = sqrt(2*alt_average/gravity);  //time to hit ground
    
  //  y_velocity = gravity*time;  //Vertical Velocity
    x_distance = vel_average*time;  //Horizontal distance travelled
    
    temperature = temperatureLoop();

    acceleroLoop();
    
    //printData1();
    printData2();

  }
  
  void printData1()
  {
    
    Serial.print("Altitude(ft): ");
    Serial.print(alt_average, 2);
    Serial.print(" Feet  ");
    
    Serial.print("Drop Altitude: ");
    Serial.println(drop_alt);
    
    Serial.print("   Velocity: "); 
    Serial.print(vel_average);
    Serial.print(" m/s");
    
    Serial.print("  time: ");
    Serial.print(time);
    
    Serial.print("   Drop Dist: ");
    Serial.print(x_distance*3.28084);
    Serial.print(" ft\n");
    
    Serial.print("  Temperature:  ");
    Serial.print(temperature);
    Serial.print("  F\n");
    
    Serial.print("  Pitch =  ");
    Serial.print(*pointerPitch);
    
    Serial.print("  Roll =  ");
    Serial.print(*pointerRoll);

    Serial.print("  xg =");
    Serial.print(*pointerXg);
    Serial.print("  yg =");
    Serial.print(*pointerYg);
    Serial.print("  zg =");
    Serial.print(*pointerZg);
    
    Serial.println("\n");
    
    delay(300);  
  }
  
  void printData2()
  {
    Serial.print(alt_average, 2);
    Serial.print(" , ");
    Serial.print(drop_alt);
    Serial.print(" , ");
    Serial.print(vel_average);
    Serial.print(" , ");
    Serial.print(temperature);
    Serial.print(" , ");
    Serial.print(*pointerPitch);
    Serial.print(" , ");
    Serial.print(*pointerRoll);
    
    Serial.println("\n");
    delay(400);
  }
  
  float temperatureLoop()
    {
     //getting the voltage reading from the temperature sensor
     int reading = analogRead(tempPin);  
     
     // converting that reading to voltage, for 3v arduino use 3.3
     float voltage = reading * 5./1024.;
     
     // now print out the temperature
     float temperatureC = (voltage - 0.5) * 100 ;  //converting from 10 mv per degree wit 500 mV offset
                                                   //to degrees ((voltage - 500mV) times 100)
     // now convert to Fahrenheit
     float temperatureF = (temperatureC * 9.0 / 5.0) + 32.0;
     return temperatureF;
    }
    
    void resetAccelero()
    {
      x_zero = (analogRead(xPin)/1023.)*3.3/0.3;
      y_zero = (analogRead(yPin)/1023.)*3.3/0.3;
      z_zero = (analogRead(zPin)/1023.)*3.3/0.3 +1;
      
    }
    
    void acceleroLoop() 
    {
      
      xv = analogRead(xPin);
      yv = analogRead(yPin);
      zv = analogRead(zPin);
      
      xg = ((xv/1023.)*3.3)/0.3 - x_zero;
      yg = ((yv/1023.)*3.3)/0.3 - y_zero;
      zg = ((zv/1023.)*3.3)/0.3 - z_zero;
      
      
      // apply trigonometry to get the pitch and roll:
      pitch = atan(xg/sqrt(pow(yg,2) + pow(zg,2))); 
      roll = atan(yg/sqrt(pow(xg,2) + pow(zg,2))); 
      //convert radians into degrees
      pitch = pitch * (180.0/PI);
      roll = roll * (180.0/PI) ;
      
      pointerPitch = &pitch;
      pointerRoll = &roll;
      pointerXg = &xg;
      pointerYg = &yg;
      pointerZg = &zg;
    }
  
  //Returns the number of meters above sea level
  float readAltitude()
  {
    toggleOneShot(); //Toggle the OST bit causing the sensor to immediately take another reading
  
    //Wait for PDR bit, indicates we have new pressure data
    int counter = 0;
    while( (IIC_Read(STATUS) & (1<<1)) == 0)
    {
        if(++counter > 300) {
          Serial.println("IIC_Read timeout");
          return(-999); //Error out
        }//end if
        delay(1);
    }
    
    // Read pressure registers
    Wire.beginTransmission(MPL3115A2_ADDRESS);
    Wire.write(OUT_P_MSB);  // Address of data to get
    Wire.endTransmission(false); // Send data to I2C dev with option for a repeated start. THIS IS NECESSARY and not supported before Arduino V1.0.1!
    Wire.requestFrom(MPL3115A2_ADDRESS, 3); // Request three bytes
  
    //Wait for data to become available
    counter = 0;
    while(Wire.available() < 3)
    {
      if(counter++ > 300) {
        Serial.println("Wire timeout");
        return(-999); //Error out
      }//end if
      delay(1);
    }
  
    byte msb, csb, lsb;
    msb = Wire.read();
    csb = Wire.read();
    lsb = Wire.read();
  
    toggleOneShot(); //Toggle the OST bit causing the sensor to immediately take another reading
  
    // The least significant bytes l_altitude and l_temp are 4-bit,
    // fractional values, so you must cast the calulation in (float),
    // shift the value over 4 spots to the right and divide by 16 (since 
    // there are 16 values in 4-bits). 
    float tempcsb = (lsb>>4)/16.0;
  
    float altitude = (float)( (msb << 8) | csb) + tempcsb;
  
    return(altitude);
  }
  
  //Returns the number of feet above sea level
  float readAltitudeFt()
  {
    return(readAltitude() * 3.28084);
  }
  
  //Reads the current pressure in Pa
  //Unit must be set in barometric pressure mode
  float readPressure()
  {
    toggleOneShot(); //Toggle the OST bit causing the sensor to immediately take another reading
  
    //Wait for PDR bit, indicates we have new pressure data
    int counter = 0;
    while( (IIC_Read(STATUS) & (1<<2)) == 0)
    {
        if(++counter > 300) return(-999); //Error out
        delay(1);
    }
  
    // Read pressure registers
    Wire.beginTransmission(MPL3115A2_ADDRESS);
    Wire.write(OUT_P_MSB);  // Address of data to get
    Wire.endTransmission(false); // Send data to I2C dev with option for a repeated start. THIS IS NECESSARY and not supported before Arduino V1.0.1!
    Wire.requestFrom(MPL3115A2_ADDRESS, 3); // Request three bytes
  
    //Wait for data to become available
    counter = 0;
    while(Wire.available() < 3)
    {
      if(counter++ > 300) return(-999); //Error out
      delay(1);
    }
  
    byte msb, csb, lsb;
    msb = Wire.read();
    csb = Wire.read();
    lsb = Wire.read();
  
    toggleOneShot(); //Toggle the OST bit causing the sensor to immediately take another reading
  
    // Pressure comes back as a left shifted 20 bit number
    long pressure_whole = (long)msb<<16 | (long)csb<<8 | (long)lsb;
    pressure_whole >>= 6; //Pressure is an 18 bit number with 2 bits of decimal. Get rid of decimal portion.
  
    lsb &= 0b00110000; //Bits 5/4 represent the fractional component
    lsb >>= 4; //Get it right aligned
    float pressure_decimal = (float)lsb/4.0; //Turn it into fraction
  
    float pressure = (float)pressure_whole + pressure_decimal;
  
    return(pressure);
  }
  
  float readTemp()
  {
    toggleOneShot(); //Toggle the OST bit causing the sensor to immediately take another reading
  
    //Wait for TDR bit, indicates we have new temp data
    int counter = 0;
    while( (IIC_Read(STATUS) & (1<<1)) == 0)
    {
        if(++counter > 100) return(-999); //Error out
        delay(1);
    }
    
    // Read temperature registers
    Wire.beginTransmission(MPL3115A2_ADDRESS);
    Wire.write(OUT_T_MSB);  // Address of data to get
    Wire.endTransmission(false); // Send data to I2C dev with option for a repeated start. THIS IS NECESSARY and not supported before Arduino V1.0.1!
    Wire.requestFrom(MPL3115A2_ADDRESS, 2); // Request two bytes
  
    //Wait for data to become available
    counter = 0;
    while(Wire.available() < 2)
    {
      if(++counter > 100) return(-999); //Error out
      delay(1);
    }
  
    byte msb, lsb;
    msb = Wire.read();
    lsb = Wire.read();
    
    // The least significant bytes l_altitude and l_temp are 4-bit,
    // fractional values, so you must cast the calulation in (float),
    // shift the value over 4 spots to the right and divide by 16 (since 
    // there are 16 values in 4-bits). 
    float templsb = (lsb>>4)/16.0; //temp, fraction of a degree
  
    float temperature = (float)(msb + templsb);
  
    return(temperature);
  }
  
  //Give me temperature in fahrenheit!
  float readTempF()
  {
    return((readTemp() * 9.0)/ 5.0 + 32.0); // Convert celsius to fahrenheit
  }
  
  //Sets the mode to Barometer
  //CTRL_REG1, ALT bit
  void setModeBarometer()
  {
    byte tempSetting = IIC_Read(CTRL_REG1); //Read current settings
    tempSetting &= ~(1<<7); //Clear ALT bit
    IIC_Write(CTRL_REG1, tempSetting);
  }
  
  //Sets the mode to Altimeter
  //CTRL_REG1, ALT bit
  void setModeAltimeter()
  {
    byte tempSetting = IIC_Read(CTRL_REG1); //Read current settings
    tempSetting |= (1<<7); //Set ALT bit
    IIC_Write(CTRL_REG1, tempSetting);
  }
  
  //Puts the sensor in standby mode
  //This is needed so that we can modify the major control registers
  void setModeStandby()
  {
    byte tempSetting = IIC_Read(CTRL_REG1); //Read current settings
    tempSetting &= ~(1<<0); //Clear SBYB bit for Standby mode
    IIC_Write(CTRL_REG1, tempSetting);
  }
  
  //Puts the sensor in active mode
  //This is needed so that we can modify the major control registers
  void setModeActive()
  {
    byte tempSetting = IIC_Read(CTRL_REG1); //Read current settings
    tempSetting |= (1<<0); //Set SBYB bit for Active mode
    IIC_Write(CTRL_REG1, tempSetting);
  }
  
  //Setup FIFO mode to one of three modes. See page 26, table 31
  //From user jr4284
  void setFIFOMode(byte f_Mode)
  {
    if (f_Mode > 3) f_Mode = 3; // FIFO value cannot exceed 3.
    f_Mode <<= 6; // Shift FIFO byte left 6 to put it in bits 6, 7.
  
    byte tempSetting = IIC_Read(F_SETUP); //Read current settings
    tempSetting &= ~(3<<6); // clear bits 6, 7
    tempSetting |= f_Mode; //Mask in new FIFO bits
    IIC_Write(F_SETUP, tempSetting);
  }
  
  //Call with a rate from 0 to 7. See page 33 for table of ratios.
  //Sets the over sample rate. Datasheet calls for 128 but you can set it 
  //from 1 to 128 samples. The higher the oversample rate the greater
  //the time between data samples.
  void setOversampleRate(byte sampleRate)
  {
    if(sampleRate > 7) sampleRate = 7; //OS cannot be larger than 0b.0111
    sampleRate <<= 3; //Align it for the CTRL_REG1 register
  
    byte tempSetting = IIC_Read(CTRL_REG1); //Read current settings
    tempSetting &= 0b11000111; //Clear out old OS bits
    tempSetting |= sampleRate; //Mask in new OS bits
    IIC_Write(CTRL_REG1, tempSetting);
  }
  
  //Clears then sets the OST bit which causes the sensor to immediately take another reading
  //Needed to sample faster than 1Hz
  void toggleOneShot(void)
  {
    byte tempSetting = IIC_Read(CTRL_REG1); //Read current settings
    tempSetting &= ~(1<<1); //Clear OST bit
    IIC_Write(CTRL_REG1, tempSetting);
  
    tempSetting = IIC_Read(CTRL_REG1); //Read current settings to be safe
    tempSetting |= (1<<1); //Set OST bit
    IIC_Write(CTRL_REG1, tempSetting);
  }
  
  //Enables the pressure and temp measurement event flags so that we can
  //test against them. This is recommended in datasheet during setup.
  void enableEventFlags()
  {
    IIC_Write(PT_DATA_CFG, 0x07); // Enable all three pressure and temp event flags 
  }
  
  // These are the two I2C functions in this sketch.
  byte IIC_Read(byte regAddr)
  {
    // This function reads one byte over IIC
    Wire.beginTransmission(MPL3115A2_ADDRESS);
    Wire.write(regAddr);  // Address of CTRL_REG1 (write() queues data to be sent back)
    Wire.endTransmission(false); // Send data to I2C dev with option for a repeated start. THIS IS NECESSARY and not supported before Arduino V1.0.1!
    Wire.requestFrom(MPL3115A2_ADDRESS, 1); // Request the data...
    return Wire.read();
  }
  
  void IIC_Write(byte regAddr, byte value)
  {
    // This function writes one byto over IIC
    Wire.beginTransmission(MPL3115A2_ADDRESS);
    Wire.write(regAddr);
    Wire.write(value);
    Wire.endTransmission(true);
  }