Skip to content

Commit 15dd5e7

Browse files
platypiiplatypii
committed
Vectronix Terrapin-X laser rangefinder protocol
1 parent 1d830c4 commit 15dd5e7

File tree

4 files changed

+367
-0
lines changed

4 files changed

+367
-0
lines changed

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

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
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 = 0xffff;
9+
10+
// Process bytes in pairs (LSB first)
11+
for (int i = 0; i < byteArray.length; i++) {
12+
int b = byteArray[i] & 0xff;
13+
crc ^= b;
14+
15+
// Process each bit
16+
for (int j = 0; j < 8; j++) {
17+
if ((crc & 0x0001) != 0) {
18+
crc = (crc >> 1) ^ 0x8408; // 0x8408 is reversed polynomial
19+
} else {
20+
crc = crc >> 1;
21+
}
22+
}
23+
}
24+
25+
// XOR with 0xfff
26+
crc ^= 0xffff;
27+
28+
return (short) crc;
29+
}
30+
}

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

Lines changed: 1 addition & 0 deletions
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/TerraSentenceIterator.java

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
package com.platypii.baseline.lasers.rangefinder;
2+
3+
import android.util.Log;
4+
import androidx.annotation.NonNull;
5+
import java.util.ArrayList;
6+
import java.util.Iterator;
7+
import java.util.List;
8+
9+
// Sentences start with 7e and end with 7e
10+
// Sometimes 1 sentence takes 2 messages
11+
// Sometimes 2 sentences come in 1 message
12+
class TerraSentenceIterator implements Iterator<byte[]> {
13+
private static final String TAG = "TerraSentenceIterator";
14+
15+
@NonNull
16+
private final List<Byte> byteBuffer = new ArrayList<>();
17+
18+
// Sentences ready to read
19+
@NonNull
20+
private final List<byte[]> sentences = new ArrayList<>();
21+
22+
private int state = 0;
23+
24+
void addBytes(@NonNull byte[] bytes) {
25+
for (byte b : bytes) {
26+
addByte(b);
27+
}
28+
}
29+
30+
private void addByte(byte b) {
31+
byteBuffer.add(b);
32+
if (state == 0) {
33+
if (b == 0x7e) state = 1;
34+
else Log.e(TAG, "invalid preamble");
35+
} else if (state == 1) {
36+
if (b == 0x7e) {
37+
addSentence();
38+
state = 0;
39+
}
40+
}
41+
}
42+
43+
private void addSentence() {
44+
byte[] sent = new byte[byteBuffer.size() - 2];
45+
for (int i = 0; i < byteBuffer.size() - 2; i++) {
46+
sent[i] = byteBuffer.get(i + 1);
47+
}
48+
sentences.add(sent);
49+
byteBuffer.clear();
50+
}
51+
52+
@Override
53+
public boolean hasNext() {
54+
return !sentences.isEmpty();
55+
}
56+
57+
@Override
58+
public byte[] next() {
59+
return sentences.remove(0);
60+
}
61+
62+
@Override
63+
public void remove() {
64+
throw new UnsupportedOperationException();
65+
}
66+
}

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

