Android GPS and IMU logger with The Tactigon

/************************************/
/* The Tactigon GPS and IMU Logging */
/* V0.7 - Michele Valentini         */
/************************************/


import blepdroid.*;
import blepdroid.BlepdroidDevice;
import android.os.Bundle;
import android.content.Context;
import java.util.UUID;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import android.location.Location;

import java.text.ParseException;
import java.text.SimpleDateFormat;
import java.util.Date;
import java.util.Locale;

Buffer buffer;
int bufferSize = 100; 
BlepdroidDevice device1;
Blepdroid blepdroid;
int textSizeVal = 60;
boolean allSetUp = false;
boolean targetCharacteristicFound = false;
float lat, lon, timestamp;
char latS, lonS;
float roll, pitch, yaw;
public static String deviceName = "TTGPS";
private SimpleDateFormat sdf;
private String filename;
private String path = "/sdcard/TheTactigon/";
public static String TACTIGON_QUAT_CHAR_STRING = "6039d2a4-080c-11e8-ba89-0ed5f89f718b";
public static UUID TACTIGON_QUAT_UUID = UUID.fromString(TACTIGON_QUAT_CHAR_STRING);

private GPSData lastGPS;
void setup() {

  lat = 0;
  lon= 0;
  timestamp = 0;
  latS = ' ';
  lonS = ' ';
  roll = 0;
  pitch = 0;
  yaw = 0;
  sdf  = new SimpleDateFormat();
  sdf.applyPattern("yyyyMMdd_HHmmss");

  filename =  "TheTactigon_"+getTime()+"_.kml";
  buffer = new Buffer(bufferSize, filename, path);
  fullScreen();
  smooth();
  blepdroid = new Blepdroid(this);
}

void draw() {
  if (allSetUp) {
    background(10);
    textSize(textSizeVal/3);
    textAlign(CENTER, CENTER);
    text("(DMm)", width/2, height/3-(3*textSizeVal));
    textSize(textSizeVal);
    textAlign(CENTER, CENTER);
    text(getLatString(), width/2, height/3-(2*textSizeVal));
    textAlign(CENTER, CENTER);
    text(getLonString(), width/2, height/3);
    textAlign(CENTER, CENTER);
    text(getTimestampString(), width/2, height/3+(2*textSizeVal));
    textAlign(CENTER, CENTER);
    text(getRollString(), width/2, (2*height/3)-(2*textSizeVal));
    textAlign(CENTER, CENTER);
    text(getPitchString(), width/2, (2*height/3));
    textAlign(CENTER, CENTER);
    text(getYawString(), width/2, (2*height/3)+(2*textSizeVal));
    textSize(textSizeVal/2);
    textAlign(CENTER, CENTER);
    text(path+filename, width/2, (2*height/3)+(3*textSizeVal));
  } else {
    background(100);
    textSize(textSizeVal*2);
    textAlign(CENTER, CENTER);
    text("TAP TO", width/2, height/2-(textSizeVal));
    text("CONNECT", width/2, height/2+(textSizeVal));
  }
}

void mousePressed() {
  if (targetCharacteristicFound == false) {
    println(" scan !");
    blepdroid.scanDevices();
  } else {
    println(" Connected " );
  }
}
void stop() {
  buffer.close();
  super.stop();
} 

void exit() {
  println("exiting");
  super.exit();
}

private String getLatString() {
  return "LAT: ".concat(Utils.NMEAtoDMm(lat)).concat(Character.toString(latS));
  // return Float.toString(lat).concat(Character.toString(latS));
}
private String getLonString() {
  return "LON: ".concat(Utils.NMEAtoDMm(lon)).concat(Character.toString(lonS));
  // return Float.toString(lon).concat(Character.toString(lonS));
}
private String getTimestampString() {
  return ""+timestamp+" GMT";
}
private String getRollString() {
  return "Roll: ".concat(String.format("%.2f", roll)).concat("");
}
private String getPitchString() {
  return "Pitch: ".concat(String.format("%.2f", pitch)).concat("");
}
private String getYawString() {
  return "Yaw: ".concat(String.format("%.2f", yaw)).concat("");
}

// get the date and time as a String:
private String getTime() {
  return sdf.format(new Date());
}


void onDeviceDiscovered(BlepdroidDevice device) {
  println("discovered device " + device.name + " address: " + device.address + " rssi: " + device.rssi );

  if (device.name != null && device.name.equals(deviceName)) {
    println("Device Found");

    if (blepdroid.connectDevice(device)) {
      println(" connected!");

      device1 = device;
    } else {
      println(" couldn't connect target device ");
    }
  }
}

