IoT Drone - Part 2 - Sensors

// This #include statement was automatically added by the Particle IDE.
#include "Adafruit_10DOF_IMU/Adafruit_10DOF_IMU.h"

// This #include statement was automatically added by the Particle IDE.
#include "TinyGPS/TinyGPS.h"

//Set-up GPS Library
TinyGPS gps;
char szInfo[100];
int updateRate = 1000; //update rate for the sensors in milliseconds
bool isValidGPS = false;

//GPS Global Variables
float lat, lon, falt, fkmph;
unsigned long age;

//IMU Global Variables
float acX, acY, acZ, gyroX, gyroY, gyroZ, pitch, roll, heading, imuPress, imuAlt, imuTemp;

//Set-up IMU
Adafruit_10DOF                dof   = Adafruit_10DOF();
Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);
Adafruit_LSM303_Mag_Unified   mag   = Adafruit_LSM303_Mag_Unified(30302);
Adafruit_BMP085_Unified       bmp   = Adafruit_BMP085_Unified(18001);
Adafruit_L3GD20_Unified       gyro  = Adafruit_L3GD20_Unified(20);

/* Update this with the correct SLP for accurate altitude measurements */
float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;

//Ultrasonic Sensor Variables
float cm;

// Pins for Ultrasonic Distance Sensor
const int TRIG_PIN = 6;
const int ECHO_PIN = 7;

// Anything over 400 cm (23200 us pulse) is "out of range"
const unsigned int MAX_DIST = 23200;


void setup() {
    
    // The Trigger pin will tell the sensor to range find
  pinMode(TRIG_PIN, OUTPUT);
  digitalWrite(TRIG_PIN, LOW);
  
  //Begin serial communication for GPS data
  Serial1.begin(9600);
  
  //Begin Diagnostics
  Serial.begin(9600);
  Serial.println(F("Adafruit 10DOF Tester")); Serial.println("");
  
  /* Initialise the sensors */
  if(!accel.begin())
  {
    /* There was a problem detecting the ADXL345 ... check your connections */
    Serial.println(F("Ooops, no LSM303 detected ... Check your wiring!"));
    while(1);
  }
  if(!mag.begin())
  {
    /* There was a problem detecting the LSM303 ... check your connections */
    Serial.println("Ooops, no LSM303 detected ... Check your wiring!");
    while(1);
  }
  if(!bmp.begin())
  {
    /* There was a problem detecting the BMP085 ... check your connections */
    Serial.print("Ooops, no BMP085 detected ... Check your wiring or I2C ADDR!");
    while(1);
  }
  if(!gyro.begin())
  {
    /* There was a problem detecting the L3GD20 ... check your connections */
    Serial.print("Ooops, no L3GD20 detected ... Check your wiring or I2C ADDR!");
    while(1);
  }

}

void loop() { 
    
    checkGPS(); //Gets GPS Values
    
    checkIMU();  //Gets IMU Values
  
    checkDist();  //Gets Distance to Obstacle
    
    //Assembles the message to publish to particle cloud
   sprintf(szInfo, "%.6f,%.6f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f", lat, lon, falt, fkmph, imuAlt, acX, acY, acZ,
            gyroX, gyroY, acZ, pitch, roll, heading, imuPress, imuTemp, cm);
    
    Spark.publish("gpsloc", szInfo); //Publish Data

    delay(updateRate);  //Loop Delay
}

void checkGPS(){
    for (unsigned long start = millis(); millis() - start < 1000;){
        // Check GPS data is available
        while (Serial1.available()){
            char c = Serial1.read();
            
            // parse GPS data
            if (gps.encode(c))
                isValidGPS = true;
        }
      }
    
    // If we have a valid GPS location then get it
    if (isValidGPS){
        
        gps.f_get_position(&lat, &lon, &age);
        falt = gps.f_altitude();
        fkmph = gps.f_speed_kmph();
        
    }
    else{
        // Not a valid GPS location, just pass 0.0
        lat = 0.0;
        lon = 0.0;
        falt = 0.0;
        fkmph = 0.0;
    }
}

void checkIMU(){
     /* Get a new sensor event */
  sensors_event_t event;
  sensors_event_t accel_event;
  sensors_event_t mag_event;
  sensors_vec_t   orientation;
   
  /* Display the results (acceleration is measured in m/s^2) */
  accel.getEvent(&event);
  acX = event.acceleration.x;
  acY = event.acceleration.y;
  acZ = event.acceleration.z;
  
/* Calculate pitch and roll from the raw accelerometer data */
  accel.getEvent(&accel_event);
  if (dof.accelGetOrientation(&accel_event, &orientation))
  {
    /* 'orientation' should have valid .roll and .pitch fields */
    roll = orientation.roll;
    pitch = orientation.pitch;
  }
  
  /* Calculate the heading using the magnetometer */
  mag.getEvent(&mag_event);
  if (dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &orientation))
  {
    /* 'orientation' should have valid .heading data now */
    heading = orientation.heading;
  }

  /* Display the results (gyrocope values in rad/s) */
  gyro.getEvent(&event);
  gyroX = event.gyro.x;
  gyroY = event.gyro.y;
  gyroZ = event.gyro.z;  

  /* Display the pressure sensor results (barometric pressure is measure in hPa) */
  bmp.getEvent(&event);
  if (event.pressure)
  {
    /* Display atmospheric pressure in hPa */

    imuPress = event.pressure;

    /* Display ambient temperature in C */
    float temperature;
    bmp.getTemperature(&temperature);
    imuTemp = temperature;

    /* Then convert the atmospheric pressure, SLP and temp to altitude    */
    /* Update this next line with the current SLP for better results      */
    float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;
    imuAlt = bmp.pressureToAltitude(seaLevelPressure, event.pressure, temperature); 

  }
}

void checkDist (){
    unsigned long t1;
    unsigned long t2;
    unsigned long pulse_width;

    // Hold the trigger pin high for at least 10 us
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);

    // Wait for pulse on echo pin
    while ( digitalRead(ECHO_PIN) == 0 );

    // Measure how long the echo pin was held high (pulse width)
    // Note: the micros() counter will overflow after ~70 min
    t1 = micros();
    while ( digitalRead(ECHO_PIN) == 1);
    t2 = micros();
    pulse_width = t2 - t1;

    // Calculate distance in centimeters and inches. The constants
    // are found in the datasheet, and calculated from the assumed speed 
    //of sound in air at sea level (~340 m/s).
    cm = pulse_width / 58.0;

}