@@ -29,6 +29,9 @@ public class Flysight2Protocol extends BleProtocol {
29
29
private static final UUID flysightCharacteristicTX = UUID .fromString ("00000001-8e22-4541-9d4c-21edae82ed19" );
30
30
private static final UUID flysightCharacteristicRX = UUID .fromString ("00000002-8e22-4541-9d4c-21edae82ed19" );
31
31
32
+ private static final long gpsEpochMilliseconds = 315964800000L - 18000L ; // January 6, 1980 - 18s
33
+ private static final long millisecondsPerWeek = 604800000L ;
34
+
32
35
public Flysight2Protocol (@ NonNull PubSub <MLocation > locationUpdates ) {
33
36
this .locationUpdates = locationUpdates ;
34
37
}
@@ -56,14 +59,21 @@ public void onMtuChanged(@NonNull BluetoothPeripheral peripheral, int mtu, @NonN
56
59
public void processBytes (@ NonNull BluetoothPeripheral peripheral , @ NonNull byte [] value ) {
57
60
try {
58
61
final ByteBuffer buf = ByteBuffer .wrap (value ).order (ByteOrder .LITTLE_ENDIAN );
59
- final int iTow = buf .getInt (0 ); // gps time of week
62
+ final int tow = buf .getInt (0 ); // gps time of week
60
63
final double lng = buf .getInt (4 ) * 1e-7 ;
61
64
final double lat = buf .getInt (8 ) * 1e-7 ;
62
65
final double alt = buf .getInt (12 ) * 1e-3 ;
63
66
final double vN = buf .getInt (16 ) * 1e-3 ;
64
67
final double vE = buf .getInt (20 ) * 1e-3 ;
65
68
final double climb = buf .getInt (24 ) * -1e-3 ;
66
- final long millis = System .currentTimeMillis (); // TODO
69
+
70
+ // Calculate gps week from current system time
71
+ final long now = System .currentTimeMillis ();
72
+ final long gpsTime = now - gpsEpochMilliseconds ;
73
+ final long gpsWeek = gpsTime / millisecondsPerWeek ;
74
+ // TODO: Check if near the start or end of the week
75
+ // Calculate epoch time from time-of-week
76
+ final long millis = gpsWeek * millisecondsPerWeek + tow + gpsEpochMilliseconds ;
67
77
68
78
final int locationError = LocationCheck .validate (lat , lng );
69
79
if (locationError == LocationCheck .VALID ) {
0 commit comments