void onServicesDiscovered(BlepdroidDevice device, int status) {

  HashMap<String, ArrayList<String>> servicesAndCharas = blepdroid.findAllServicesCharacteristics(device);
  for ( String service : servicesAndCharas.keySet()) {
    print( service + " has " );

    // this will list the UUIDs of each service, in the future we're going to make
    // this tell you more about each characteristic, e.g. whether it's readable or writable
    //println( servicesAndCharas.get(service));
    for (String charact : servicesAndCharas.get(service)) {
      print( " charact: " + charact);



      if (charact.equals(TACTIGON_QUAT_CHAR_STRING)) {
        targetCharacteristicFound = true;
        print( " target QUAT characteristic found! ");
        println("Registered UUID: " + TACTIGON_QUAT_UUID);
        blepdroid.setCharacteristicToListen(device, TACTIGON_QUAT_UUID);
      }
    }
  }
  // we want to set this for whatever device we just connected to
  //blepdroid.setCharacteristicToListen(device, RFDUINO_UUID_RECEIVE);

  allSetUp = true;
}
void onBluetoothRSSI(BlepdroidDevice device, int rssi) {
  println(" onBluetoothRSSI " + device.address + " " + Integer.toString(rssi));
}

void onBluetoothConnection( BlepdroidDevice device, int state) {
  blepdroid.discoverServices(device);
}

void onCharacteristicChanged(BlepdroidDevice device, String characteristic, byte[] data) {
  println("XXX onCharacteristicChanged " + characteristic);
  /** Aggiornare dati nuovi **/
  byte[] IDMexA = new byte[1];
  System.arraycopy(data, 14, IDMexA, 0, 1);



  if (IDMexA[0] == (byte) 0xAA) {
    println("  GPS");
    byte[] latA = new byte[4];
    byte[] latSA = new byte[1];
    byte[] lonA = new byte[4];
    byte[] lonSA = new byte[1];
    byte[] timestampA = new byte[4];
    System.arraycopy(data, 0, latA, 0, 4);
    System.arraycopy(data, 4, latSA, 0, 1);
    System.arraycopy(data, 5, lonA, 0, 4);
    System.arraycopy(data, 9, lonSA, 0, 1);
    System.arraycopy(data, 10, timestampA, 0, 4);
    lat = ByteBuffer.wrap(latA).order(ByteOrder.LITTLE_ENDIAN).getFloat();
    lon = ByteBuffer.wrap(lonA).order(ByteOrder.LITTLE_ENDIAN).getFloat();
    timestamp = ByteBuffer.wrap(timestampA).order(ByteOrder.LITTLE_ENDIAN).getFloat();
    latS = (char)latSA[0];
    lonS = (char)lonSA[0];
    println("   " + lat + " " + latS + " " + lon + " " + lonS + " " + timestamp);
    lastGPS = new GPSData(lat, latS, lon, lonS, timestamp);
    buffer.addData(lastGPS);
  } else if (IDMexA[0] == 0x55) {
    println("  IMU");
    byte[] rollA = new byte[4];
    byte[] pitchA = new byte[4];
    byte[] yawA = new byte[4];
    System.arraycopy(data, 0, rollA, 0, 4);
    System.arraycopy(data, 4, pitchA, 0, 4);
    System.arraycopy(data, 8, yawA, 0, 4);
    roll = ByteBuffer.wrap(rollA).order(ByteOrder.LITTLE_ENDIAN).getFloat();
    pitch = ByteBuffer.wrap(pitchA).order(ByteOrder.LITTLE_ENDIAN).getFloat();
    yaw = ByteBuffer.wrap(yawA).order(ByteOrder.LITTLE_ENDIAN).getFloat();

    roll = (roll * 180)/(float)Math.PI;

    pitch =  (pitch * 180)/(float)Math.PI;

    yaw =  (yaw * 180)/(float)Math.PI;

    println("   Roll: "+roll+" Pitch: "+ pitch + " Yaw: "+yaw);
    if (lastGPS == null) {
      println("lastGPS == null");
    } else {
      lastGPS.addIMUData(new IMUData(roll, pitch, yaw));
      println("lastGPS.addIMUData()");
    }
  } else
  {
    println("  UNKNOWN: " + IDMexA[0]);
  }
}

