Skip to content

Commit 8e99e47

Browse files
platypiiplatypii
committed
Vectronix Terrapin-X laser rangefinder protocol
1 parent 1d830c4 commit 8e99e47

File tree

3 files changed

+286
-0
lines changed

3 files changed

+286
-0
lines changed

‎app/src/main/java/com/platypii/baseline/lasers/rangefinder/Crc16.java

+50
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
package com.platypii.baseline.lasers.rangefinder;
2+
3+
class Crc16 {
4+
/**
5+
* Computes the CRC16 checksum for a given byte array.
6+
*/
7+
public static short crc16(byte[] byteArray) {
8+
int crc = 65535;
9+
for (byte b : byteArray) {
10+
int index = (crc ^ (b & 0xff)) & 0xff;
11+
crc = (crc >> 8) ^ CRC16_LOOKUP_TABLE[index];
12+
}
13+
return (short) crc;
14+
}
15+
16+
private static final int[] CRC16_LOOKUP_TABLE = {
17+
0, 4489, 8978, 12955, 17956, 22445, 25910, 29887,
18+
35912, 40385, 44890, 48851, 51820, 56293, 59774, 63735,
19+
4225, 264, 13203, 8730, 22181, 18220, 30135, 25662,
20+
40137, 36160, 49115, 44626, 56045, 52068, 63999, 59510,
21+
8450, 12427, 528, 5017, 26406, 30383, 17460, 21949,
22+
44362, 48323, 36440, 40913, 60270, 64231, 51324, 55797,
23+
12675, 8202, 4753, 792, 30631, 26158, 21685, 17724,
24+
48587, 44098, 40665, 36688, 64495, 60006, 55549, 51572,
25+
16900, 21389, 24854, 28831, 1056, 5545, 10034, 14011,
26+
52812, 57285, 60766, 64727, 34920, 39393, 43898, 47859,
27+
21125, 17164, 29079, 24606, 5281, 1320, 14259, 9786,
28+
57037, 53060, 64991, 60502, 39145, 35168, 48123, 43634,
29+
25350, 29327, 16404, 20893, 9506, 13483, 1584, 6073,
30+
61262, 65223, 52316, 56789, 43370, 47331, 35448, 39921,
31+
29575, 25102, 20629, 16668, 13731, 9258, 5809, 1848,
32+
65487, 60998, 56541, 52564, 47595, 43106, 39673, 35696,
33+
33800, 38273, 42778, 46739, 49708, 54181, 57662, 61623,
34+
2112, 6601, 11090, 15067, 20068, 24557, 28022, 31999,
35+
38025, 34048, 47003, 42514, 53933, 49956, 61887, 57398,
36+
6337, 2376, 15315, 10842, 24293, 20332, 32247, 27774,
37+
42250, 46211, 34328, 38801, 58158, 62119, 49212, 53685,
38+
10562, 14539, 2640, 7129, 28518, 32495, 19572, 24061,
39+
46475, 41986, 38553, 34576, 62383, 57894, 53437, 49460,
40+
14787, 10314, 6865, 2904, 32743, 28270, 23797, 19836,
41+
50700, 55173, 58654, 62615, 32808, 37281, 41786, 45747,
42+
19012, 23501, 26966, 30943, 3168, 7657, 12146, 16123,
43+
54925, 50948, 62879, 58390, 37033, 33056, 46011, 41522,
44+
23237, 19276, 31191, 26718, 7393, 3432, 16371, 11898,
45+
59150, 63111, 50204, 54677, 41258, 45219, 33336, 37809,
46+
27462, 31439, 18516, 23005, 11618, 15595, 3696, 8185,
47+
63375, 58886, 54429, 50452, 45483, 40994, 37561, 33584,
48+
31687, 27214, 22741, 18780, 15843, 11370, 7921, 3960
49+
};
50+
}

‎app/src/main/java/com/platypii/baseline/lasers/rangefinder/RangefinderService.java