Lines changed: 270 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,270 @@
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 = 0x00;
44+
private static final short packetTypeData = 0x03;
45+
private static final short packetTypeAck = 0x04;
46+
private static final short packetTypeNack = 0x05;
47+
48+
// Terrapin commands
49+
private static final short commandNewMeasurementAvailable = 0x1000;
50+
private static final short commandStartMeasurement = 0x1001;
51+
private static final short commandGetLastRange = 0x1002;
52+
private static final short commandGetLastInclination = 0x1003;
53+
private static final short commandGetLastDirection = 0x1004;
54+
private static final short commandGetLastTemperature = 0x1005;
55+
private static final short commandGetLastPressure = 0x1006;
56+
private static final short commandGetLastEHR = 0x1007;
57+
private static final short commandGetLaserMode = 0x1022;
58+
private static final short commandGetDeclination = 0x1027;
59+
private static final short commandGetRangeGate = 0x1028;
60+
// private static final short commandGetLastSNR = 0xf006;
61+
62+
// private static final short commandGetComVersion = 0x01;
63+
// private static final short commandGetSupportedCommandSet = 0x02;
64+
// private static final short commandGetSerialNumber = 0x03;
65+
// private static final short commandActivateFactoryMode = 0x04;
66+
// private static final short commandGetHardwareRevision = 0x05;
67+
// private static final short commandGetFirmwareVersion = 0x06;
68+
// private static final short commandGetBatteryLevel = 0x07;
69+
// private static final short commandGetDeviceName = 0x08;
70+
// private static final short commandSetDeviceName = 0x09;
71+
// private static final short commandGetDeviceId = 0x0a;
72+
73+
@NonNull
74+
private final TerraSentenceIterator sentenceIterator = new TerraSentenceIterator();
75+
76+
@Override
77+
public void onServicesDiscovered(@NonNull BluetoothPeripheral peripheral) {
78+
try {
79+
// Request rangefinder service
80+
Log.i(TAG, "app -> rf: subscribe");
81+
peripheral.setNotify(terrapinService, terrapinCharacteristic1, true);
82+
} catch (Throwable e) {
83+
Log.e(TAG, "rangefinder handshake exception", e);
84+
}
85+
}
86+
87+
@Override
88+
public void processBytes(@NonNull BluetoothPeripheral peripheral, @NonNull byte[] value) {
89+
Log.d(TAG, "rf -> app: processBytes " + byteArrayToHex(value));
90+
sentenceIterator.addBytes(value);
91+
while (sentenceIterator.hasNext()) {
92+
processSentence(peripheral, sentenceIterator.next());
93+
}
94+
}
95+
96+
private void processSentence(@NonNull BluetoothPeripheral peripheral, @NonNull byte[] value) {
97+
Log.d(TAG, "rf -> app: processSentence " + byteArrayToHex(value));
98+
99+
// Unescape special characters 0x7e and 0x7d
100+
byte[] frame = unescape(value);
101+
102+
// Check checksum
103+
final int checksumValue = getShort(frame, frame.length - 2);
104+
byte[] checksumContent = Arrays.copyOfRange(frame, 0, frame.length - 2);
105+
final short checksumComputed = Crc16.crc16(checksumContent);
106+
if (checksumValue != checksumComputed) {
107+
Log.w(TAG, "rf -> app: invalid checksum " + byteArrayToHex(checksumContent) + " " + shortToHex(checksumValue) + " != " + shortToHex(checksumComputed));
108+
}
109+
110+
// Packet types
111+
final short packetType = getShort(frame, 0);
112+
// Data length
113+
int dataLength = getShort(frame, 2);
114+
if (dataLength == 512) dataLength = 0;
115+
// Command
116+
final int command = getShort(frame, 4);
117+
if (packetType == packetTypeCommand) {
118+
final byte[] data = Arrays.copyOfRange(frame, 6, frame.length - 2);
119+
if (command == commandNewMeasurementAvailable) {
120+
Log.i(TAG, "rf -> app: new measurement available");
121+
getLastRange(peripheral);
122+
} else {
123+
Log.w(TAG, "rf -> app: command unknown 0x" + shortToHex(command) + " " + dataLength + " " + byteArrayToHex(data));
124+
}
125+
} else if (packetType == packetTypeData) {
126+
Log.i(TAG, "rf -> app: data " + byteArrayToHex(frame));
127+
} else if (packetType == packetTypeAck) {
128+
Log.i(TAG, "rf -> app: ack " + byteArrayToHex(frame));
129+
} else if (packetType == packetTypeNack) {
130+
Log.i(TAG, "rf -> app: nack " + dataLength + " " + shortToHex(command) + " " + byteArrayToHex(frame));
131+
} else {
132+
Log.w(TAG, "rf -> app: unknown packet type " + shortToHex(packetType) + " " + byteArrayToHex(frame));
133+
}
134+
}
135+
136+
private String shortToHex(int value) {
137+
return Integer.toHexString(value & 0xffff);
138+
}
139+
140+
private void processMeasurement(@NonNull byte[] value) {
141+
Log.d(TAG, "rf -> app: measure " + byteArrayToHex(value));
142+
// TODO
143+
EventBus.getDefault().post(new LaserMeasurement(0, 0));
144+
}
145+
private void startMeasurement(@NonNull BluetoothPeripheral peripheral) {
146+
Log.i(TAG, "app -> rf: start measurement");
147+
sendCommand(peripheral, commandStartMeasurement, null);
148+
}
149+
150+
private void getLastRange(@NonNull BluetoothPeripheral peripheral) {
151+
Log.i(TAG, "app -> rf: get last range");
152+
sendCommand(peripheral, commandGetLastRange, null);
153+
}
154+
155+
private void sendCommand(@NonNull BluetoothPeripheral peripheral, short command, @Nullable byte[] data) {
156+
final int dataLength = data == null ? 0 : data.length; // TODO: / 2 ?
157+
byte[] frame = new byte[8 + dataLength];
158+
// Packet type
159+
frame[0] = (byte) (packetTypeCommand & 0xff);
160+
frame[1] = (byte) ((packetTypeCommand >> 8) & 0xff);
161+
// Data length
162+
if (data != null) {
163+
frame[2] = (byte) (dataLength & 0xff);
164+
frame[3] = (byte) ((dataLength >> 8) & 0xff);
165+
System.arraycopy(data, 0, frame, 6, data.length);
166+
} else {
167+
frame[2] = 2;
168+
frame[3] = 0;
169+
}
170+
// Command
171+
frame[4] = (byte) (command & 0xff);
172+
frame[5] = (byte) ((command >> 8) & 0xff);
173+
// Checksum
174+
final int checksum = Crc16.crc16(Arrays.copyOfRange(frame, 0, frame.length - 2));
175+
frame[frame.length - 1] = (byte) ((checksum >> 8) & 0xff);
176+
frame[frame.length - 2] = (byte) (checksum & 0xff);
177+
// Escape special characters 0x7e and 0x7d
178+
frame = escape(frame);
179+
// Wrap frame
180+
byte[] wrapped = new byte[frame.length + 2];
181+
wrapped[0] = 0x7e;
182+
System.arraycopy(frame, 0, wrapped, 1, frame.length);
183+
wrapped[wrapped.length - 1] = 0x7e;
184+
Log.d(TAG, "app -> rf: send command " + byteArrayToHex(wrapped));
185+
peripheral.writeCharacteristic(terrapinService, terrapinCharacteristic2, wrapped, WriteType.WITH_RESPONSE);
186+
}
187+
188+
/**
189+
* Return true iff a bluetooth scan result looks like a rangefinder
190+
*/
191+
@Override
192+
public boolean canParse(@NonNull BluetoothPeripheral peripheral, @Nullable ScanRecord record) {
193+
final String deviceName = peripheral.getName();
194+
if (record != null && Arrays.equals(record.getManufacturerSpecificData(manufacturerId1), manufacturerData1)) {
195+
return true; // Manufacturer match (kenny's laser)
196+
} else if (
197+
(record != null && hasRangefinderService(record))
198+
|| deviceName.startsWith("FastM")
199+
|| deviceName.startsWith("Terrapin")) {
200+
// Send manufacturer data to firebase
201+
final String mfg = toManufacturerString(record);
202+
Exceptions.report(new BleException("Terrapin laser unknown mfg data: " + deviceName + " " + mfg));
203+
return true;
204+
} else {
205+
return false;
206+
}
207+
}
208+
209+
private boolean hasRangefinderService(@NonNull ScanRecord record) {
210+
final List<ParcelUuid> uuids = record.getServiceUuids();
211+
return uuids != null && uuids.contains(new ParcelUuid(terrapinService));
212+
}
213+
214+
private byte[] escape(byte[] data) {
215+
final byte[] escaped = new byte[data.length * 2];
216+
int j = 0;
217+
for (byte b : data) {
218+
if (b == 0x7e) {
219+
escaped[j++] = 0x7d;
220+
escaped[j++] = 0x5e;
221+
} else if (b == 0x7d) {
222+
escaped[j++] = 0x7d;
223+
escaped[j++] = 0x5d;
224+
} else {
225+
escaped[j++] = b;
226+
}
227+
}
228+
return Arrays.copyOf(escaped, j);
229+
}
230+
231+
private byte[] unescape(byte[] data) {
232+
final byte[] unescaped = new byte[data.length];
233+
int j = 0;
234+
for (int i = 0; i < data.length; i++) {
235+
if (data[i] == 0x7d) {
236+
if (data[i + 1] == 0x5e) {
237+
unescaped[j++] = 0x7e;
238+
} else if (data[i + 1] == 0x5d) {
239+
unescaped[j++] = 0x7d;
240+
} else {
241+
Log.w(TAG, "rf -> app: invalid escape sequence " + byteArrayToHex(data));
242+
}
243+
i++;
244+
} else {
245+
unescaped[j++] = data[i];
246+
}
247+
}
248+
return Arrays.copyOf(unescaped, j);
249+
}
250+
251+
@NonNull
252+
private byte[] swapEndianness(@NonNull byte[] data) {
253+
if (data.length % 2 != 0) {
254+
Log.w(TAG, "rf -> app: data length must be even " + byteArrayToHex(data));
255+
}
256+
final byte[] swapped = new byte[data.length];
257+
for (int i = 0; i < data.length; i += 2) {
258+
swapped[i] = data[i + 1];
259+
swapped[i + 1] = data[i];
260+
}
261+
return swapped;
262+
}
263+
264+
/**
265+
* Endian-swapped short from byte array
266+
*/
267+
private short getShort(@NonNull byte[] data, int offset) {
268+
return (short) ((data[offset] & 0xff) | (data[offset + 1] << 8));
269+
}
270+
}

0 commit comments

Comments
 (0)