void onDescriptorWrite(BlepdroidDevice device, String characteristic, String data) {
  println(" onDescriptorWrite " + characteristic + " " + data);
}

void onDescriptorRead(BlepdroidDevice device, String characteristic, String data) {
  println(" onDescriptorRead " + characteristic + " " + data);
}

void onCharacteristicRead(BlepdroidDevice device, String characteristic, byte[] data) {
  println(" onCharacteristicRead " + characteristic + " " + data);
}

void onCharacteristicWrite(BlepdroidDevice device, String characteristic, byte[] data) {
  println(" onCharacteristicWrite " + characteristic + " " + data);
}

import java.util.*;
import java.io.FileOutputStream;
import android.content.Context;
import android.os.Environment;
import java.io.*;


public class Buffer {
  private int size;
  private String path;
  private String filename;
  private PrintWriter out;
 
  private List<GPSData> DataList = new ArrayList<GPSData>();

  public Buffer(int size) {
    this.size = size;
    this.filename="TheTactigonGPS.txt";
    this.path = "//sdcard//TheTactigon//";
    this.open();
   
  }
  public Buffer (int size, String filename) {
    this.size = size;
    this.filename = filename;
    println(filename);
    this.path="//sdcard//TheTactigon//";
        this.open();
  }

  public Buffer(int size, String filename, String path) {
    this.size = size;
    this.path = path;
    this.filename=filename;
        this.open();
  }
  
  public void open(){
    if(OpenFile()){
      //Write the tags to open
    out.println("<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n<Document>\n\t<name>"+this.filename+"</name>\n");
        out.flush();
    out.close();
    }

    
  }
  public void close() {
    //Write the tags to close
    if (OpenFile()){
      out.println("</Document>\n</kml>");
          out.flush();
    out.close();
    }

  }

  public void addData(GPSData data) {
    if (DataList.size()<size) {
      DataList.add(data);
    } else {
      DataList.add(data);
      //Write to path
      if (OpenFile()) {
        for (GPSData d : DataList) {
          out.println(d.toKML());
        }
        out.flush();
        out.close();
        DataList.clear();
      }
    }
  }

  private void createFile(File f) {
    File parentDir = f.getParentFile();
    try {
      parentDir.mkdirs(); 
      f.createNewFile();
    }
    catch(Exception e) {
      e.printStackTrace();
    }
  }

  private boolean OpenFile() {
    boolean fileOpened = false;
    try {

      File f = new File(dataPath(path+filename));
      File dir = new File(dataPath(path));
      if (!dir.exists()) {
        dir.mkdir();
      }
      if (!f.exists()) {  
        createFile(f);
      }

      try {
        out = new PrintWriter(new BufferedWriter(new FileWriter(f, true)));
       // out.println("Open!!###\n");
        fileOpened = true;
        //out.close();
      }
      catch (IOException e) {
        e.printStackTrace();
      }
    }  
    catch(Exception exc) {
      print("Eccezione apertura file! " + exc.getMessage());
    }
    return fileOpened;
  }

}

public interface Data {
  public String toKML();
}

public class GPSData implements Data{

  private float altitude;
  private float lat;
  private float lon;
  private float timestamp;
  private char latS;
  private char lonS;

  private List<Data> IMU;
  public GPSData(float lat, char latS, float lon, char lonS, float timestamp) {

    IMU = new ArrayList<Data>();
    this.latS = latS;
    this.lat = lat;
    this.lonS = lonS; 
    this.lon = lon;
    this.timestamp = timestamp;
    this.altitude = 0;
  }
  public GPSData(float lat, char latS, float lon, char lonS, float timestamp, float altitude) {
    IMU = new ArrayList<Data>();
    this.latS = latS;
    this.lat = lat;
    this.lonS = lonS; 
    this.lon = lon;
    this.timestamp = timestamp;
    this.altitude=altitude;
  }
  public void addIMUData(IMUData imu) {
    IMU.add(imu);
  }



  public float getDDLat() {
    float ret;
    ret = Utils.NMEAtoDD(lat);
    if (latS == 'S') {
      ret = -(ret);
    }
    return ret;
  }
  public float getDDLon() {
    float ret;
    ret = Utils.NMEAtoDD(lon);
    if (lonS == 'W') {
      ret = -(ret);
    }
    return ret;
  }
  public float getAltitude() {
    return this.altitude;
  }

