Vectronix Terrapin-X laser rangefinder protocol

Author: platypii

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()
     };
 

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

@@ -0,0 +1,138 @@
+package com.platypii.baseline.lasers.rangefinder;
+
+import com.platypii.baseline.bluetooth.BleException;
+import com.platypii.baseline.bluetooth.BleProtocol;
+import com.platypii.baseline.bluetooth.BluetoothUtil;
+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.bytesToShort;
+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("85920000-0338-4b83-ae4a-ac1d217adb03");
+    // Terrapin characteristic: read, indicate
+    private static final UUID terrapinCharacteristic1 = UUID.fromString("85920100-0338-4b83-ae4a-ac1d217adb03");
+    // Terrapin characteristic: notify, write
+    private static final UUID terrapinCharacteristic2 = UUID.fromString("85920200-0338-4b83-ae4a-ac1d217adb03");
+
+    private static final String factoryModeSecretKey = "B6987833";
+    private static final String packetTypeCommand = "0000";
+    private static final String packetTypeData = "0300";
+    private static final String packetTypeAck = "0400";
+    private static final String packetTypeNack = "0500";
+
+    // Say hello to laser
+    private static final byte[] commandStartMeasurement = {1, 16}; // 0110
+
+    @Override
+    public void onServicesDiscovered(@NonNull BluetoothPeripheral peripheral) {
+        try {
+            // Request rangefinder service
+            Log.i(TAG, "app -> rf: subscribe");
+            peripheral.setNotify(terrapinService, terrapinCharacteristic2, true);
+            sendHello(peripheral);
+            readRangefinder(peripheral);
+        } catch (Throwable e) {
+            Log.e(TAG, "rangefinder handshake exception", e);
+        }
+    }
+
+    @Override
+    public void processBytes(@NonNull BluetoothPeripheral peripheral, @NonNull byte[] value) {
+        final String hex = byteArrayToHex(value);
+        if (!hex.startsWith("7e-") || !hex.endsWith("7e")) {
+            Log.w(TAG, "rf -> app: invalid command " + hex);
+            return;
+        }
+        Log.i(TAG, "rf -> app: unknown " + hex);
+    }
+
+    private void readRangefinder(@NonNull BluetoothPeripheral peripheral) {
+        Log.i(TAG, "app -> rf: read");
+        peripheral.readCharacteristic(terrapinService, terrapinCharacteristic1);
+    }
+
+    private void sendHello(@NonNull BluetoothPeripheral peripheral) {
+        Log.d(TAG, "app -> rf: hello");
+        BluetoothUtil.sleep(5000);
+        peripheral.writeCharacteristic(terrapinService, terrapinCharacteristic2, commandStartMeasurement, WriteType.WITH_RESPONSE);
+    }
+
+    private void processMeasurement(@NonNull byte[] value) {
+        Log.d(TAG, "rf -> app: measure " + byteArrayToHex(value));
+
+        final double units; // unit multiplier
+        if (value[21] == 1) {
+            units = 1; // meters
+        } else if (value[21] == 2) {
+            units = 0.9144; // yards
+        } else if (value[21] == 3) {
+            units = 0.3048; // feet
+        } else {
+            Exceptions.report(new IllegalStateException("Unexpected units value from uineye " + value[21]));
+            units = 0;
+        }
+        final double pitch = bytesToShort(value[3], value[4]) * 0.1 * units; // degrees
+//        final double total = Util.bytesToShort(value[5], value[6]) * 0.1 * units; // meters
+        double vert = bytesToShort(value[7], value[8]) * 0.1 * units; // meters
+        double horiz = bytesToShort(value[9], value[10]) * 0.1 * units; // meters
+//        double bearing = (value[22] & 0xff) * 360.0 / 256.0; // degrees
+        if (pitch < 0 && vert > 0) {
+            vert = -vert;
+        }
+
+        final LaserMeasurement meas = new LaserMeasurement(horiz, vert);
+        Log.i(TAG, "rf -> app: measure " + meas);
+        EventBus.getDefault().post(meas);
+    }
+
+    /**
+     * 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));
+    }
+}