Boat Data Logger

//required for sdcard
#include <SPI.h>
#include <SD.h>
//required for gps
#include <TinyGPS++.h>
#include <SoftwareSerial.h>
//required for IMU
#include <CurieIMU.h>
#include <MadgwickAHRS.h>


const bool DEBUG = false;

static const int RXPin = 7, TXPin = 8;  //Pins for gps module serial port
static const uint32_t GPSBaud = 9600;   // gps baud rate, the module I have use 9600
SoftwareSerial ss(RXPin, TXPin);        // The serial connection to the GPS device
TinyGPSPlus gps;                        // The TinyGPS++ object

unsigned long last = 0UL;               // For stats that happen every 5 seconds

const int chipSelect = 10;              //sd card

//IMU
Madgwick filter;
unsigned long microsPerReading, microsPrevious;
float accelScale, gyroScale;

void setup()
{
  Serial.begin(115200);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }

  Serial.print("Initializing SD card...");

  // see if the card is present and can be initialized:
  if (!SD.begin(chipSelect)) {
    Serial.println("Card failed, or not present");
    // don't do anything more:
    return;
  }
  Serial.println("card initialized.");
  
  ss.begin(GPSBaud);

  Serial.println(F("imu1gps5ssd1.ino"));

  Serial.println(TinyGPSPlus::libraryVersion());

  Serial.println();

  //IMU
    // start the IMU and filter
  CurieIMU.begin();
  CurieIMU.setGyroRate(25);
  CurieIMU.setAccelerometerRate(25);
  filter.begin(25);

  // Set the accelerometer range to 2G
  CurieIMU.setAccelerometerRange(2);
  // Set the gyroscope range to 250 degrees/second
  CurieIMU.setGyroRange(250);

   //Serial.print("Starting Gyroscope calibration...");
    CurieIMU.autoCalibrateGyroOffset();
    //Serial.println(" Done");
    //Serial.print("Starting Acceleration calibration...");
    CurieIMU.autoCalibrateXAccelOffset(0);
    CurieIMU.autoCalibrateYAccelOffset(0);
    CurieIMU.autoCalibrateZAccelOffset(1);
    //Serial.println(" Done");

    //Serial.println("Enabling Gyroscope/Acceleration offset compensation");
    CurieIMU.setGyroOffsetEnabled(true);
    CurieIMU.setAccelOffsetEnabled(true);
  // initialize variables to pace updates to correct rate
  microsPerReading = 1000000 / 25;
  microsPrevious = micros();
}

