1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
|
package org.traccar.helper;
import java.util.HashMap;
import java.util.Map;
public class NMEA {
// fucking java interfaces
interface SentenceParser {
public boolean parse(String[] tokens, GPSPosition position);
}
// utils
static float Latitude2Decimal(String lat, String NS) {
float med = Float.parseFloat(lat.substring(2)) / 60.0f;
med += Float.parseFloat(lat.substring(0, 2));
if (NS.startsWith("S")) {
med = -med;
}
return med;
}
static float Longitude2Decimal(String lon, String WE) {
float med = Float.parseFloat(lon.substring(3)) / 60.0f;
med += Float.parseFloat(lon.substring(0, 3));
if (WE.startsWith("W")) {
med = -med;
}
return med;
}
// parsers
class GPGGA implements SentenceParser {
public boolean parse(String[] tokens, GPSPosition position) {
position.time = Float.parseFloat(tokens[1]);
position.lat = Latitude2Decimal(tokens[2], tokens[3]);
position.lon = Longitude2Decimal(tokens[4], tokens[5]);
position.quality = Integer.parseInt(tokens[6]);
position.altitude = Float.parseFloat(tokens[9]);
return true;
}
}
class GPGGL implements SentenceParser {
public boolean parse(String[] tokens, GPSPosition position) {
position.lat = Latitude2Decimal(tokens[1], tokens[2]);
position.lon = Longitude2Decimal(tokens[3], tokens[4]);
position.time = Float.parseFloat(tokens[5]);
return true;
}
}
class GPRMC implements SentenceParser {
public boolean parse(String[] tokens, GPSPosition position) {
position.time = Float.parseFloat(tokens[1]);
position.lat = Latitude2Decimal(tokens[3], tokens[4]);
position.lon = Longitude2Decimal(tokens[5], tokens[6]);
position.velocity = Float.parseFloat(tokens[7]);
position.dir = Float.parseFloat(tokens[8]);
return true;
}
}
class GPVTG implements SentenceParser {
public boolean parse(String[] tokens, GPSPosition position) {
position.dir = Float.parseFloat(tokens[3]);
return true;
}
}
class GPRMZ implements SentenceParser {
public boolean parse(String[] tokens, GPSPosition position) {
position.altitude = Float.parseFloat(tokens[1]);
return true;
}
}
public class GPSPosition {
public float time = 0.0f;
public float lat = 0.0f;
public float lon = 0.0f;
public boolean fixed = false;
public int quality = 0;
public float dir = 0.0f;
public float altitude = 0.0f;
public float velocity = 0.0f;
public void updatefix() {
fixed = quality > 0;
}
public String toString() {
return String.format("POSITION: lat: %f, lon: %f, time: %f, Q: %d, dir: %f, alt: %f, vel: %f", lat, lon, time, quality, dir, altitude, velocity);
}
}
GPSPosition position = new GPSPosition();
private static final Map<String, SentenceParser> sentenceParsers = new HashMap<String, SentenceParser>();
public NMEA() {
sentenceParsers.put("GPGGA", new GPGGA());
sentenceParsers.put("GPGGL", new GPGGL());
sentenceParsers.put("GPRMC", new GPRMC());
sentenceParsers.put("GPRMZ", new GPRMZ());
//only really good GPS devices have this sentence but ...
sentenceParsers.put("GPVTG", new GPVTG());
}
public GPSPosition parse(String line) {
if (line.startsWith("$")) {
String nmea = line.substring(1);
String[] tokens = nmea.split(",");
String type = tokens[0];
//TODO check crc
if (sentenceParsers.containsKey(type)) {
sentenceParsers.get(type).parse(tokens, position);
}
position.updatefix();
}
return position;
}
}
|
