Display compass diagnostic information

This commit is contained in:
James Babcock committed 2026-09-08 11:20:43 -07:00
1 parent 1154474754
commit 083628f37a
13 files changed
+807 -10

No files matched your search

@@ -1567,7 +1567,7 @@ public class FaceclawBleCommunicator implements FaceclawBleListener, Runnable {
emitWearState(decodedWearState > 0);
}
if (compassEvent != null) {
emitCompassEvent(compassEvent.command, compassEvent.headingDegrees);
emitCompassEvent(compassEvent);
}
if (event != null) {
if (event.hasImu) {
@@ -3456,11 +3456,22 @@ public class FaceclawBleCommunicator implements FaceclawBleListener, Runnable {
});
}
private void emitCompassEvent(int command, int headingDegrees) {
private void emitCompassEvent(BleProtocol.CompassEvent event) {
if (event.diagnosticFlags >= 0) {
String[] sources = { "unknown", "GRV", "GMRV", "RV" };
Log.i("FaceclawCompass", "heading=" + event.headingDegrees
+ " magneticAccuracy=" + event.magneticAccuracy
+ " magneticAnomalies=" + event.magneticAnomalies
+ " orientationSource=" + sources[event.orientationSource]
+ " flags=0x" + Integer.toHexString(event.diagnosticFlags)
+ " sampleTimeMs=" + event.sampleTimeMs);
}
for (CompassSubscription subscription : compassSubscriptions) {
subscription.handler.post(() -> {
try {
subscription.listener.onCompassEvent(command, headingDegrees);
subscription.listener.onCompassEvent(event.command, event.headingDegrees,
event.magneticAccuracy, event.magneticAnomalies, event.orientationSource,
event.diagnosticFlags, event.sampleTimeMs);
} catch (Throwable t) {
Log.w(TAG, "listener onCompassEvent failed", t);
}
@@ -2,5 +2,6 @@ package com.faceclaw.app;
/** Receives stock compass heading and calibration notifications on the thread that subscribed. */
public interface FaceclawCompassListener {
void onCompassEvent(int command, int headingDegrees);
void onCompassEvent(int command, int headingDegrees, int magneticAccuracy, int magneticAnomalies,
int orientationSource, int diagnosticFlags, long sampleTimeMs);
}
@@ -772,7 +772,23 @@ public class BleProtocol {
byte[] compass = readFieldBytes(root, 10);
if (compass == null) return null;
int heading = readVarintFieldValue(compass, 1, -1);
return heading >= 0 ? new CompassEvent(command, heading) : null;
if (heading < 0 || heading >= 360) return null;
byte[] diagnostic = readFieldBytes(root, 100);
if (diagnostic != null && diagnostic.length == 12
&& diagnostic[0] == 'C' && diagnostic[1] == 'M' && diagnostic[2] == 1
&& diagnostic[7] == 0
&& ((diagnostic[3] & 0xff) <= 3 || diagnostic[3] == (byte) 255)
&& ((diagnostic[4] & 0xff) <= 2 || diagnostic[4] == (byte) 255)
&& (diagnostic[5] & 0xff) <= 3) {
long sampleTimeMs = 0;
for (int i = 0; i < 4; i++) sampleTimeMs |= (long) (diagnostic[8 + i] & 0xff) << (8 * i);
return new CompassEvent(command, heading,
diagnostic[3] == (byte) 255 ? -1 : diagnostic[3] & 0xff,
diagnostic[4] == (byte) 255 ? -1 : diagnostic[4] & 0xff,
diagnostic[5] & 0xff, diagnostic[6] & 0xff, sampleTimeMs);
}
// Stock/older CFW and unknown extensions still supply usable headings.
return new CompassEvent(command, heading);
}
if (command == NAV_CMD_COMPASS_CALIBRATION_STARTED
|| command == NAV_CMD_COMPASS_CALIBRATION_COMPLETE) {
@@ -1014,10 +1030,23 @@ public class BleProtocol {
public static final class CompassEvent {
public final int command;
public final int headingDegrees;
/** -1 means unavailable; source: 0 unknown, 1 GRV, 2 GMRV, 3 RV. */
public final int magneticAccuracy, magneticAnomalies, orientationSource, diagnosticFlags;
public final long sampleTimeMs;
CompassEvent(int command, int headingDegrees) {
this(command, headingDegrees, -1, -1, -1, -1, -1);
}
CompassEvent(int command, int headingDegrees, int magneticAccuracy, int magneticAnomalies,
int orientationSource, int diagnosticFlags, long sampleTimeMs) {
this.command = command;
this.headingDegrees = headingDegrees;
this.magneticAccuracy = magneticAccuracy;
this.magneticAnomalies = magneticAnomalies;
this.orientationSource = orientationSource;
this.diagnosticFlags = diagnosticFlags;
this.sampleTimeMs = sampleTimeMs;
}
}