Skip to content

Commit

Permalink
Vectronix Terrapin-X laser rangefinder protocol
Browse files Browse the repository at this point in the history
  • Loading branch information
@platypii
platypii committed Oct 27, 2024
1 parent 1d830c4 commit da16349
Show file tree
Hide file tree
Showing 3 changed files with 296 additions and 0 deletions.
30 changes: 30 additions & 0 deletions app/src/main/java/com/platypii/baseline/lasers/rangefinder/Crc16.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,30 @@
package com.platypii.baseline.lasers.rangefinder;

class Crc16 {
/**
* Computes the CRC16 checksum for a given byte array.
*/
public static short crc16(byte[] byteArray) {
int crc = 0xffff;

// Process bytes in pairs (LSB first)
for (int i = 0; i < byteArray.length; i++) {
int b = byteArray[i] & 0xff;
crc ^= b;

// Process each bit
for (int j = 0; j < 8; j++) {
if ((crc & 0x0001) != 0) {
crc = (crc >> 1) ^ 0x8408; // 0x8408 is reversed polynomial
} else {
crc = crc >> 1;
}
}
}

// XOR with 0xfff
crc ^= 0xffff;

return (short) crc;
}
}
1 change: 1 addition & 0 deletions app/src/main/java/com/platypii/baseline/lasers/rangefinder/RangefinderService.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@ public class RangefinderService {
private final BleProtocol protocols[] = {
new ATNProtocol(),
new SigSauerProtocol(),
new TerrapinProtocol(),
new UineyeProtocol()
};

Expand Down
265 changes: 265 additions & 0 deletions app/src/main/java/com/platypii/baseline/lasers/rangefinder/TerrapinProtocol.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,265 @@
package com.platypii.baseline.lasers.rangefinder;

import com.platypii.baseline.bluetooth.BleException;
import com.platypii.baseline.bluetooth.BleProtocol;
import com.platypii.baseline.lasers.LaserMeasurement;
import com.platypii.baseline.util.Exceptions;

import android.bluetooth.le.ScanRecord;
import android.os.ParcelUuid;
import android.util.Log;
import androidx.annotation.NonNull;
import androidx.annotation.Nullable;
import com.welie.blessed.BluetoothPeripheral;
import com.welie.blessed.WriteType;
import java.util.Arrays;
import java.util.List;
import java.util.UUID;
import org.greenrobot.eventbus.EventBus;

import static com.platypii.baseline.bluetooth.BluetoothUtil.byteArrayToHex;
import static com.platypii.baseline.bluetooth.BluetoothUtil.toManufacturerString;

/**
* This class contains ids, commands, and decoders for Vectronix Terrapin-X laser rangefinders.
*/
class TerrapinProtocol extends BleProtocol {
private static final String TAG = "TerrapinProtocol";

// Manufacturer ID
private static final int manufacturerId1 = 1164;
private static final byte[] manufacturerData1 = {1, -96, -1, -1, -1, -1, 0}; // 01-a0-ff-ff-ff-ff-00

// Terrapin service
private static final UUID terrapinService = UUID.fromString("81480000-b0b7-4074-8a24-ae554e5cdbc4");
// Terrapin characteristic: read, indicate
private static final UUID terrapinCharacteristic1 = UUID.fromString("81480100-b0b7-4074-8a24-ae554e5cdbc4");
// Terrapin characteristic: notify, write
private static final UUID terrapinCharacteristic2 = UUID.fromString("81480200-b0b7-4074-8a24-ae554e5cdbc4");

private static final String factoryModeSecretKey = "b6987833";

// Terrapin packet types
private static final short packetTypeCommand = 0x00;
private static final short packetTypeData = 0x03;
private static final short packetTypeAck = 0x04;
private static final short packetTypeNack = 0x05;

// Terrapin commands
private static final short commandNewMeasurementAvailable = 0x1000;
private static final short commandStartMeasurement = 0x1001;
private static final short commandGetLastRange = 0x1002;
private static final short commandGetLastInclination = 0x1003;
private static final short commandGetLastDirection = 0x1004;
private static final short commandGetLastTemperature = 0x1005;
private static final short commandGetLastPressure = 0x1006;
private static final short commandGetLastEHR = 0x1007;
private static final short commandGetLaserMode = 0x1022;
private static final short commandGetDeclination = 0x1027;
private static final short commandGetRangeGate = 0x1028;
// private static final short commandGetLastSNR = 0xf006;

// private static final short commandGetComVersion = 0x01;
// private static final short commandGetSupportedCommandSet = 0x02;
// private static final short commandGetSerialNumber = 0x03;
// private static final short commandActivateFactoryMode = 0x04;
// private static final short commandGetHardwareRevision = 0x05;
// private static final short commandGetFirmwareVersion = 0x06;
// private static final short commandGetBatteryLevel = 0x07;
// private static final short commandGetDeviceName = 0x08;
// private static final short commandSetDeviceName = 0x09;
// private static final short commandGetDeviceId = 0x0a;

@Override
public void onServicesDiscovered(@NonNull BluetoothPeripheral peripheral) {
try {
// Request rangefinder service
Log.i(TAG, "app -> rf: subscribe");
peripheral.setNotify(terrapinService, terrapinCharacteristic1, true);
} catch (Throwable e) {
Log.e(TAG, "rangefinder handshake exception", e);
}
}

@Override
public void processBytes(@NonNull BluetoothPeripheral peripheral, @NonNull byte[] value) {
Log.d(TAG, "rf -> app: processBytes " + byteArrayToHex(value));
if (value[0] != 0x7e || value[value.length - 1] != 0x7e) {
Log.w(TAG, "rf -> app: invalid sentence " + byteArrayToHex(value));
return;
}

// Remove frame
byte[] frame = Arrays.copyOfRange(value, 1, value.length - 1);
// Unescape special characters 0x7e and 0x7d
frame = unescape(frame);

// Check checksum
final int checksumValue = getShort(frame, frame.length - 2);
byte[] checksumContent = Arrays.copyOfRange(frame, 0, frame.length - 2);
final short checksumComputed = Crc16.crc16(checksumContent);
if (checksumValue != checksumComputed) {
Log.w(TAG, "rf -> app: invalid checksum " + byteArrayToHex(checksumContent) + " " + shortToHex(checksumValue) + " != " + shortToHex(checksumComputed));
}

// Packet types
final short packetType = getShort(frame, 0);
// Data length
int dataLength = getShort(frame, 2);
if (dataLength == 512) dataLength = 0;
// Command
final int command = getShort(frame, 4);
if (packetType == packetTypeCommand) {
final byte[] data = Arrays.copyOfRange(frame, 6, frame.length - 2);
if (command == commandNewMeasurementAvailable) {
Log.i(TAG, "rf -> app: new measurement available");
getLastRange(peripheral);
} else {
Log.w(TAG, "rf -> app: command unknown 0x" + shortToHex(command) + " " + dataLength + " " + byteArrayToHex(data));
}
} else if (packetType == packetTypeData) {
Log.i(TAG, "rf -> app: data " + byteArrayToHex(frame));
} else if (packetType == packetTypeAck) {
Log.i(TAG, "rf -> app: ack " + byteArrayToHex(frame));
} else if (packetType == packetTypeNack) {
Log.i(TAG, "rf -> app: nack " + dataLength + " " + shortToHex(command) + " " + byteArrayToHex(frame));
} else {
Log.w(TAG, "rf -> app: unknown packet type " + shortToHex(packetType) + " " + byteArrayToHex(frame));
}
}

private String shortToHex(int value) {
return Integer.toHexString(value & 0xffff);
}

private void processMeasurement(@NonNull byte[] value) {
Log.d(TAG, "rf -> app: measure " + byteArrayToHex(value));
// TODO
EventBus.getDefault().post(new LaserMeasurement(0, 0));
}
private void startMeasurement(@NonNull BluetoothPeripheral peripheral) {
Log.i(TAG, "app -> rf: start measurement");
sendCommand(peripheral, commandStartMeasurement, null);
}

private void getLastRange(@NonNull BluetoothPeripheral peripheral) {
Log.i(TAG, "app -> rf: get last range");
sendCommand(peripheral, commandGetLastRange, null);
}

private void sendCommand(@NonNull BluetoothPeripheral peripheral, short command, @Nullable byte[] data) {
final int dataLength = data == null ? 0 : data.length; // TODO: / 2 ?
byte[] frame = new byte[8 + dataLength];
// Packet type
frame[0] = (byte) (packetTypeCommand & 0xff);
frame[1] = (byte) ((packetTypeCommand >> 8) & 0xff);
// Data length
if (data != null) {
frame[2] = (byte) (dataLength & 0xff);
frame[3] = (byte) ((dataLength >> 8) & 0xff);
System.arraycopy(data, 0, frame, 6, data.length);
} else {
frame[2] = 2;
frame[3] = 0;
}
// Command
frame[4] = (byte) (command & 0xff);
frame[5] = (byte) ((command >> 8) & 0xff);
// Checksum
final int checksum = Crc16.crc16(Arrays.copyOfRange(frame, 0, frame.length - 2));
frame[frame.length - 1] = (byte) ((checksum >> 8) & 0xff);
frame[frame.length - 2] = (byte) (checksum & 0xff);
// Escape special characters 0x7e and 0x7d
frame = escape(frame);
// Wrap frame
byte[] wrapped = new byte[frame.length + 2];
wrapped[0] = 0x7e;
System.arraycopy(frame, 0, wrapped, 1, frame.length);
wrapped[wrapped.length - 1] = 0x7e;
Log.d(TAG, "app -> rf: send command " + byteArrayToHex(wrapped));
peripheral.writeCharacteristic(terrapinService, terrapinCharacteristic2, wrapped, WriteType.WITH_RESPONSE);
}

/**
* Return true iff a bluetooth scan result looks like a rangefinder
*/
@Override
public boolean canParse(@NonNull BluetoothPeripheral peripheral, @Nullable ScanRecord record) {
final String deviceName = peripheral.getName();
if (record != null && Arrays.equals(record.getManufacturerSpecificData(manufacturerId1), manufacturerData1)) {
return true; // Manufacturer match (kenny's laser)
} else if (
(record != null && hasRangefinderService(record))
|| deviceName.startsWith("FastM")
|| deviceName.startsWith("Terrapin")) {
// Send manufacturer data to firebase
final String mfg = toManufacturerString(record);
Exceptions.report(new BleException("Terrapin laser unknown mfg data: " + deviceName + " " + mfg));
return true;
} else {
return false;
}
}

private boolean hasRangefinderService(@NonNull ScanRecord record) {
final List<ParcelUuid> uuids = record.getServiceUuids();
return uuids != null && uuids.contains(new ParcelUuid(terrapinService));
}

private byte[] escape(byte[] data) {
final byte[] escaped = new byte[data.length * 2];
int j = 0;
for (byte b : data) {
if (b == 0x7e) {
escaped[j++] = 0x7d;
escaped[j++] = 0x5e;
} else if (b == 0x7d) {
escaped[j++] = 0x7d;
escaped[j++] = 0x5d;
} else {
escaped[j++] = b;
}
}
return Arrays.copyOf(escaped, j);
}

private byte[] unescape(byte[] data) {
final byte[] unescaped = new byte[data.length];
int j = 0;
for (int i = 0; i < data.length; i++) {
if (data[i] == 0x7d) {
if (data[i + 1] == 0x5e) {
unescaped[j++] = 0x7e;
} else if (data[i + 1] == 0x5d) {
unescaped[j++] = 0x7d;
} else {
Log.w(TAG, "rf -> app: invalid escape sequence " + byteArrayToHex(data));
}
i++;
} else {
unescaped[j++] = data[i];
}
}
return Arrays.copyOf(unescaped, j);
}

@NonNull
private byte[] swapEndianness(@NonNull byte[] data) {
if (data.length % 2 != 0) {
Log.w(TAG, "rf -> app: data length must be even " + byteArrayToHex(data));
}
final byte[] swapped = new byte[data.length];
for (int i = 0; i < data.length; i += 2) {
swapped[i] = data[i + 1];
swapped[i + 1] = data[i];
}
return swapped;
}

/**
* Endian-swapped short from byte array
*/
private short getShort(@NonNull byte[] data, int offset) {
return (short) ((data[offset] & 0xff) | (data[offset + 1] << 8));
}
}

0 comments on commit da16349

Please sign in to comment.