From 8e99e47544c8e8184990affbbd8f1be711984d7b Mon Sep 17 00:00:00 2001 From: Kenny Daniel Date: Sun, 13 Oct 2024 13:47:57 -0700 Subject: [PATCH] Vectronix Terrapin-X laser rangefinder protocol --- .../baseline/lasers/rangefinder/Crc16.java | 50 ++++ .../rangefinder/RangefinderService.java | 1 + .../lasers/rangefinder/TerrapinProtocol.java | 235 ++++++++++++++++++ 3 files changed, 286 insertions(+) create mode 100644 app/src/main/java/com/platypii/baseline/lasers/rangefinder/Crc16.java create mode 100644 app/src/main/java/com/platypii/baseline/lasers/rangefinder/TerrapinProtocol.java diff --git a/app/src/main/java/com/platypii/baseline/lasers/rangefinder/Crc16.java b/app/src/main/java/com/platypii/baseline/lasers/rangefinder/Crc16.java new file mode 100644 index 00000000..5b0fbdcb --- /dev/null +++ b/app/src/main/java/com/platypii/baseline/lasers/rangefinder/Crc16.java @@ -0,0 +1,50 @@ +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 = 65535; + for (byte b : byteArray) { + int index = (crc ^ (b & 0xff)) & 0xff; + crc = (crc >> 8) ^ CRC16_LOOKUP_TABLE[index]; + } + return (short) crc; + } + + private static final int[] CRC16_LOOKUP_TABLE = { + 0, 4489, 8978, 12955, 17956, 22445, 25910, 29887, + 35912, 40385, 44890, 48851, 51820, 56293, 59774, 63735, + 4225, 264, 13203, 8730, 22181, 18220, 30135, 25662, + 40137, 36160, 49115, 44626, 56045, 52068, 63999, 59510, + 8450, 12427, 528, 5017, 26406, 30383, 17460, 21949, + 44362, 48323, 36440, 40913, 60270, 64231, 51324, 55797, + 12675, 8202, 4753, 792, 30631, 26158, 21685, 17724, + 48587, 44098, 40665, 36688, 64495, 60006, 55549, 51572, + 16900, 21389, 24854, 28831, 1056, 5545, 10034, 14011, + 52812, 57285, 60766, 64727, 34920, 39393, 43898, 47859, + 21125, 17164, 29079, 24606, 5281, 1320, 14259, 9786, + 57037, 53060, 64991, 60502, 39145, 35168, 48123, 43634, + 25350, 29327, 16404, 20893, 9506, 13483, 1584, 6073, + 61262, 65223, 52316, 56789, 43370, 47331, 35448, 39921, + 29575, 25102, 20629, 16668, 13731, 9258, 5809, 1848, + 65487, 60998, 56541, 52564, 47595, 43106, 39673, 35696, + 33800, 38273, 42778, 46739, 49708, 54181, 57662, 61623, + 2112, 6601, 11090, 15067, 20068, 24557, 28022, 31999, + 38025, 34048, 47003, 42514, 53933, 49956, 61887, 57398, + 6337, 2376, 15315, 10842, 24293, 20332, 32247, 27774, + 42250, 46211, 34328, 38801, 58158, 62119, 49212, 53685, + 10562, 14539, 2640, 7129, 28518, 32495, 19572, 24061, + 46475, 41986, 38553, 34576, 62383, 57894, 53437, 49460, + 14787, 10314, 6865, 2904, 32743, 28270, 23797, 19836, + 50700, 55173, 58654, 62615, 32808, 37281, 41786, 45747, + 19012, 23501, 26966, 30943, 3168, 7657, 12146, 16123, + 54925, 50948, 62879, 58390, 37033, 33056, 46011, 41522, + 23237, 19276, 31191, 26718, 7393, 3432, 16371, 11898, + 59150, 63111, 50204, 54677, 41258, 45219, 33336, 37809, + 27462, 31439, 18516, 23005, 11618, 15595, 3696, 8185, + 63375, 58886, 54429, 50452, 45483, 40994, 37561, 33584, + 31687, 27214, 22741, 18780, 15843, 11370, 7921, 3960 + }; +} diff --git a/app/src/main/java/com/platypii/baseline/lasers/rangefinder/RangefinderService.java b/app/src/main/java/com/platypii/baseline/lasers/rangefinder/RangefinderService.java index e34ebef2..669cff39 100644 --- a/app/src/main/java/com/platypii/baseline/lasers/rangefinder/RangefinderService.java +++ b/app/src/main/java/com/platypii/baseline/lasers/rangefinder/RangefinderService.java @@ -30,6 +30,7 @@ public class RangefinderService { private final BleProtocol protocols[] = { new ATNProtocol(), new SigSauerProtocol(), + new TerrapinProtocol(), new UineyeProtocol() }; diff --git a/app/src/main/java/com/platypii/baseline/lasers/rangefinder/TerrapinProtocol.java b/app/src/main/java/com/platypii/baseline/lasers/rangefinder/TerrapinProtocol.java new file mode 100644 index 00000000..ace2a9d2 --- /dev/null +++ b/app/src/main/java/com/platypii/baseline/lasers/rangefinder/TerrapinProtocol.java @@ -0,0 +1,235 @@ +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 = 0x0000; + private static final short packetTypeData = 0x0300; + private static final short packetTypeAck = 0x0400; + private static final short packetTypeNack = 0x0500; + + // Terrapin commands + private static final short commandNewMeasurementAvailable = 0x0010; + private static final short commandStartMeasurement = 0x0110; + private static final short commandGetLastRange = 0x0210; + private static final short commandGetLastInclination = 0x0310; + private static final short commandGetLastDirection = 0x0410; + private static final short commandGetLastTemperature = 0x0510; + private static final short commandGetLastPressure = 0x0610; + private static final short commandGetLastEHR = 0x0710; + + // private static final short commandGetComVersion = 0x0100; + // private static final short commandGetSupportedCommandSet = 0x0200; + // private static final short commandGetSerialNumber = 0x0300; + // private static final short commandActivateFactoryMode = 0x0400; + // private static final short commandGetHardwareRevision = 0x0500; + // private static final short commandGetFirmwareVersion = 0x0600; + // private static final short commandGetBatteryLevel = 0x0700; + // private static final short commandGetDeviceName = 0x0800; + // private static final short commandSetDeviceName = 0x0900; + // private static final short commandGetDeviceId = 0x0a00; + + @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: process " + byteArrayToHex(value)); + if (value[0] != 0x7e || value[value.length - 1] != 0x7e) { + Log.w(TAG, "rf -> app: invalid command " + 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 checksum = frame[frame.length - 2] + (frame[frame.length - 1] << 8); + if (Crc16.crc16(frame) != checksum) { + Log.w(TAG, "rf -> app: invalid checksum " + byteArrayToHex(frame) + " " + Integer.toHexString(Crc16.crc16(frame)) + " != " + Integer.toHexString(checksum)); + } + + // Packet types + if (frame[0] + (frame[1] << 8) == packetTypeCommand) { + // Data length + int dataLength = frame[3] + (frame[2] << 8); + if (dataLength == 512) dataLength = 0; + // Command + final int command = frame[5] + (frame[4] << 8); + final byte[] data = Arrays.copyOfRange(frame, 6, frame.length - 2); + if (command == commandNewMeasurementAvailable) { + Log.i(TAG, "rf -> app: new measurement available " + byteArrayToHex(data)); + getLastRange(peripheral); + } else { + Log.w(TAG, "rf -> app: command unknown 0x" + Integer.toHexString(command) + " " + dataLength + " " + byteArrayToHex(data)); + } + } else if (frame[0] + (frame[1] << 8) == packetTypeData) { + Log.i(TAG, "rf -> app: data " + byteArrayToHex(frame)); + } else if (frame[0] + (frame[1] << 8) == packetTypeAck) { + Log.i(TAG, "rf -> app: ack " + byteArrayToHex(frame)); + } else if (frame[0] + (frame[1] << 8) == packetTypeNack) { + Log.i(TAG, "rf -> app: nack " + byteArrayToHex(frame)); + } else { + Log.w(TAG, "rf -> app: unknown " + byteArrayToHex(frame)); + } + } + + 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[dataLength + 6]; + // 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); + // Data + // Checksum + final int checksum = Crc16.crc16(Arrays.copyOfRange(frame, 1, 7)); + frame[frame.length - 2] = (byte) (checksum & 0xff); + frame[frame.length - 1] = (byte) ((checksum >> 8) & 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 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); + } +}