129 lines
4.1 KiB
Java
129 lines
4.1 KiB
Java
package hendrikschutter.com.externgnss;
|
|
|
|
import android.util.Log;
|
|
|
|
import java.io.BufferedReader;
|
|
import java.io.ByteArrayInputStream;
|
|
import java.io.ByteArrayOutputStream;
|
|
import java.io.IOException;
|
|
import java.io.InputStreamReader;
|
|
import java.nio.charset.StandardCharsets;
|
|
import java.util.Iterator;
|
|
import java.util.List;
|
|
import java.util.concurrent.BlockingQueue;
|
|
|
|
public class NMEAParser {
|
|
private double latitude;
|
|
private double longitude;
|
|
private boolean fix;
|
|
private int satCount;
|
|
private int sentenceCounter;
|
|
private BlockingQueue<Byte> rawByteData = null;
|
|
|
|
public NMEAParser() {
|
|
this.fix = false;
|
|
this.satCount = 0;
|
|
this.latitude = 0;
|
|
this.longitude = 0;
|
|
this.sentenceCounter = 0;
|
|
}
|
|
|
|
public void setReceiveByteStream(BlockingQueue<Byte> rawData) {
|
|
this.rawByteData = rawData;
|
|
}
|
|
|
|
/**
|
|
* find strings in raw data and sent them to parse
|
|
*/
|
|
public void handleReceiveByteStream() {
|
|
String nmeaSentence;
|
|
ByteArrayOutputStream rawDataByteStream = new ByteArrayOutputStream();
|
|
|
|
//wait for sufficient data in buffer TODO don't do this block wise, losing data in edge
|
|
if (this.rawByteData.size() > 512) {
|
|
for (Iterator<Byte> rawByte = this.rawByteData.iterator(); rawByte.hasNext(); ) {
|
|
rawDataByteStream.write(rawByte.next());
|
|
rawByte.remove(); //remove this data from stream
|
|
}
|
|
ByteArrayInputStream stream = new ByteArrayInputStream(rawDataByteStream.toByteArray());
|
|
InputStreamReader streamReader = new InputStreamReader(stream, StandardCharsets.US_ASCII);
|
|
BufferedReader bufferedReader = new BufferedReader(streamReader);
|
|
|
|
try {
|
|
while ((nmeaSentence = bufferedReader.readLine()) != null) {
|
|
//Log.i("NMEAParser", nmeaSentence);
|
|
this.parseSentence(nmeaSentence);
|
|
}
|
|
} catch (IOException e) {
|
|
e.printStackTrace();
|
|
}
|
|
} else {
|
|
//Log.i("NMEAParser", "wait for more data in buffer");
|
|
}
|
|
}
|
|
|
|
public int getSentenceCounter(){
|
|
return this.sentenceCounter;
|
|
}
|
|
|
|
public void parseSentence(String sentence) {
|
|
this.sentenceCounter++;
|
|
if (sentence.startsWith("$GNGLL")) {
|
|
parseGNGLL(sentence);
|
|
}
|
|
if (sentence.startsWith("$GPGSV")) {
|
|
parseGPGSV(sentence);
|
|
}
|
|
}
|
|
|
|
private void parseGNGLL(String sentence) {
|
|
if (sentence.length() >= 45) {
|
|
if (sentence.substring(44, 45).equals("A")) {
|
|
this.fix = true;
|
|
this.latitude = (Double.parseDouble(sentence.substring(9, 17)) / 60) + Double.parseDouble(sentence.substring(7, 9));
|
|
this.longitude = (Double.parseDouble(sentence.substring(23, 30)) / 60) + Double.parseDouble(sentence.substring(20, 23));
|
|
} else {
|
|
this.fix = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
private void parseGPGSV(String sentence) {
|
|
if (sentence.length() >= 13) {
|
|
this.satCount = (Integer.parseInt(sentence.substring(11, 13)));
|
|
}
|
|
}
|
|
|
|
|
|
public double calcDistance(double latitude, double longitude) {
|
|
final int R = 6371; // Radius of the earth
|
|
|
|
double latDistance = Math.toRadians(latitude - this.latitude);
|
|
double lonDistance = Math.toRadians(longitude - this.longitude);
|
|
double a = Math.sin(latDistance / 2) * Math.sin(latDistance / 2)
|
|
+ Math.cos(Math.toRadians(this.latitude)) * Math.cos(Math.toRadians(latitude))
|
|
* Math.sin(lonDistance / 2) * Math.sin(lonDistance / 2);
|
|
double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
|
|
double distance = R * c * 1000; // convert to meters
|
|
distance = Math.pow(distance, 2);
|
|
return Math.sqrt(distance);
|
|
}
|
|
|
|
|
|
public double getLatitude() {
|
|
return this.latitude;
|
|
}
|
|
|
|
public double getLongitude() {
|
|
return this.longitude;
|
|
}
|
|
|
|
public boolean checkFix() {
|
|
return this.fix;
|
|
}
|
|
|
|
public int getSatCount() {
|
|
return this.satCount;
|
|
}
|
|
}
|