  public String toXML() {
    String ret = "";
    ret+="<GPS>\n";
    ret+="\t<lat>"+lat+"</lat>\n";
    //   ret+="\t<latS>"+latS+"</latS>\n";
    ret+="\t<lon>"+lon+"</lon>\n";
    //    ret+="\t<lonS>"+lonS+"</lonS>\n";
    ret+="\t<timestamp>"+timestamp+"</timestamp>\n";
    ret+="</GPS>\n";
    return ret;
  }

  public String toKML() {
    String ret = "";
    ret+="\t<Placemark>\n";
    ret+="\t\t<name>"+timestamp+"</name>\n";
    if (!IMU.isEmpty()) {
      ret+="\t\t<ExtendedData>\n";
      for (Data i : IMU) {
        ret+=i.toKML();
      }
      ret+="\t\t</ExtendedData>\n";
    }
    ret+="\t\t<Point>\n";
    ret+="\t\t\t<coordinates>"+this.getDDLon()+","+this.getDDLat()+","+this.getAltitude()+"</coordinates>\n";
    ret+="\t\t</Point>\n";
    ret+="\t</Placemark>\n";
    return ret;
  }
}

public class IMUData implements Data{
  private  float roll;
  private float pitch;
  private float yaw;
  
  public IMUData(float roll, float pitch, float yaw) {
    this.roll = roll;
    this.pitch = pitch;
    this.yaw = yaw;
  }
  public float getRoll() {
    return this.roll;
  }
  public float getPitch() {
    return this.pitch;
  }
  public float getYaw() {
    return this.yaw;
  }
  public String toXML() {
    String ret = "";
    ret+="<IMU>\n";
    ret+="\t<roll>"+roll+"</roll>\n";
    ret+="\t<pitch>"+pitch+"</pitch>\n";
    ret+="\t<yaw>"+yaw+"</yaw>\n";
    ret+="</IMU>\n";
    return ret;
  }

  public String toKML() {
    String ret = "";
    ret+="\t\t\t<Data name=\"Roll\">\n";
    ret+="\t\t\t\t<value>"+this.roll+"</value>\n";
    ret+="\t\t\t</Data>\n";
    ret+="\t\t\t<Data name=\"Pitch\">\n";
    ret+="\t\t\t\t<value>"+this.pitch+"</value>\n";
    ret+="\t\t\t</Data>\n";
    ret+="\t\t\t<Data name=\"Yaw\">\n";
    ret+="\t\t\t\t<value>"+this.yaw+"</value>\n";
    ret+="\t\t\t</Data>\n";
    return ret;
  }
}

public static class Utils {
  public static String NMEAtoDMm(float data) {
    String ret = "";
    int deg = (int)data/100;
    ret+=deg;
    ret+=" ";
    ret+=String.format("%.8f", data-(deg*100));
    ret+="' ";
    return ret;
  }
  
  public static float NMEAtoDD(float data){
    String ret = "";
    float deg = (int)data/100;
    //println(deg);
    deg+=((data-(deg*100))/60);
    ret+=String.format("%.8f",deg);
    ret.replace(',','.');
   // println(data +" " +deg);
    return deg;
    
  }
}

<?xml version="1.0" encoding="UTF-8"?>
<manifest xmlns:android="http://schemas.android.com/apk/res/android" android:versionCode="1" android:versionName="1.0" package="">
  <uses-sdk android:minSdkVersion="15" android:targetSdkVersion="24"/>
  <application android:debuggable="true" android:icon="@drawable/icon" android:label="">
    <activity android:name=".MainActivity" android:theme="@android:style/Theme.NoTitleBar">
      <intent-filter>
        <action android:name="android.intent.action.MAIN"/>
        <category android:name="android.intent.category.LAUNCHER"/>
      </intent-filter>
    </activity>
    <service android:name="blepdroid.BluetoothLeService"/>
  </application>
  <uses-permission android:name="android.permission.ACCESS_COARSE_LOCATION"/>
  <uses-permission android:name="android.permission.ACCESS_FINE_LOCATION"/>
  <uses-permission android:name="android.permission.BLUETOOTH"/>
  <uses-permission android:name="android.permission.BLUETOOTH_ADMIN"/>
  <uses-permission android:name="android.permission.READ_EXTERNAL_STORAGE"/>
  <uses-permission android:name="android.permission.WAKE_LOCK"/>
  <uses-permission android:name="android.permission.WRITE_EXTERNAL_STORAGE"/>
</manifest>

mode=Android
mode.id=processing.mode.android.AndroidMode