can now recieve visemes through websocket
parent
d4683aa385
commit
263bb2590f
|
|
@ -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://<robot-ip>: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.
|
||||
|
||||
|
|
|
|||
|
|
@ -284,7 +284,7 @@ private:
|
|||
std::vector<Viseme> 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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
// ============================================================================
|
||||
|
|
|
|||
|
|
@ -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<uint8_t> allMotorIDs;
|
||||
for (const Motor& motor : config.motors) {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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://<ip>: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('<HH', length, _seq)
|
||||
crc_data = header_tail + payload
|
||||
crc = crc16_ccitt(crc_data)
|
||||
packet = SYNC + crc_data + struct.pack('<H', crc)
|
||||
_seq = (_seq + 1) & 0xFFFF
|
||||
return packet
|
||||
|
||||
def parse_packet(data: bytes):
|
||||
"""Parse a protocol packet, return (tag, payload) or None."""
|
||||
if len(data) < 12:
|
||||
return None
|
||||
if data[0:2] != SYNC:
|
||||
return None
|
||||
tag = data[2:6].decode('ascii', errors='replace')
|
||||
length = struct.unpack('<H', data[6:8])[0]
|
||||
# seq = struct.unpack('<H', data[8:10])[0]
|
||||
if len(data) < 10 + length + 2:
|
||||
return None
|
||||
payload = data[10:10 + length]
|
||||
received_crc = struct.unpack('<H', data[10 + length:12 + length])[0]
|
||||
computed_crc = crc16_ccitt(data[2:10 + length])
|
||||
if received_crc != computed_crc:
|
||||
return None
|
||||
return tag, payload
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Viseme table
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
VISEMES = {
|
||||
0: 'sil',
|
||||
1: 'AA',
|
||||
2: 'AE',
|
||||
3: 'AH',
|
||||
4: 'AO',
|
||||
5: 'EH',
|
||||
6: 'IH',
|
||||
7: 'IY',
|
||||
8: 'OW',
|
||||
9: 'UH',
|
||||
10: 'UW',
|
||||
}
|
||||
|
||||
KEY_MAP = {
|
||||
'0': 0, '1': 1, '2': 2, '3': 3, '4': 4,
|
||||
'5': 5, '6': 6, '7': 7, '8': 8, '9': 9,
|
||||
'a': 10, 'b': 11, 'c': 12, 'd': 13, 'e': 14,
|
||||
'f': 15, 'g': 16, 'h': 17, 'i': 18, 'j': 19,
|
||||
' ': 0,
|
||||
}
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Main
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
async def main():
|
||||
ip = sys.argv[1] if len(sys.argv) > 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('<H', payload[pos+1:pos+3])[0]
|
||||
motors.append(f'{mid}={mpos}')
|
||||
pos += 3
|
||||
print(f' [{vid:2d}] "{label}" motors: {", ".join(motors) if motors else "(none)"}')
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Cross-platform single key input
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def get_key() -> 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.')
|
||||
|
|
@ -1,6 +1,7 @@
|
|||
#include "websocket_client.h"
|
||||
#include "sensors.h"
|
||||
#include "protocol.h"
|
||||
#include "commands.h"
|
||||
#include <ArduinoWebsockets.h>
|
||||
#include <WiFi.h>
|
||||
|
||||
|
|
@ -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<const uint8_t*>(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<const uint8_t*>(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<const uint8_t*>(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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,8 +1,9 @@
|
|||
#pragma once
|
||||
#include <Arduino.h>
|
||||
|
||||
// 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();
|
||||
|
|
|
|||
Loading…
Reference in New Issue