void loop(){

   // open the file. note that only one file can be open at a time,
  // so you have to close this one before opening another.
  File dataFile = SD.open("gps.txt", FILE_WRITE);

  // if the file is available, write to it:
  if (dataFile) { 
    // Dispatch incoming characters
    while (ss.available() > 0){
      char c = ss.read();
      dataFile.write(c);
      gps.encode(c);
    }
    String imuRetVal = imu();
    dataFile.println(imuRetVal);
    //Serial.println("IMU Return Value:" + imuRetVal);
    dataFile.close();
  } else {
    Serial.println("error opening datalog.txt");// if the file isn't open, pop up an error:
  }
  
   if (gps.location.isUpdated()){
      Serial.print(F("LOCATION   Fix Age="));
      Serial.print(gps.location.age());
      Serial.print(F("ms Raw Lat="));
      Serial.print(gps.location.rawLat().negative ? "-" : "+");
      Serial.print(gps.location.rawLat().deg);
      Serial.print("[+");
      Serial.print(gps.location.rawLat().billionths);
      Serial.print(F(" billionths],  Raw Long="));
      Serial.print(gps.location.rawLng().negative ? "-" : "+");
      Serial.print(gps.location.rawLng().deg);
      Serial.print("[+");
      Serial.print(gps.location.rawLng().billionths);
      Serial.print(F(" billionths],  Lat="));
      Serial.print(gps.location.lat(), 6);
      Serial.print(F(" Long="));
      Serial.println(gps.location.lng(), 6);
    } else if (gps.date.isUpdated()){
      Serial.print(F("DATE       Fix Age="));
      Serial.print(gps.date.age());
      Serial.print(F("ms Raw="));
      Serial.print(gps.date.value());
      Serial.print(F(" Year="));
      Serial.print(gps.date.year());
      Serial.print(F(" Month="));
      Serial.print(gps.date.month());
      Serial.print(F(" Day="));
      Serial.println(gps.date.day());
    } else if (gps.time.isUpdated()){
      Serial.print(F("TIME       Fix Age="));
      Serial.print(gps.time.age());
      Serial.print(F("ms Raw="));
      Serial.print(gps.time.value());
      Serial.print(F(" Hour="));
      Serial.print(gps.time.hour());
      Serial.print(F(" Minute="));
      Serial.print(gps.time.minute());
      Serial.print(F(" Second="));
      Serial.print(gps.time.second());
      Serial.print(F(" Hundredths="));
      Serial.println(gps.time.centisecond());
    } else if (gps.speed.isUpdated()){
      Serial.print(F("SPEED      Fix Age="));
      Serial.print(gps.speed.age());
      Serial.print(F("ms Raw="));
      Serial.print(gps.speed.value());
      Serial.print(F(" Knots="));
      Serial.print(gps.speed.knots());
      Serial.print(F(" MPH="));
      Serial.print(gps.speed.mph());
      Serial.print(F(" m/s="));
      Serial.print(gps.speed.mps());
      Serial.print(F(" km/h="));
      Serial.println(gps.speed.kmph());
    } else if (gps.course.isUpdated()){
      Serial.print(F("COURSE     Fix Age="));
      Serial.print(gps.course.age());
      Serial.print(F("ms Raw="));
      Serial.print(gps.course.value());
      Serial.print(F(" Deg="));
      Serial.println(gps.course.deg());
    } else if (gps.altitude.isUpdated()){
      Serial.print(F("ALTITUDE   Fix Age="));
      Serial.print(gps.altitude.age());
      Serial.print(F("ms Raw="));
      Serial.print(gps.altitude.value());
      Serial.print(F(" Meters="));
      Serial.print(gps.altitude.meters());
      Serial.print(F(" Miles="));
      Serial.print(gps.altitude.miles());
      Serial.print(F(" KM="));
      Serial.print(gps.altitude.kilometers());
      Serial.print(F(" Feet="));
      Serial.println(gps.altitude.feet());
    } else if (gps.satellites.isUpdated()){
      Serial.print(F("SATELLITES Fix Age="));
      Serial.print(gps.satellites.age());
      Serial.print(F("ms Value="));
      Serial.println(gps.satellites.value());
    } else if (gps.hdop.isUpdated()){
      Serial.print(F("HDOP       Fix Age="));
      Serial.print(gps.hdop.age());
      Serial.print(F("ms Value="));
      Serial.println(gps.hdop.value());
    } else if (millis() - last > 5000){
      Serial.println();
      if (gps.location.isValid()){
//        static const double LONDON_LAT = 51.508131, LONDON_LON = -0.128002;
//        double distanceToLondon =
//          TinyGPSPlus::distanceBetween(
//            gps.location.lat(),
//            gps.location.lng(),
//            LONDON_LAT, 
//            LONDON_LON);
//        double courseToLondon =
//          TinyGPSPlus::courseTo(
//            gps.location.lat(),
//            gps.location.lng(),
//            LONDON_LAT, 
//            LONDON_LON);
  
//        Serial.print(F("LONDON     Distance="));
//        Serial.print(distanceToLondon/1000, 6);
//        Serial.print(F(" km Course-to="));
//        Serial.print(courseToLondon, 6);
//        Serial.print(F(" degrees ["));
//        Serial.print(TinyGPSPlus::cardinal(courseToLondon));
//        Serial.println(F("]"));
      }
  
      Serial.print(F("DIAGS      Chars="));
      Serial.print(gps.charsProcessed());
      Serial.print(F(" Sentences-with-Fix="));
      Serial.print(gps.sentencesWithFix());
      Serial.print(F(" Failed-checksum="));
      Serial.print(gps.failedChecksum());
      Serial.print(F(" Passed-checksum="));
      Serial.println(gps.passedChecksum());

      //Send the gps coords out
      Serial.print("$GPS,");
      Serial.print(gps.location.lat());
      Serial.print( "," );
      Serial.print(gps.location.lng());
      Serial.println(" ");

      //send the data out
      Serial.print("$DATE,");
      Serial.print(gps.date.year());
      Serial.print(F("/"));
      Serial.print(gps.date.month());
      Serial.print(F("/"));
      Serial.println(gps.date.day());

      //Send the time out.
      Serial.print("$TIME,");
      Serial.print(gps.time.hour());
      Serial.print(":");
      Serial.print(gps.time.minute());
      Serial.print(":"); 
      Serial.print(gps.time.second());
      Serial.print(" GMT");
      
      if (gps.charsProcessed() < 10)
        Serial.println(F("WARNING: No GPS data.  Check wiring."));
  
      last = millis();
      Serial.println();
   } 
}

String imu(){
  int aix, aiy, aiz;
  int gix, giy, giz;
  float ax, ay, az;
  float gx, gy, gz;
  float roll, pitch, heading;
  unsigned long microsNow;
  
  String retVal = "$IMU,";
   
  // check if it's time to read data and update the filter
  microsNow = micros();
  if (microsNow - microsPrevious >= microsPerReading) {

    // read raw data from CurieIMU
    CurieIMU.readMotionSensor(aix, aiy, aiz, gix, giy, giz);

    // convert from raw data to gravity and degrees/second units
    ax = convertRawAcceleration(aix);
    ay = convertRawAcceleration(aiy);
    az = convertRawAcceleration(aiz);
    gx = convertRawGyro(gix);
    gy = convertRawGyro(giy);
    gz = convertRawGyro(giz);

    // update the filter, which computes orientation
    filter.updateIMU(gx, gy, gz, ax, ay, az);

    // print the heading, pitch and roll
    roll = filter.getRoll();
    pitch = filter.getPitch();
    heading = filter.getYaw();

    retVal = retVal + roll + "," + pitch + "," + heading;

    Serial.println(retVal);
    
    if (DEBUG){
      Serial.print("Orientation: ");
      Serial.print(heading);
      Serial.print(" ");
      Serial.print(pitch);
      Serial.print(" ");
      Serial.println(roll);
    }
    // increment previous time, so we keep proper pace
    microsPrevious = microsPrevious + microsPerReading;
  }
  return retVal;
}


float convertRawAcceleration(int aRaw) {
  // since we are using 2G range
  // -2g maps to a raw value of -32768
  // +2g maps to a raw value of 32767
  
  float a = (aRaw * 2.0) / 32768.0;
  return a;
}

float convertRawGyro(int gRaw) {
  // since we are using 250 degrees/seconds range
  // -250 maps to a raw value of -32768
  // +250 maps to a raw value of 32767
  
  float g = (gRaw * 250.0) / 32768.0;
  return g;
}