+1
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@ public class RangefinderService {
3030
private final BleProtocol protocols[] = {
3131
new ATNProtocol(),
3232
new SigSauerProtocol(),
33+
new TerrapinProtocol(),
3334
new UineyeProtocol()
3435
};
3536

‎app/src/main/java/com/platypii/baseline/lasers/rangefinder/TerrapinProtocol.java

+235
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,235 @@
1+
package com.platypii.baseline.lasers.rangefinder;
2+
3+
import com.platypii.baseline.bluetooth.BleException;
4+
import com.platypii.baseline.bluetooth.BleProtocol;
5+
import com.platypii.baseline.lasers.LaserMeasurement;
6+
import com.platypii.baseline.util.Exceptions;
7+
8+
import android.bluetooth.le.ScanRecord;
9+
import android.os.ParcelUuid;
10+
import android.util.Log;
11+
import androidx.annotation.NonNull;
12+
import androidx.annotation.Nullable;
13+
import com.welie.blessed.BluetoothPeripheral;
14+
import com.welie.blessed.WriteType;
15+
import java.util.Arrays;
16+
import java.util.List;
17+
import java.util.UUID;
18+
import org.greenrobot.eventbus.EventBus;
19+
20+
import static com.platypii.baseline.bluetooth.BluetoothUtil.byteArrayToHex;
21+
import static com.platypii.baseline.bluetooth.BluetoothUtil.toManufacturerString;
22+
23+
/**
24+
* This class contains ids, commands, and decoders for Vectronix Terrapin-X laser rangefinders.
25+
*/
26+
class TerrapinProtocol extends BleProtocol {
27+
private static final String TAG = "TerrapinProtocol";
28+
29+
// Manufacturer ID
30+
private static final int manufacturerId1 = 1164;
31+
private static final byte[] manufacturerData1 = {1, -96, -1, -1, -1, -1, 0}; // 01-a0-ff-ff-ff-ff-00
32+
33+
// Terrapin service
34+
private static final UUID terrapinService = UUID.fromString("81480000-b0b7-4074-8a24-ae554e5cdbc4");
35+
// Terrapin characteristic: read, indicate
36+
private static final UUID terrapinCharacteristic1 = UUID.fromString("81480100-b0b7-4074-8a24-ae554e5cdbc4");
37+
// Terrapin characteristic: notify, write
38+
private static final UUID terrapinCharacteristic2 = UUID.fromString("81480200-b0b7-4074-8a24-ae554e5cdbc4");
39+
40+
private static final String factoryModeSecretKey = "b6987833";
41+
42+
// Terrapin packet types
43+
private static final short packetTypeCommand = 0x0000;
44+
private static final short packetTypeData = 0x0300;
45+
private static final short packetTypeAck = 0x0400;
46+
private static final short packetTypeNack = 0x0500;
47+
48+
// Terrapin commands
49+
private static final short commandNewMeasurementAvailable = 0x0010;
50+
private static final short commandStartMeasurement = 0x0110;
51+
private static final short commandGetLastRange = 0x0210;
52+
private static final short commandGetLastInclination = 0x0310;
53+
private static final short commandGetLastDirection = 0x0410;
54+
private static final short commandGetLastTemperature = 0x0510;
55+
private static final short commandGetLastPressure = 0x0610;
56+
private static final short commandGetLastEHR = 0x0710;
57+
58+
// private static final short commandGetComVersion = 0x0100;
59+
// private static final short commandGetSupportedCommandSet = 0x0200;
60+
// private static final short commandGetSerialNumber = 0x0300;
61+
// private static final short commandActivateFactoryMode = 0x0400;
62+
// private static final short commandGetHardwareRevision = 0x0500;
63+
// private static final short commandGetFirmwareVersion = 0x0600;
64+
// private static final short commandGetBatteryLevel = 0x0700;
65+
// private static final short commandGetDeviceName = 0x0800;
66+
// private static final short commandSetDeviceName = 0x0900;
67+
// private static final short commandGetDeviceId = 0x0a00;
68+
69+
@Override
70+
public void onServicesDiscovered(@NonNull BluetoothPeripheral peripheral) {
71+
try {
72+
// Request rangefinder service
73+
Log.i(TAG, "app -> rf: subscribe");
74+
peripheral.setNotify(terrapinService, terrapinCharacteristic1, true);
75+
} catch (Throwable e) {
76+
Log.e(TAG, "rangefinder handshake exception", e);
77+
}
78+
}
79+
80+
@Override
81+
public void processBytes(@NonNull BluetoothPeripheral peripheral, @NonNull byte[] value) {
82+
Log.d(TAG, "rf -> app: process " + byteArrayToHex(value));
83+
if (value[0] != 0x7e || value[value.length - 1] != 0x7e) {
84+
Log.w(TAG, "rf -> app: invalid command " + byteArrayToHex(value));
85+
return;
86+
}
87+
88+
// Remove frame
89+
byte[] frame = Arrays.copyOfRange(value, 1, value.length - 1);
90+
// Unescape special characters 0x7e and 0x7d
91+
frame = unescape(frame);
92+
93+
// Check checksum
94+
final int checksum = frame[frame.length - 2] + (frame[frame.length - 1] << 8);
95+
if (Crc16.crc16(frame) != checksum) {
96+
Log.w(TAG, "rf -> app: invalid checksum " + byteArrayToHex(frame) + " " + Integer.toHexString(Crc16.crc16(frame)) + " != " + Integer.toHexString(checksum));
97+
}
98+
99+
// Packet types
100+
if (frame[0] + (frame[1] << 8) == packetTypeCommand) {
101+
// Data length
102+
int dataLength = frame[3] + (frame[2] << 8);
103+
if (dataLength == 512) dataLength = 0;
104+
// Command
105+
final int command = frame[5] + (frame[4] << 8);
106+
final byte[] data = Arrays.copyOfRange(frame, 6, frame.length - 2);
107+
if (command == commandNewMeasurementAvailable) {
108+
Log.i(TAG, "rf -> app: new measurement available " + byteArrayToHex(data));
109+
getLastRange(peripheral);
110+
} else {
111+
Log.w(TAG, "rf -> app: command unknown 0x" + Integer.toHexString(command) + " " + dataLength + " " + byteArrayToHex(data));
112+
}
113+
} else if (frame[0] + (frame[1] << 8) == packetTypeData) {
114+
Log.i(TAG, "rf -> app: data " + byteArrayToHex(frame));
115+
} else if (frame[0] + (frame[1] << 8) == packetTypeAck) {
116+
Log.i(TAG, "rf -> app: ack " + byteArrayToHex(frame));
117+
} else if (frame[0] + (frame[1] << 8) == packetTypeNack) {
118+
Log.i(TAG, "rf -> app: nack " + byteArrayToHex(frame));
119+
} else {
120+
Log.w(TAG, "rf -> app: unknown " + byteArrayToHex(frame));
121+
}
122+
}
123+
124+
private void processMeasurement(@NonNull byte[] value) {
125+
Log.d(TAG, "rf -> app: measure " + byteArrayToHex(value));
126+
// TODO
127+
EventBus.getDefault().post(new LaserMeasurement(0, 0));
128+
}
129+
private void startMeasurement(@NonNull BluetoothPeripheral peripheral) {
130+
Log.i(TAG, "app -> rf: start measurement");
131+
sendCommand(peripheral, commandStartMeasurement, null);
132+
}
133+
134+
private void getLastRange(@NonNull BluetoothPeripheral peripheral) {
135+
Log.i(TAG, "app -> rf: get last range");
136+
sendCommand(peripheral, commandGetLastRange, null);
137+
}
138+
139+
private void sendCommand(@NonNull BluetoothPeripheral peripheral, short command, @Nullable byte[] data) {
140+
final int dataLength = data == null ? 0 : data.length; // TODO: / 2 ?
141+
byte[] frame = new byte[dataLength + 6];
142+
// Packet type
143+
frame[0] = (byte) (packetTypeCommand & 0xff);
144+
frame[1] = (byte) ((packetTypeCommand >> 8) & 0xff);
145+
// Data length
146+
if (data != null) {
147+
frame[2] = (byte) (dataLength & 0xff);
148+
frame[3] = (byte) ((dataLength >> 8) & 0xff);
149+
System.arraycopy(data, 0, frame, 6, data.length);
150+
} else {
151+
frame[2] = 2;
152+
frame[3] = 0;
153+
}
154+
// Command
155+
frame[4] = (byte) (command & 0xff);
156+
frame[5] = (byte) ((command >> 8) & 0xff);
157+
// Data
158+
// Checksum
159+
final int checksum = Crc16.crc16(Arrays.copyOfRange(frame, 1, 7));
160+
frame[frame.length - 2] = (byte) (checksum & 0xff);
161+
frame[frame.length - 1] = (byte) ((checksum >> 8) & 0xff);
162+
// Escape special characters 0x7e and 0x7d
163+
frame = escape(frame);
164+
// Wrap frame
165+
byte[] wrapped = new byte[frame.length + 2];
166+
wrapped[0] = 0x7e;
167+
System.arraycopy(frame, 0, wrapped, 1, frame.length);
168+
wrapped[wrapped.length - 1] = 0x7e;
169+
Log.d(TAG, "app -> rf: send command " + byteArrayToHex(wrapped));
170+
peripheral.writeCharacteristic(terrapinService, terrapinCharacteristic2, wrapped, WriteType.WITH_RESPONSE);
171+
}
172+
173+
/**
174+
* Return true iff a bluetooth scan result looks like a rangefinder
175+
*/
176+
@Override
177+
public boolean canParse(@NonNull BluetoothPeripheral peripheral, @Nullable ScanRecord record) {
178+
final String deviceName = peripheral.getName();
179+
if (record != null && Arrays.equals(record.getManufacturerSpecificData(manufacturerId1), manufacturerData1)) {
180+
return true; // Manufacturer match (kenny's laser)
181+
} else if (
182+
(record != null && hasRangefinderService(record))
183+
|| deviceName.startsWith("FastM")
184+
|| deviceName.startsWith("Terrapin")) {
185+
// Send manufacturer data to firebase
186+
final String mfg = toManufacturerString(record);
187+
Exceptions.report(new BleException("Terrapin laser unknown mfg data: " + deviceName + " " + mfg));
188+
return true;
189+
} else {
190+
return false;
191+
}
192+
}
193+
194+
private boolean hasRangefinderService(@NonNull ScanRecord record) {
195+
final List<ParcelUuid> uuids = record.getServiceUuids();
196+
return uuids != null && uuids.contains(new ParcelUuid(terrapinService));
197+
}
198+
199+
private byte[] escape(byte[] data) {
200+
final byte[] escaped = new byte[data.length * 2];
201+
int j = 0;
202+
for (byte b : data) {
203+
if (b == 0x7e) {
204+
escaped[j++] = 0x7d;
205+
escaped[j++] = 0x5e;
206+
} else if (b == 0x7d) {
207+
escaped[j++] = 0x7d;
208+
escaped[j++] = 0x5d;
209+
} else {
210+
escaped[j++] = b;
211+
}
212+
}
213+
return Arrays.copyOf(escaped, j);
214+
}
215+
216+
private byte[] unescape(byte[] data) {
217+
final byte[] unescaped = new byte[data.length];
218+
int j = 0;
219+
for (int i = 0; i < data.length; i++) {
220+
if (data[i] == 0x7d) {
221+
if (data[i + 1] == 0x5e) {
222+
unescaped[j++] = 0x7e;
223+
} else if (data[i + 1] == 0x5d) {
224+
unescaped[j++] = 0x7d;
225+
} else {
226+
Log.w(TAG, "rf -> app: invalid escape sequence " + byteArrayToHex(data));
227+
}
228+
i++;
229+
} else {
230+
unescaped[j++] = data[i];
231+
}
232+
}
233+
return Arrays.copyOf(unescaped, j);
234+
}
235+
}

0 commit comments

Comments
 (0)