Add a compass using the G2's magnetometer

This commit is contained in:
James Babcock committed 2026-08-04 02:09:32 -07:00
1 parent 4022f06d49
commit 852626e950
10 files changed
+424 -23

No files matched your search

@@ -57,6 +57,8 @@ public class FaceclawBleCommunicator implements FaceclawBleListener, Runnable {
private volatile FaceclawBleCommunicatorListener listener;
private final java.util.List<FaceclawImuListener> imuListeners =
new java.util.concurrent.CopyOnWriteArrayList<>();
private final java.util.List<FaceclawCompassListener> compassListeners =
new java.util.concurrent.CopyOnWriteArrayList<>();
private volatile Thread workerThread;
private volatile boolean running;
private volatile boolean userDisconnectRequested;
@@ -77,6 +79,10 @@ public class FaceclawBleCommunicator implements FaceclawBleListener, Runnable {
// overlay state is re-asserted on every reconnect and whenever the value changes.
private volatile boolean firmwareDebugFlagsEnabled;
private int firmwareDebugFlagsLastSent = -1;
// Desired CFW mode-10 compass state. It survives reconnects; lastSent is
// reset with each session so an open Compass window is re-asserted.
private boolean compassEnabled;
private int compassControlLastSent = -1;
private boolean startupProbePending;
// Desired ownership of CFW's fail-open stock-wake lease (dashboard launch
// and Even AI foreground takeover). This survives a transport reconnect;
@@ -413,6 +419,25 @@ public class FaceclawBleCommunicator implements FaceclawBleListener, Runnable {
interruptibleSleep.interrupt();
}
/**
* Enable/disable the stock compass through CFW image-handler mode 10. The
* desired state is retained across reconnects; headings arrive through
* stock sid-0x08 navigation notifications and FaceclawCompassListeners.
*/
public void setCompassEnabled(boolean enable) {
synchronized (lock) {
compassEnabled = enable;
compassControlLastSent = -1;
clearMessagesOfKindLocked("compass-control");
if (running && sessionReady && !shutdownRequested && fixedLayoutCreated && warmedUp) {
enqueueCompassControlLocked(true);
} else {
logLine("defer compass " + (enable ? "enable" : "disable") + "; display path not ready");
}
}
interruptibleSleep.interrupt();
}
/**
* Set the lens brightness. Fire-and-forget, like the IMU control: the
* message is queued ahead of other traffic and any not-yet-sent brightness
@@ -472,6 +497,18 @@ public class FaceclawBleCommunicator implements FaceclawBleListener, Runnable {
}
}
public void addCompassListener(FaceclawCompassListener listener) {
if (listener != null) {
compassListeners.add(listener);
}
}
public void removeCompassListener(FaceclawCompassListener listener) {
if (listener != null) {
compassListeners.remove(listener);
}
}
// The most recently started communicator; lets app worker threads submit
// surface frames without holding a cross-isolate reference to the bridge
@@ -955,6 +992,9 @@ public class FaceclawBleCommunicator implements FaceclawBleListener, Runnable {
Log.d(TAG, "onNotification: address=" + address + " characteristicUuid=" + characteristicUuid + " data.length=" + data.length);
BleProtocol.ParsedFrame frame = BleProtocol.parseFrame(data);
int decodedWearState = BleProtocol.parseWearState(frame);
BleProtocol.CompassEvent compassEvent = address.equalsIgnoreCase(rightAddress)
? BleProtocol.parseCompassEvent(frame)
: null;
boolean emitWearState = false;
G2Event event = null;
synchronized (lock) {
@@ -1065,6 +1105,9 @@ public class FaceclawBleCommunicator implements FaceclawBleListener, Runnable {
logLine(decodedWearState > 0 ? "wear state ON_HEAD" : "wear state OFF_HEAD");
emitWearState(decodedWearState > 0);
}
if (compassEvent != null) {
emitCompassEvent(compassEvent.command, compassEvent.headingDegrees);
}
if (event != null) {
if (event.hasImu) {
emitImuData(event.imuX, event.imuY, event.imuZ, event.eventSource);
@@ -1477,6 +1520,13 @@ public class FaceclawBleCommunicator implements FaceclawBleListener, Runnable {
enqueueFirmwareDebugFlagsLocked();
}
if (messageToPrewrite == null && !shutdownRequested && fixedLayoutCreated && warmedUp
&& (compassEnabled ? 1 : 0) != compassControlLastSent
&& pendingMessages.isEmpty() && inFlightMessages.isEmpty()) {
Log.i(TAG, "enqueueing compass " + (compassEnabled ? "enable" : "disable"));
enqueueCompassControlLocked(false);
}
// Up to WINDOW_SIZE messages may be in flight at once (full
// pipelining); a slot frees when an ack arrives.
boolean windowHasRoom = inFlightMessages.size() < Math.max(1, connectionOptions.WINDOW_SIZE);
@@ -1946,6 +1996,28 @@ public class FaceclawBleCommunicator implements FaceclawBleListener, Runnable {
logLine("queue firmware debug flags " + (show ? "show" : "hide"));
}
/** Send CFW image-handler mode 10: [10][1] start, [10][0] stop. */
private void enqueueCompassControlLocked(boolean priority) {
boolean enable = compassEnabled;
int sentState = enable ? 1 : 0;
byte[] payload = new byte[] { (byte) 10, (byte) sentState };
OutboundMessage message = messageBuilder.imagePayload(
"compass-control",
DASHBOARD_TILE,
nextMapSessionId(),
payload,
"compass " + (enable ? "enable" : "disable"),
connectionOptions.sendImagesToLeft);
message.onTimeout = () -> {
compassControlLastSent = -1;
logLine("compass control ack timeout");
};
if (priority) pendingMessages.addFirst(message);
else pendingMessages.addLast(message);
compassControlLastSent = sentState;
logLine("queue " + message.label);
}
private void enqueueDesiredImageLocked() {
String fingerprint = getDesiredFingerprint();
byte[] packed;
@@ -2558,6 +2630,7 @@ public class FaceclawBleCommunicator implements FaceclawBleListener, Runnable {
lastAudioControlAckMagic = 0;
audioCaptureActive = false;
audioPacketListener = null;
compassControlLastSent = -1;
faceclawWakePendingNonce = -1;
lastFaceclawWakeLeaseQueuedAtMs = 0;
faceclawWakeControlSentCount = 0;
@@ -2620,6 +2693,21 @@ public class FaceclawBleCommunicator implements FaceclawBleListener, Runnable {
});
}
private void emitCompassEvent(int command, int headingDegrees) {
if (compassListeners.isEmpty()) {
return;
}
mainHandler.post(() -> {
for (FaceclawCompassListener compassListener : compassListeners) {
try {
compassListener.onCompassEvent(command, headingDegrees);
} catch (Throwable t) {
Log.w(TAG, "listener onCompassEvent failed", t);
}
}
});
}
private void emitSilentMode(boolean silent) {
final FaceclawBleCommunicatorListener current = listener;
if (current == null) {
@@ -0,0 +1,6 @@
package com.faceclaw.app;
/** Receives stock compass heading and calibration notifications on the main thread. */
public interface FaceclawCompassListener {
void onCompassEvent(int command, int headingDegrees);
}
@@ -54,6 +54,10 @@ public class BleProtocol {
// Payload is g2.even_ai.EvenAIDataPackage (see
// g2-kit-unofficial/ble/gen/even_ai_pb.ts).
public static final int SID_EVEN_AI = 0x07;
public static final int SID_NAVIGATION = 0x08;
public static final int NAV_CMD_COMPASS_CHANGED = 15;
public static final int NAV_CMD_COMPASS_CALIBRATION_STARTED = 16;
public static final int NAV_CMD_COMPASS_CALIBRATION_COMPLETE = 17;
// eEvenAICommandId
public static final int EVEN_AI_CMD_CTRL = 1;
// eEvenAIStatus, carried in EvenAIDataPackage.ctrl.status (field 3, sub 1).
@@ -602,6 +606,29 @@ public class BleProtocol {
return Arrays.copyOf(pb, pb.length - 2);
}
/** Decode the stock firmware's compass navigation notifications (sid 0x08). */
public static CompassEvent parseCompassEvent(ParsedFrame frame) {
if (frame == null
|| !frame.ok
|| frame.sid != SID_NAVIGATION
|| (frame.flag != FLAG_NOTIFY && frame.flag != FLAG_NOTIFY_ALT)) {
return null;
}
byte[] root = stripTrailingCrc(frame.pb);
int command = readVarintFieldValue(root, 1, -1);
if (command == NAV_CMD_COMPASS_CHANGED) {
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 (command == NAV_CMD_COMPASS_CALIBRATION_STARTED
|| command == NAV_CMD_COMPASS_CALIBRATION_COMPLETE) {
return new CompassEvent(command, -1);
}
return null;
}
/**
* Display wake notification observed after a ring or right-arm double tap
* while no EvenHub page is running:
@@ -832,6 +859,16 @@ public class BleProtocol {
}
}
public static final class CompassEvent {
public final int command;
public final int headingDegrees;
CompassEvent(int command, int headingDegrees) {
this.command = command;
this.headingDegrees = headingDegrees;
}
}
public static final class ImageTileOptions {
final String name;
final int containerId;
@@ -60,12 +60,23 @@ public class MessageBuilder {
byte[] payload,
String label,
boolean leftArm
) {
return imagePayload("sound", tile, sessionId, payload, label, leftArm);
}
public OutboundMessage imagePayload(
String kind,
BleProtocol.ImageTileOptions tile,
int sessionId,
byte[] payload,
String label,
boolean leftArm
) {
BleProtocol.ImageFragment fragment =
new BleProtocol.ImageFragment(0, payload, payload.length);
int magic = magicPool.allocate();
return new OutboundMessage(
"sound",
kind,
label,
BleProtocol.SID_EVENHUB,
BleProtocol.FLAG_REQUEST,