From 072090f51d023d138875ac8dd9c4158348c0430d Mon Sep 17 00:00:00 2001 From: Yuan SY Date: Sun, 12 Jul 2026 19:18:46 +0800 Subject: [PATCH] Serialize BLE manual control writes --- android/cartfollow-devlog.md | 41 +++++ .../cartfollow/ManualControlArbiter.java | 19 ++- .../cartfollow/RealCartFollowFragment.java | 63 +++++-- .../cartfollow/RealCartSafetyController.java | 10 +- .../openbot/vehicle/BleSerialWriteQueue.java | 156 ++++++++++++++++++ .../org/openbot/vehicle/BluetoothManager.java | 62 ++++++- .../java/org/openbot/vehicle/Vehicle.java | 29 ++++ .../layout/fragment_human_cart_simulator.xml | 1 + .../cartfollow/ManualControlArbiterTest.java | 31 ++-- .../ManualControlMotionEventTest.java | 61 +++++++ .../RealCartSafetyControllerTest.java | 25 ++- .../vehicle/BleSerialWriteQueueTest.java | 112 +++++++++++++ 12 files changed, 567 insertions(+), 43 deletions(-) create mode 100644 android/robot/src/main/java/org/openbot/vehicle/BleSerialWriteQueue.java create mode 100644 android/robot/src/test/java/org/openbot/cartfollow/ManualControlMotionEventTest.java create mode 100644 android/robot/src/test/java/org/openbot/vehicle/BleSerialWriteQueueTest.java diff --git a/android/cartfollow-devlog.md b/android/cartfollow-devlog.md index 352e635c0..30fe54dbb 100644 --- a/android/cartfollow-devlog.md +++ b/android/cartfollow-devlog.md @@ -1082,3 +1082,44 @@ Debug APK android/robot/build/outputs/apk/debug/robot-debug.a 3. 空载落地只开放手动模式。 4. 手动全部通过后，才在空旷场地长按解锁自动实验。 5. 传感器和物理急停到位前，不进入货架、拥挤或无人看护场景。 + +--- + +## 18. BLE 手动控制抢占与低速调参（2026-07-12） + +### 18.1 问题与修复 + +真机发现方向按钮切换后仍可能继续执行旧动作。第一版 `ManualControlArbiter` 只处理 +方向状态，没有覆盖多 pointer 触摸和异步 GATT 连续写入。 + +本轮加入单在途 BLE 串口写队列：停车、急停、运动、心跳和查询统一排队；周期运动 +采用 latest-wins；方向切换以 `c0,0 -> 新方向` 有序事务提交，心跳和周期重发不能拆开 +该事务。关键停车写失败会重试一次，再失败则撤销 BLE ready 并禁止继续运动。 + +手动仲裁现在同时记录方向、pointer ID 和 generation。新手指按下后取得唯一控制权， +旧手指迟到的 `UP/CANCEL` 不得停止或恢复旧方向。页面同时显示当前方向、输出、BLE +写入状态和构建版本。 + +### 18.2 低速参数 + +```text +手动前进 14 +手动后退 12 +手动原地转向 5 +自动最大输出 14 +自动搜索速度 5 +``` + +ESP32 的 500 ms 运动保鲜、40 ms 控制周期和步长 6 保持不变。 + +### 18.3 自动化验证 + +```text +robot 单元测试 26/26 +checkStyle 通过 +assembleDebug 通过 +Debug APK android/robot/build/outputs/apk/debug/robot-debug.apk +``` + +真机仍需在车轮悬空状态验证单指换向、双指抢占、旧手指迟放、最终松手、退后台和 BLE +断开停车。手动验收通过前不得恢复自动跟随测试。 diff --git a/android/robot/src/main/java/org/openbot/cartfollow/ManualControlArbiter.java b/android/robot/src/main/java/org/openbot/cartfollow/ManualControlArbiter.java index 74fb0a468..08e68bacb 100644 --- a/android/robot/src/main/java/org/openbot/cartfollow/ManualControlArbiter.java +++ b/android/robot/src/main/java/org/openbot/cartfollow/ManualControlArbiter.java @@ -26,20 +26,24 @@ private PressResult(boolean replacedActiveControl, long generation) { } private Control activeControl; + private int activePointerId = -1; private long generation; - synchronized PressResult press(Control control) { + synchronized PressResult press(Control control, int pointerId) { if (control == null) throw new IllegalArgumentException("control must not be null"); + if (pointerId < 0) throw new IllegalArgumentException("pointerId must be non-negative"); - boolean replaced = activeControl != null && activeControl != control; - if (activeControl != control) generation++; + boolean replaced = activeControl != null && (activeControl != control || activePointerId != pointerId); + if (activeControl != control || activePointerId != pointerId) generation++; activeControl = control; + activePointerId = pointerId; return new PressResult(replaced, generation); } - synchronized boolean release(Control control) { - if (control == null || activeControl != control) return false; + synchronized boolean release(Control control, int pointerId) { + if (control == null || activeControl != control || activePointerId != pointerId) return false; activeControl = null; + activePointerId = -1; generation++; return true; } @@ -47,6 +51,7 @@ synchronized boolean release(Control control) { synchronized boolean clear() { if (activeControl == null) return false; activeControl = null; + activePointerId = -1; generation++; return true; } @@ -55,6 +60,10 @@ synchronized Control getActiveControl() { return activeControl; } + synchronized int getActivePointerId() { + return activePointerId; + } + synchronized long getGeneration() { return generation; } diff --git a/android/robot/src/main/java/org/openbot/cartfollow/RealCartFollowFragment.java b/android/robot/src/main/java/org/openbot/cartfollow/RealCartFollowFragment.java index c94e259bc..a085e6c35 100644 --- a/android/robot/src/main/java/org/openbot/cartfollow/RealCartFollowFragment.java +++ b/android/robot/src/main/java/org/openbot/cartfollow/RealCartFollowFragment.java @@ -6,6 +6,7 @@ import android.view.MotionEvent; import android.view.View; import androidx.navigation.Navigation; +import org.openbot.BuildConfig; import org.openbot.R; import org.openbot.vehicle.Control; @@ -39,6 +40,10 @@ public void run() { } RealCartSafetyController.Output watchdog = safetyController.watchdog(now); if (watchdog != null) latestOutput = watchdog; + if (safetyController.getMode() == RealCartSafetyController.Mode.MANUAL + && manualControlArbiter.getActiveControl() == null) { + latestOutput = RealCartSafetyController.stop("manual_idle"); + } sendOutput(latestOutput); refreshRealUi(); mainHandler.postDelayed(this, COMMAND_REPEAT_MS); @@ -86,6 +91,17 @@ protected void onCartFollowViewCreated() { binding.connectBle.setOnClickListener( v -> Navigation.findNavController(requireView()).navigate(R.id.open_bluetooth_fragment)); installAutoUnlock(); + binding + .getRoot() + .getViewTreeObserver() + .addOnWindowFocusChangeListener( + hasFocus -> { + if (!hasFocus + && binding != null + && safetyController.getMode() == RealCartSafetyController.Mode.MANUAL) { + invalidateManualControl("window_focus_lost", true); + } + }); binding.emergencyStop.setOnClickListener( v -> { safetyController.latchEmergency(); @@ -167,6 +183,8 @@ private void installDeadMan( (view, event) -> { switch (event.getActionMasked()) { case MotionEvent.ACTION_DOWN: + case MotionEvent.ACTION_POINTER_DOWN: + int downPointerId = event.getPointerId(event.getActionIndex()); RealCartSafetyController.Output nextOutput = safetyController.manual(left, right); if (nextOutput.isStop()) { invalidateManualControl("manual_blocked", true); @@ -174,31 +192,38 @@ private void installDeadMan( return true; } - ManualControlArbiter.PressResult pressResult = manualControlArbiter.press(control); + ManualControlArbiter.PressResult pressResult = + manualControlArbiter.press(control, downPointerId); if (pressResult.replacedActiveControl) { - // c0,0 bypasses the firmware ramp. Send it before the new target so the AT8236 - // starts the replacement direction from a known zero output. - latestOutput = RealCartSafetyController.stop("manual_replace"); - sendOutput(latestOutput); if (activeManualButton != null && activeManualButton != view) { activeManualButton.setPressed(false); } + latestOutput = nextOutput; + sendReplacementOutput(nextOutput, pressResult.generation); + } else { + latestOutput = nextOutput; + sendOutput(latestOutput); } activeManualButton = view; - latestOutput = nextOutput; - sendOutput(latestOutput); view.setPressed(true); return true; case MotionEvent.ACTION_UP: - case MotionEvent.ACTION_CANCEL: + case MotionEvent.ACTION_POINTER_UP: case MotionEvent.ACTION_OUTSIDE: + int upPointerId = event.getPointerId(event.getActionIndex()); view.setPressed(false); - if (manualControlArbiter.release(control)) { + if (manualControlArbiter.release(control, upPointerId)) { activeManualButton = null; latestOutput = RealCartSafetyController.stop("manual_release"); sendOutput(latestOutput); } return true; + case MotionEvent.ACTION_CANCEL: + view.setPressed(false); + if (manualControlArbiter.getActiveControl() == control) { + invalidateManualControl("manual_cancel", true); + } + return true; default: return true; } @@ -257,6 +282,14 @@ private void sendOutput(RealCartSafetyController.Output output) { new Control(output.left / (float) multiplier, output.right / (float) multiplier)); } + private void sendReplacementOutput(RealCartSafetyController.Output output, long generation) { + if (vehicle == null || output == null) return; + int multiplier = Math.max(1, vehicle.getSpeedMultiplier()); + vehicle.setControlReplacing( + new Control(output.left / (float) multiplier, output.right / (float) multiplier), + generation); + } + private void refreshRealUi() { if (binding == null) return; requireActivity() @@ -269,7 +302,17 @@ private void refreshRealUi() { : vehicle.isBleSerialReady() ? "BLE 已连接 · 等待固件握手" : "BLE 未连接"; String output = latestOutput == null ? "0,0" : latestOutput.left + "," + latestOutput.right; - binding.realConnectionStatus.setText(connection + " · 输出 " + output); + ManualControlArbiter.Control active = manualControlArbiter.getActiveControl(); + binding.realConnectionStatus.setText( + connection + + " | output=" + + output + + " | direction=" + + (active == null ? "STOP" : active.name()) + + " | ble=" + + vehicle.getBleWriteStatus() + + " | build=" + + BuildConfig.VERSION_NAME); boolean emergency = safetyController.isEmergencyLatched(); binding.emergencyStop.setEnabled(!emergency); binding.unlockAuto.setEnabled(!emergency && vehicle.isCartFirmwareReady()); diff --git a/android/robot/src/main/java/org/openbot/cartfollow/RealCartSafetyController.java b/android/robot/src/main/java/org/openbot/cartfollow/RealCartSafetyController.java index 1ff404f60..df782261e 100644 --- a/android/robot/src/main/java/org/openbot/cartfollow/RealCartSafetyController.java +++ b/android/robot/src/main/java/org/openbot/cartfollow/RealCartSafetyController.java @@ -9,11 +9,11 @@ public enum Mode { AUTO } - public static final int MANUAL_FORWARD = 28; - public static final int MANUAL_REVERSE = 24; - public static final int MANUAL_TURN = 20; - public static final int AUTO_MAX = 32; - public static final int SEARCH_SPEED = 18; + public static final int MANUAL_FORWARD = 14; + public static final int MANUAL_REVERSE = 12; + public static final int MANUAL_TURN = 5; + public static final int AUTO_MAX = 14; + public static final int SEARCH_SPEED = 5; public static final long INFERENCE_TIMEOUT_MS = 400L; public static final long SEARCH_LIMIT_MS = 2000L; diff --git a/android/robot/src/main/java/org/openbot/vehicle/BleSerialWriteQueue.java b/android/robot/src/main/java/org/openbot/vehicle/BleSerialWriteQueue.java new file mode 100644 index 000000000..b6ec81bca --- /dev/null +++ b/android/robot/src/main/java/org/openbot/vehicle/BleSerialWriteQueue.java @@ -0,0 +1,156 @@ +package org.openbot.vehicle; + +import java.util.ArrayDeque; +import java.util.Deque; +import java.util.Iterator; + +/** Serializes BLE UART writes and keeps stale drive commands out of the GATT pipeline. */ +final class BleSerialWriteQueue { + enum Type { + EMERGENCY, + STOP, + MOTION, + HEARTBEAT, + QUERY + } + + interface Sender { + void send(String payload); + } + + interface CriticalFailureListener { + void onCriticalWriteFailure(String payload); + } + + private static final class Entry { + final Type type; + final String payload; + final long generation; + int retries; + + Entry(Type type, String payload, long generation) { + this.type = type; + this.payload = payload; + this.generation = generation; + } + + boolean isCritical() { + return type == Type.EMERGENCY || type == Type.STOP; + } + } + + private final Deque pending = new ArrayDeque<>(); + private final Sender sender; + private final CriticalFailureListener failureListener; + private Entry inFlight; + private String status = "idle"; + + BleSerialWriteQueue(Sender sender, CriticalFailureListener failureListener) { + this.sender = sender; + this.failureListener = failureListener; + } + + synchronized void enqueue(Type type, String payload, long generation) { + if (type == Type.EMERGENCY) { + pending.clear(); + pending.addFirst(new Entry(type, payload, generation)); + } else if (type == Type.STOP) { + removePendingDriveCommands(); + pending.addFirst(new Entry(type, payload, generation)); + } else if (type == Type.MOTION) { + removePending(Type.MOTION); + addMotionAfterPendingStop(new Entry(type, payload, generation)); + } else { + pending.addLast(new Entry(type, payload, generation)); + } + dispatchNext(); + } + + synchronized void enqueueTransition(String stopPayload, String motionPayload, long generation) { + removePendingDriveCommands(); + // addFirst in reverse order keeps the pair contiguous ahead of heartbeat/query traffic. + pending.addFirst(new Entry(Type.MOTION, motionPayload, generation)); + pending.addFirst(new Entry(Type.STOP, stopPayload, generation)); + dispatchNext(); + } + + synchronized void onWriteComplete(boolean success) { + if (inFlight == null) return; + Entry completed = inFlight; + if (!success && completed.isCritical() && completed.retries == 0) { + completed.retries++; + status = "retry:" + summarize(completed.payload); + sender.send(completed.payload); + return; + } + + inFlight = null; + status = (success ? "ok:" : "failed:") + summarize(completed.payload); + if (!success && completed.isCritical()) { + pending.clear(); + failureListener.onCriticalWriteFailure(completed.payload); + return; + } + dispatchNext(); + } + + synchronized void clear() { + pending.clear(); + inFlight = null; + status = "cleared"; + } + + synchronized String getStatus() { + return status; + } + + synchronized int getPendingCount() { + return pending.size(); + } + + synchronized boolean hasInFlight() { + return inFlight != null; + } + + private void dispatchNext() { + if (inFlight != null || pending.isEmpty()) return; + inFlight = pending.removeFirst(); + status = "writing:" + summarize(inFlight.payload); + sender.send(inFlight.payload); + } + + private void removePendingDriveCommands() { + removePending(Type.STOP); + removePending(Type.MOTION); + } + + private void addMotionAfterPendingStop(Entry motion) { + if (pending.isEmpty()) { + pending.addLast(motion); + return; + } + Deque reordered = new ArrayDeque<>(); + boolean inserted = false; + while (!pending.isEmpty()) { + Entry entry = pending.removeFirst(); + reordered.addLast(entry); + if (!inserted && entry.type == Type.STOP) { + reordered.addLast(motion); + inserted = true; + } + } + if (!inserted) reordered.addLast(motion); + pending.addAll(reordered); + } + + private void removePending(Type type) { + Iterator iterator = pending.iterator(); + while (iterator.hasNext()) { + if (iterator.next().type == type) iterator.remove(); + } + } + + private static String summarize(String payload) { + return payload == null ? "null" : payload.trim(); + } +} diff --git a/android/robot/src/main/java/org/openbot/vehicle/BluetoothManager.java b/android/robot/src/main/java/org/openbot/vehicle/BluetoothManager.java index 71a96468c..7064d1a36 100644 --- a/android/robot/src/main/java/org/openbot/vehicle/BluetoothManager.java +++ b/android/robot/src/main/java/org/openbot/vehicle/BluetoothManager.java @@ -29,6 +29,8 @@ public interface ConnectionListener { void onBleSerialReady(); void onBleDisconnected(); + + void onBleCriticalWriteFailure(); } private static final String SERVICE_UUID = "61653dc3-4021-4d1e-ba83-8b4eec61d613"; @@ -48,12 +50,23 @@ public interface ConnectionListener { public String readValue; private final LocalBroadcastManager localBroadcastManager; private final ConnectionListener connectionListener; + private final BleSerialWriteQueue writeQueue; + private long motionGeneration; private boolean notifyEnabled; UUID[] uuidArray = new UUID[] {UUID.fromString(SERVICE_UUID)}; public BluetoothManager(Context context, ConnectionListener connectionListener) { this.context = context; this.connectionListener = connectionListener; + writeQueue = + new BleSerialWriteQueue( + this::writeGatt, + payload -> { + Logger.e("critical BLE write failed twice: " + payload.trim()); + if (this.connectionListener != null) { + this.connectionListener.onBleCriticalWriteFailure(); + } + }); initBleManager(); localBroadcastManager = LocalBroadcastManager.getInstance(this.context); } @@ -153,6 +166,7 @@ public void onConnected(BleDevice device) { @Override public void onDisconnected(String info, int status, BleDevice device) { bleDevice = null; + writeQueue.clear(); clearSerialCharacteristics(); if (connectionListener != null) connectionListener.onBleDisconnected(); notifyAdapter(); @@ -195,16 +209,44 @@ public void addDeviceInfoDataAndUpdate() { } } - public void write(String msg) { - if (isSerialReady()) { - BleManager.getInstance() - .write( - bleDevice, - writeServiceInfo.uuid, - writeCharacteristic.uuid, - msg.getBytes(UTF_8), - writeCallback); + public synchronized void write(String msg) { + if (!isSerialReady() || msg == null) return; + BleSerialWriteQueue.Type type = classifyWrite(msg); + long generation = type == BleSerialWriteQueue.Type.MOTION ? ++motionGeneration : 0L; + writeQueue.enqueue(type, msg, generation); + } + + public synchronized void writeControlTransition(String stop, String motion, long generation) { + if (!isSerialReady()) return; + motionGeneration = Math.max(motionGeneration, generation); + writeQueue.enqueueTransition(stop, motion, generation); + } + + public String getWriteStatus() { + return writeQueue.getStatus(); + } + + private void writeGatt(String msg) { + if (!isSerialReady()) { + writeQueue.onWriteComplete(false); + return; } + BleManager.getInstance() + .write( + bleDevice, + writeServiceInfo.uuid, + writeCharacteristic.uuid, + msg.getBytes(UTF_8), + writeCallback); + } + + private static BleSerialWriteQueue.Type classifyWrite(String msg) { + String line = msg.trim(); + if (line.startsWith("!S,")) return BleSerialWriteQueue.Type.EMERGENCY; + if (line.equals("c0,0")) return BleSerialWriteQueue.Type.STOP; + if (line.startsWith("c")) return BleSerialWriteQueue.Type.MOTION; + if (line.startsWith("h")) return BleSerialWriteQueue.Type.HEARTBEAT; + return BleSerialWriteQueue.Type.QUERY; } public BleMtuCallback mtuCallback = @@ -226,11 +268,13 @@ public void onFailure(int failCode, String info, BleDevice device) { public void onWriteSuccess(byte[] data, BleDevice device) { String value = new String(data, StandardCharsets.UTF_8); Logger.i("write success:" + value); + writeQueue.onWriteComplete(true); } @Override public void onFailure(int failCode, String info, BleDevice device) { Logger.e("write fail:" + info + " " + failCode); + writeQueue.onWriteComplete(false); } }; diff --git a/android/robot/src/main/java/org/openbot/vehicle/Vehicle.java b/android/robot/src/main/java/org/openbot/vehicle/Vehicle.java index 467fb081c..f0c846813 100644 --- a/android/robot/src/main/java/org/openbot/vehicle/Vehicle.java +++ b/android/robot/src/main/java/org/openbot/vehicle/Vehicle.java @@ -287,6 +287,22 @@ public void setControl(Control control) { sendControl(); } + /** Atomically queues a stop followed by a replacement BLE drive target. */ + public void setControlReplacing(Control control, long generation) { + this.control = control; + int left = (int) getLeftSpeed(); + int right = (int) getRightSpeed(); + String motion = String.format(Locale.US, "c%d,%d\n", left, right); + if (getConnectionType().equals("Bluetooth") + && bluetoothManager != null + && bluetoothManager.isBleConnected()) { + bluetoothManager.writeControlTransition("c0,0\n", motion, generation); + } else { + sendStringToDevice("c0,0\n"); + sendStringToDevice(motion); + } + } + public void setControl(float left, float right) { this.control = new Control(left, right); sendControl(); @@ -535,6 +551,15 @@ public void onBleDisconnected() { setReady(false); stopHeartbeat(); } + + @Override + public void onBleCriticalWriteFailure() { + control = new Control(0, 0); + bleSerialReady = false; + cartFirmwareReady = false; + setReady(false); + stopHeartbeat(); + } }); } @@ -550,6 +575,10 @@ public boolean isBleSerialReady() { return bleSerialReady && bluetoothManager != null && bluetoothManager.isSerialReady(); } + public String getBleWriteStatus() { + return bluetoothManager == null ? "unavailable" : bluetoothManager.getWriteStatus(); + } + public boolean isCartFirmwareReady() { return isBleSerialReady() && cartFirmwareReady && isReady(); } diff --git a/android/robot/src/main/res/layout/fragment_human_cart_simulator.xml b/android/robot/src/main/res/layout/fragment_human_cart_simulator.xml index d41467e58..1c3783b8f 100644 --- a/android/robot/src/main/res/layout/fragment_human_cart_simulator.xml +++ b/android/robot/src/main/res/layout/fragment_human_cart_simulator.xml @@ -212,6 +212,7 @@ android:layout_width="wrap_content" android:layout_height="wrap_content" android:gravity="center" + android:splitMotionEvents="true" android:orientation="horizontal">