diff --git a/PROTOCOL_MIGRATION.md b/PROTOCOL_MIGRATION.md index b84009a..b0f8a4f 100644 --- a/PROTOCOL_MIGRATION.md +++ b/PROTOCOL_MIGRATION.md @@ -652,4 +652,22 @@ Key files: - `protocol.h/cpp` - Packet format and CRC implementation - `commands.h/cpp` - Command handlers - `sensors.h/cpp` - IMU and radar drivers +- `websocket_client.h/cpp` - WiFi, WebSocket client + server + +--- + +## WebSocket Server + +The firmware runs a WebSocket server on port **81** alongside the outbound client. +Any device on the same WiFi network can connect and send protocol-framed packets. + +**URL:** `ws://:81/` + +All commands from the serial protocol are supported over WebSocket: +`VSME`, `MSET`, `BHVR`, `SSET`, `FPLY`, `FSTP`, etc. + +Packets must use the same binary framing as serial: +`0xA5 0x5A + TAG(4) + LEN(2) + SEQ(2) + PAYLOAD(N) + CRC16(2)` + +Up to 2 simultaneous WebSocket client connections are supported. diff --git a/behaviors.h b/behaviors.h index 3788bc4..59df60a 100644 --- a/behaviors.h +++ b/behaviors.h @@ -284,7 +284,7 @@ private: std::vector visemes; // Configuration - static constexpr unsigned long TIMEOUT_MS = 3000; // 3 second timeout + static constexpr unsigned long TIMEOUT_MS = 200; // 3 second timeout static constexpr uint16_t DEFAULT_POSITION = 2047; // Center/rest position // Helper to find viseme by ID diff --git a/commands.cpp b/commands.cpp index c76ccdd..54cc916 100644 --- a/commands.cpp +++ b/commands.cpp @@ -87,10 +87,10 @@ bool MotorStreamState::shouldStream() { // ============================================================================ void dispatchCommand() { - const char* tag = getReceivedTag(); - const uint8_t* payload = getReceivedPayload(); - uint16_t len = getReceivedPayloadLen(); - + dispatchCommand(getReceivedTag(), getReceivedPayload(), getReceivedPayloadLen()); +} + +void dispatchCommand(const char tag[4], const uint8_t* payload, uint16_t len) { // Identity & Config if (tagMatches(tag, Tag::IDENT)) { handleIdent(payload, len); diff --git a/commands.h b/commands.h index 123f5fa..189a857 100644 --- a/commands.h +++ b/commands.h @@ -47,6 +47,9 @@ extern MotorStreamState motorStream; // Process a received packet - call after receivePacket() returns true void dispatchCommand(); +// Dispatch a command by tag and payload directly (for WebSocket server, etc.) +void dispatchCommand(const char tag[4], const uint8_t* payload, uint16_t len); + // ============================================================================ // Individual Command Handlers // ============================================================================ diff --git a/ls_firmware.ino b/ls_firmware.ino index 4277633..d54551d 100644 --- a/ls_firmware.ino +++ b/ls_firmware.ino @@ -559,9 +559,6 @@ void setup() { delay(500); Serial.println("\n[HansonServo] Starting..."); - // WebSocket client (WiFi + connect to remote, receive FACE packets) - websocketSetup(); - // Initialize servo manager servoManager.init(); Serial.println("[HansonServo] Servos initialized"); @@ -645,6 +642,9 @@ void setup() { behaviorManager.setBehaviorEnabled(BEHAVIOR_IDLE, true); } + // WebSocket client + server (WiFi must be after config load so credentials are ready) + websocketSetup(); + // Initialize idle behavior motors (needs config.motors to be loaded) std::vector allMotorIDs; for (const Motor& motor : config.motors) { diff --git a/robotconfig.cpp b/robotconfig.cpp index 9be3e90..cbab3ef 100644 --- a/robotconfig.cpp +++ b/robotconfig.cpp @@ -650,13 +650,20 @@ bool RobotConfig::loadFromFFatV2(const char* path, BehaviorManager* behaviorMana case KEY_WIFI_SETTINGS: { if (type == TYPE_WIFI_SETTINGS) { - readStr(file, wifiSettings.ssid, 32); - readStr(file, wifiSettings.password, 64); - readStr(file, wifiSettings.host, 63); - wifiSettings.port = readU16(file); - readStr(file, wifiSettings.path, 31); + // TODO: re-enable when SSET wifi writes are working + // For now, skip loading and use hardcoded defaults from WiFiSettings struct + // readStr(file, wifiSettings.ssid, 32); + // readStr(file, wifiSettings.password, 64); + // readStr(file, wifiSettings.host, 63); + // wifiSettings.port = readU16(file); + // readStr(file, wifiSettings.path, 31); + // Serial.println("[Config] WiFi settings loaded"); - Serial.println("[Config] WiFi settings loaded"); + // Skip the stored blob so file position stays correct + for (int s = 0; s < 3; s++) { uint8_t l = file.read(); for (uint8_t k = 0; k < l; k++) file.read(); } + file.read(); file.read(); // port + for (int s = 0; s < 2; s++) { uint8_t l = file.read(); for (uint8_t k = 0; k < l; k++) file.read(); } + Serial.println("[Config] WiFi settings skipped (using hardcoded)"); } else { // Skip: 5 length-prefixed strings + 2 byte port - can't know exact size // Best effort: skip based on stored lengths diff --git a/tools/viseme_sender.py b/tools/viseme_sender.py new file mode 100644 index 0000000..1cbf8d6 --- /dev/null +++ b/tools/viseme_sender.py @@ -0,0 +1,232 @@ +""" +Viseme Sender - sends viseme packets to the robot over WebSocket. + +Usage: + python viseme_sender.py [robot_ip] + +Default IP: 192.168.1.x (auto-discovered or specify as argument) +Connects to ws://:81 + +Controls: + 0-9, A = send viseme 0-10 + Space = send neutral (viseme 0 / sil) + Q / Esc = quit + L = request viseme list (VLST) +""" + +import asyncio +import struct +import sys + +try: + import websockets +except ImportError: + print("Missing dependency. Install with: pip install websockets") + sys.exit(1) + +# --------------------------------------------------------------------------- +# Protocol helpers +# --------------------------------------------------------------------------- + +SYNC = b'\xA5\x5A' + +def crc16_ccitt(data: bytes, init: int = 0xFFFF) -> int: + crc = init + for b in data: + crc ^= b << 8 + for _ in range(8): + if crc & 0x8000: + crc = (crc << 1) ^ 0x1021 + else: + crc <<= 1 + crc &= 0xFFFF + return crc + +_seq = 0 + +def build_packet(tag: str, payload: bytes = b'') -> bytes: + global _seq + tag_bytes = tag.encode('ascii')[:4].ljust(4, b'\x00') + length = len(payload) + header_tail = tag_bytes + struct.pack(' 1 else '192.168.1.1' + uri = f'ws://{ip}:81' + + print(f'Connecting to {uri} ...') + try: + ws = await websockets.connect(uri) + except Exception as e: + print(f'Connection failed: {e}') + return + + print(f'Connected to {uri}') + print() + print('Viseme keys:') + for k, vid in sorted(KEY_MAP.items()): + label = VISEMES.get(vid, f'viseme {vid}') + key_label = 'Space' if k == ' ' else k.upper() + print(f' [{key_label}] -> {vid}: {label}') + print(' [L] -> list visemes from device') + print(' [Q/Esc] -> quit') + print() + + # Background task to receive and print responses + async def receiver(): + try: + async for message in ws: + if isinstance(message, bytes): + result = parse_packet(message) + if result: + tag, payload = result + if tag == 'VLST': + print_viseme_list(payload) + elif tag == 'ACK!': + print(f' <- ACK') + elif tag.startswith('NAC'): + msg = payload.decode('ascii', errors='replace') if payload else '' + print(f' <- NACK: {msg}') + elif tag == 'MSGE': + msg = payload.decode('ascii', errors='replace') + print(f' <- MSG: {msg}') + else: + print(f' <- [{tag}] {len(payload)} bytes') + except websockets.exceptions.ConnectionClosed: + pass + + recv_task = asyncio.create_task(receiver()) + + # Input loop (runs in executor to avoid blocking) + loop = asyncio.get_event_loop() + try: + while True: + key = await loop.run_in_executor(None, get_key) + if key is None or key in ('q', '\x1b'): + break + if key == 'l': + pkt = build_packet('VLST') + await ws.send(pkt) + print(' -> VLST (list visemes)') + continue + if key in KEY_MAP: + vid = KEY_MAP[key] + pkt = build_packet('VSME', bytes([vid])) + await ws.send(pkt) + label = VISEMES.get(vid, f'viseme {vid}') + print(f' -> VSME {vid}: {label}') + except (KeyboardInterrupt, EOFError): + pass + finally: + recv_task.cancel() + await ws.close() + print('Disconnected.') + + +def print_viseme_list(payload: bytes): + if len(payload) < 1: + print(' <- VLST: (empty)') + return + count = payload[0] + print(f' <- VLST: {count} visemes') + pos = 1 + for _ in range(count): + if pos + 4 >= len(payload): + break + vid = payload[pos] + label = payload[pos+1:pos+4].decode('ascii', errors='replace') + motor_count = payload[pos+4] + pos += 5 + motors = [] + for _ in range(motor_count): + if pos + 3 > len(payload): + break + mid = payload[pos] + mpos = struct.unpack(' str | None: + """Block until a single key is pressed, return it lowercase.""" + try: + import msvcrt # Windows + ch = msvcrt.getwch() + return ch.lower() + except ImportError: + pass + # Unix / macOS + import tty, termios + fd = sys.stdin.fileno() + old = termios.tcgetattr(fd) + try: + tty.setraw(fd) + ch = sys.stdin.read(1) + return ch.lower() + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old) + + +if __name__ == '__main__': + try: + asyncio.run(main()) + except KeyboardInterrupt: + print('\nBye.') diff --git a/websocket_client.cpp b/websocket_client.cpp index 0aaebb2..4feae7b 100644 --- a/websocket_client.cpp +++ b/websocket_client.cpp @@ -1,6 +1,7 @@ #include "websocket_client.h" #include "sensors.h" #include "protocol.h" +#include "commands.h" #include #include @@ -8,6 +9,10 @@ using namespace websockets; WiFiSettings wifiSettings; // Global runtime instance +// ============================================================================ +// Outbound client (connects to Radxa for FACE/ALIV) +// ============================================================================ + static WebsocketsClient client; static bool s_connected = false; static unsigned long lastReconnectAttempt = 0; @@ -18,70 +23,48 @@ static String buildWsUrl() { } // ============================================================================ -// Packet parsing for WebSocket binary messages -// Uses the same protocol format: 0xA5 0x5A TAG(4) LEN(2) SEQ(2) PAYLOAD(N) CRC(2) +// Inbound server (accepts commands from computer) // ============================================================================ -static void processPacketPayload(const char tag[4], const uint8_t* payload, uint16_t len) { - if (memcmp(tag, Tag::FACE, 4) == 0) { - // Face detection data - feed to FaceDetect sensor - // SensorManager will send the FACE packet over serial - faceDetect.feedPayload(payload, len); - } - else if (memcmp(tag, Tag::ALIV, 4) == 0) { - // ALIV payload: [component_id:1][alive:1] - if (len >= 2) { - uint8_t componentId = payload[0]; - uint8_t alive = payload[1]; - - // Component 3 = Face detection (Radxa) - if (componentId == 3) { - faceDetect.setAlive(alive != 0); - } - } - // Forward ALIV as a proper protocol packet over serial - sendPacket(Tag::ALIV, payload, len); - } -} +static WebsocketsServer* server = nullptr; +constexpr uint8_t MAX_SERVER_CLIENTS = 2; +static WebsocketsClient serverClients[MAX_SERVER_CLIENTS]; +static bool serverClientActive[MAX_SERVER_CLIENTS] = {false}; -static void parseProtocolMessage(const uint8_t* data, size_t len) { - // Walk through the message looking for protocol packets - // Format: SYNC0(0xA5) SYNC1(0x5A) TAG(4) LEN(2) SEQ(2) PAYLOAD(N) CRC(2) +// ============================================================================ +// Shared protocol packet parser +// ============================================================================ + +// Parse protocol-framed binary data, call handler for each valid packet +static void parseProtocolPackets(const uint8_t* data, size_t len, + void (*handler)(const char tag[4], const uint8_t* payload, uint16_t payloadLen)) +{ size_t pos = 0; - while (pos + 12 <= len) { // Minimum packet: 2 sync + 4 tag + 2 len + 2 seq + 0 payload + 2 crc = 12 - // Look for sync bytes + while (pos + 12 <= len) { if (data[pos] != 0xA5 || data[pos + 1] != 0x5A) { pos++; continue; } - // Read tag char tag[4]; memcpy(tag, &data[pos + 2], 4); - // Read length (LE) uint16_t payloadLen = data[pos + 6] | (data[pos + 7] << 8); - // Sanity check - size_t totalPacketLen = 2 + 4 + 2 + 2 + payloadLen + 2; // sync + tag + len + seq + payload + crc - if (pos + totalPacketLen > len) { - // Incomplete packet - break; - } + size_t totalPacketLen = 2 + 4 + 2 + 2 + payloadLen + 2; + if (pos + totalPacketLen > len) break; - // Payload starts after sync(2) + tag(4) + len(2) + seq(2) = offset 10 const uint8_t* payload = &data[pos + 10]; - // Verify CRC over tag + len + seq + payload - const uint8_t* crcData = &data[pos + 2]; // starts at tag - uint16_t crcDataLen = 4 + 2 + 2 + payloadLen; // tag + len + seq + payload + // Verify CRC + const uint8_t* crcData = &data[pos + 2]; + uint16_t crcDataLen = 4 + 2 + 2 + payloadLen; uint16_t computed = crc16Compute(crcData, crcDataLen); - uint16_t received = data[pos + 10 + payloadLen] | (data[pos + 10 + payloadLen + 1] << 8); if (computed == received) { - processPacketPayload(tag, payload, payloadLen); + handler(tag, payload, payloadLen); } pos += totalPacketLen; @@ -89,46 +72,84 @@ static void parseProtocolMessage(const uint8_t* data, size_t len) { } // ============================================================================ -// WebSocket callbacks +// Client packet handler (FACE/ALIV from Radxa) // ============================================================================ -static void onMessage(WebsocketsMessage message) { - std::string raw = message.rawData(); - size_t len = raw.size(); - if (len == 0) return; - - const uint8_t* data = reinterpret_cast(raw.data()); - parseProtocolMessage(data, len); +static void handleClientPacket(const char tag[4], const uint8_t* payload, uint16_t len) { + if (memcmp(tag, Tag::FACE, 4) == 0) { + faceDetect.feedPayload(payload, len); + } + else if (memcmp(tag, Tag::ALIV, 4) == 0) { + if (len >= 2) { + uint8_t componentId = payload[0]; + uint8_t alive = payload[1]; + if (componentId == 3) { + faceDetect.setAlive(alive != 0); + } + } + sendPacket(Tag::ALIV, payload, len); + } } -static void onEvent(WebsocketsEvent event, String data) { +// ============================================================================ +// Server packet handler (any command from computer) +// ============================================================================ + +static void handleServerPacket(const char tag[4], const uint8_t* payload, uint16_t len) { + // Route to the standard command dispatcher - handles VSME, MSET, BHVR, SSET, etc. + dispatchCommand(tag, payload, len); +} + +// ============================================================================ +// Client callbacks +// ============================================================================ + +static void onClientMessage(WebsocketsMessage message) { + std::string raw = message.rawData(); + if (raw.empty()) return; + parseProtocolPackets(reinterpret_cast(raw.data()), raw.size(), handleClientPacket); +} + +static void onClientEvent(WebsocketsEvent event, String data) { switch (event) { case WebsocketsEvent::ConnectionOpened: s_connected = true; - Serial.println("[WebSocket] Connected"); + Serial.println("[WS Client] Connected"); break; case WebsocketsEvent::ConnectionClosed: s_connected = false; - Serial.println("[WebSocket] Disconnected"); - break; - case WebsocketsEvent::GotPing: - break; - case WebsocketsEvent::GotPong: - break; - default: + Serial.println("[WS Client] Disconnected"); break; + default: break; } } +// ============================================================================ +// Server client callbacks (created per-client via lambda in loop) +// ============================================================================ + +static void onServerMessage(WebsocketsMessage message) { + std::string raw = message.rawData(); + if (raw.empty()) return; + parseProtocolPackets(reinterpret_cast(raw.data()), raw.size(), handleServerPacket); +} + +static void onServerEvent(WebsocketsEvent event, String data) { + // Connection events are handled in the accept logic +} + // ============================================================================ // Public API // ============================================================================ void websocketSetup() { + Serial.print("[WiFi] SSID: '"); + Serial.print(wifiSettings.ssid); + Serial.println("'"); + WiFi.mode(WIFI_STA); WiFi.begin(wifiSettings.ssid, wifiSettings.password); - Serial.print("[WebSocket] WiFi connecting to "); - Serial.print(wifiSettings.ssid); + Serial.print("[WiFi] Connecting"); int attempts = 0; while (WiFi.status() != WL_CONNECTED && attempts < 30) { delay(500); @@ -137,22 +158,33 @@ void websocketSetup() { } Serial.println(); if (WiFi.status() != WL_CONNECTED) { - Serial.println("[WebSocket] WiFi failed"); + Serial.println("[WiFi] Failed to connect"); return; } - Serial.print("[WebSocket] WiFi OK "); + Serial.print("[WiFi] OK "); Serial.println(WiFi.localIP()); - client.onMessage(onMessage); - client.onEvent(onEvent); + // --- Start outbound client (to Radxa) --- + client.onMessage(onClientMessage); + client.onEvent(onClientEvent); String url = buildWsUrl(); - Serial.println("[WebSocket] Connecting to " + url); + Serial.println("[WS Client] Connecting to " + url); if (client.connect(url)) { s_connected = true; - Serial.println("[WebSocket] Connected"); + Serial.println("[WS Client] Connected"); } else { - Serial.println("[WebSocket] Connect failed (will retry in loop)"); + Serial.println("[WS Client] Connect failed (will retry)"); + } + + // --- Start inbound server (allocated after WiFi to avoid early init issues) --- + server = new WebsocketsServer(); + server->listen(WS_SERVER_PORT); + if (server->available()) { + Serial.print("[WS Server] Listening on port "); + Serial.println(WS_SERVER_PORT); + } else { + Serial.println("[WS Server] Failed to start"); } } @@ -162,6 +194,7 @@ void websocketLoop() { return; } + // --- Outbound client --- if (!s_connected && !client.available()) { unsigned long now = millis(); if (now - lastReconnectAttempt >= RECONNECT_INTERVAL) { @@ -169,7 +202,7 @@ void websocketLoop() { String url = buildWsUrl(); if (client.connect(url)) { s_connected = true; - Serial.println("[WebSocket] Reconnected"); + Serial.println("[WS Client] Reconnected"); } } } @@ -177,6 +210,46 @@ void websocketLoop() { if (client.available()) { client.poll(); } + + // --- Inbound server: accept new connections --- + if (server && server->available() && server->poll()) { + WebsocketsClient newClient = server->accept(); + if (newClient.available()) { + // Find an empty slot + bool accepted = false; + for (uint8_t i = 0; i < MAX_SERVER_CLIENTS; i++) { + if (!serverClientActive[i] || !serverClients[i].available()) { + serverClients[i] = std::move(newClient); + serverClients[i].onMessage(onServerMessage); + serverClients[i].onEvent(onServerEvent); + serverClientActive[i] = true; + Serial.print("[WS Server] Client connected (slot "); + Serial.print(i); + Serial.println(")"); + accepted = true; + break; + } + } + if (!accepted) { + Serial.println("[WS Server] Max clients reached, rejecting"); + newClient.close(); + } + } + } + + // --- Inbound server: poll existing clients --- + for (uint8_t i = 0; i < MAX_SERVER_CLIENTS; i++) { + if (serverClientActive[i]) { + if (serverClients[i].available()) { + serverClients[i].poll(); + } else { + serverClientActive[i] = false; + Serial.print("[WS Server] Client disconnected (slot "); + Serial.print(i); + Serial.println(")"); + } + } + } } bool websocketConnected() { @@ -184,15 +257,21 @@ bool websocketConnected() { } void websocketReconnect() { - // Close existing connection and force reconnect with new settings client.close(); s_connected = false; - // Reconnect WiFi if SSID changed WiFi.disconnect(); WiFi.begin(wifiSettings.ssid, wifiSettings.password); - Serial.print("[WebSocket] Reconnecting WiFi to "); + Serial.print("[WiFi] Reconnecting to "); Serial.println(wifiSettings.ssid); - lastReconnectAttempt = 0; // Force immediate reconnect attempt in loop + lastReconnectAttempt = 0; +} + +uint8_t websocketServerClientCount() { + uint8_t count = 0; + for (uint8_t i = 0; i < MAX_SERVER_CLIENTS; i++) { + if (serverClientActive[i] && serverClients[i].available()) count++; + } + return count; } diff --git a/websocket_client.h b/websocket_client.h index 8c239a6..a9a20c7 100644 --- a/websocket_client.h +++ b/websocket_client.h @@ -1,8 +1,9 @@ #pragma once #include -// WebSocket client: connect to remote device, receive bytes (e.g. FACE packets). -// Extension to the serial protocol - same packet concepts over WebSocket. +// WebSocket client + server +// Client: connects out to Radxa to receive FACE/ALIV packets +// Server: listens for incoming connections to accept any protocol command (VSME, MSET, etc.) // Runtime-configurable WiFi + WebSocket settings (persisted to NVM) struct WiFiSettings { @@ -15,14 +16,20 @@ struct WiFiSettings { extern WiFiSettings wifiSettings; +// WebSocket server port (for incoming commands from computer) +constexpr uint16_t WS_SERVER_PORT = 81; + // Call once from setup() after Serial is ready void websocketSetup(); -// Call every loop() - handles connect, reconnect, and incoming messages +// Call every loop() - handles client + server void websocketLoop(); -// True when connected and ready to send/receive +// True when outbound client is connected bool websocketConnected(); -// Force reconnect (call after settings change) +// Force client reconnect (call after settings change) void websocketReconnect(); + +// Number of server clients currently connected +uint8_t websocketServerClientCount();