realrobotshk
/
HansonServo
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

settings updates for wifi and focus behaviour

websocket
Jake 2026-02-09 23:47:08 +08:00
parent 1ed18624cb
commit d4683aa385
8 changed files with 239 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
PROTOCOL_MIGRATION.md
View File

@ -430,10 +430,12 @@ For each motor:
**Request:**
```
[setting_id: 2 bytes LE]
[value: 2 bytes LE]
[data: N bytes] // 2 bytes for numeric, variable for strings
```
**Response:** `ACK!` on success, `NACK` if unknown setting ID
**Notes:** Writing a WiFi/WebSocket setting triggers an automatic reconnect.
**Dump all settings:**
**Request:** Empty payload (0 bytes)
**Response:** `SSET` packet:
@ -441,15 +443,17 @@ For each motor:
[count: 2 bytes LE]
For each setting:
[setting_id: 2 bytes LE]
[value: 2 bytes LE]
[data_len: 2 bytes LE]
[data: data_len bytes]
```
**Value encoding:**
- `uint8`/`uint16`/`bool`: stored directly as uint16
- `float` (0.0–65.535): stored as `value × 1000` (e.g., 0.15 → 150)
- `int16` (signed): stored as uint16 reinterpret (e.g., -140 → 0xFF74)
- `uint8`/`uint16`/`bool`: `data_len=2`, stored as uint16 LE
- `float` (0.0–65.535): `data_len=2`, stored as `value × 1000` (e.g., 0.15 → 150)
- `int16` (signed): `data_len=2`, stored as uint16 reinterpret (e.g., -140 → 0xFF74)
- `string`: `data_len=N`, raw UTF-8 bytes (no null terminator)
**Focus Behavior Setting IDs:**
**Focus Behavior Setting IDs (0x0500–0x0512):**
| ID | Name | Type | Default | Description |
|----|------|------|---------|-------------|
@ -473,10 +477,20 @@ For each setting:
| `0x0511` | FOCUS_EYE_CENTERING | float×1000 | 30 | Eye centering speed (no face) |
| `0x0512` | FOCUS_NECK_CENTERING | float×1000 | 20 | Neck centering speed (no face) |
**WiFi / WebSocket Setting IDs (0x0600–0x0604):**
| ID | Name | Type | Default | Description |
|----|------|------|---------|-------------|
| `0x0600` | WIFI_SSID | string | "Police Surveillance Van" | WiFi network name (max 32 chars) |
| `0x0601` | WIFI_PASSWORD | string | "ourpassword" | WiFi password (max 64 chars) |
| `0x0602` | WIFI_HOST | string | "192.168.1.206" | WebSocket server IP/hostname (max 63 chars) |
| `0x0603` | WIFI_PORT | uint16 | 5001 | WebSocket server port |
| `0x0604` | WIFI_PATH | string | "/" | WebSocket URL path (max 31 chars) |
**Notes:**
- Settings are persisted to flash on write
- The dump response includes all registered settings (currently Focus only, extensible)
- Future setting ranges (e.g., `0x0600+`) can be added for other behaviors
- All settings are persisted to flash on write
- The dump includes all registered settings with a length prefix per entry
- Setting ranges are extensible: `0x0500` = Focus, `0x0600` = WiFi, future = `0x0700+`
---

11
behaviors.h
View File

@ -88,6 +88,17 @@ namespace SettingID {
constexpr uint16_t FOCUS_FIRST = 0x0500;
constexpr uint16_t FOCUS_LAST = 0x0512;
constexpr uint16_t FOCUS_COUNT = FOCUS_LAST - FOCUS_FIRST + 1;
// WiFi / WebSocket settings (0x0600 range)
constexpr uint16_t WIFI_SSID = 0x0600; // string (max 32 chars)
constexpr uint16_t WIFI_PASSWORD = 0x0601; // string (max 64 chars)
constexpr uint16_t WIFI_HOST = 0x0602; // string (max 63 chars)
constexpr uint16_t WIFI_PORT = 0x0603; // uint16
constexpr uint16_t WIFI_PATH = 0x0604; // string (max 31 chars)
constexpr uint16_t WIFI_FIRST = 0x0600;
constexpr uint16_t WIFI_LAST = 0x0604;
constexpr uint16_t WIFI_COUNT = WIFI_LAST - WIFI_FIRST + 1;
}
// Tuneable settings - all values exposed for external adjustment

127
commands.cpp
View File

@ -2,6 +2,7 @@
#include "nodegraph.h"
#include "sensors.h"
#include "behaviors.h"
#include "websocket_client.h"
#include "esp_system.h"
#include "soc/rtc_cntl_reg.h"
#include <vector>
@ -832,9 +833,18 @@ void handleMotorStream(const uint8_t* payload, uint16_t len) {
// System Handlers
// ============================================================================
// Settings (SSET) - Read/write individual settings by ID
// Supports numeric (uint16) and string values.
// Dump format: [count:2][id:2][len:2][data:N]...
// Write format: [id:2][data:N] (len=2 for uint16, len>2 for strings)
// ============================================================================
// Get a setting value by ID. Returns true if valid ID, fills value.
// Returns true if this setting ID is a string type
static bool isStringSetting(uint16_t id) {
return id == SettingID::WIFI_SSID || id == SettingID::WIFI_PASSWORD ||
id == SettingID::WIFI_HOST || id == SettingID::WIFI_PATH;
}
// Get a numeric setting value by ID. Returns true if valid.
static bool getSettingValue(uint16_t id, uint16_t& value) {
FocusSettings& fs = focusBehavior.getSettings();
@ -858,12 +868,25 @@ static bool getSettingValue(uint16_t id, uint16_t& value) {
case SettingID::FOCUS_NECK_INVERT: value = fs.neckInvert ? 1 : 0; break;
case SettingID::FOCUS_EYE_CENTERING: value = (uint16_t)(fs.eyeCenteringSpeed * 1000.0f); break;
case SettingID::FOCUS_NECK_CENTERING: value = (uint16_t)(fs.neckCenteringSpeed * 1000.0f); break;
// WiFi numeric
case SettingID::WIFI_PORT: value = wifiSettings.port; break;
default: return false;
}
return true;
}
// Set a setting value by ID. Returns true if valid ID.
// Get a string setting by ID. Returns pointer to string, or nullptr.
static const char* getSettingString(uint16_t id) {
switch (id) {
case SettingID::WIFI_SSID: return wifiSettings.ssid;
case SettingID::WIFI_PASSWORD: return wifiSettings.password;
case SettingID::WIFI_HOST: return wifiSettings.host;
case SettingID::WIFI_PATH: return wifiSettings.path;
default: return nullptr;
}
}
// Set a numeric setting value by ID. Returns true if valid.
static bool setSettingValue(uint16_t id, uint16_t value) {
FocusSettings& fs = focusBehavior.getSettings();
@ -887,30 +910,91 @@ static bool setSettingValue(uint16_t id, uint16_t value) {
case SettingID::FOCUS_NECK_INVERT: fs.neckInvert = value != 0; break;
case SettingID::FOCUS_EYE_CENTERING: fs.eyeCenteringSpeed = (float)value / 1000.0f; break;
case SettingID::FOCUS_NECK_CENTERING: fs.neckCenteringSpeed = (float)value / 1000.0f; break;
// WiFi numeric
case SettingID::WIFI_PORT: wifiSettings.port = value; break;
default: return false;
}
return true;
}
// Set a string setting by ID. Returns true if valid.
static bool setSettingString(uint16_t id, const char* str, uint16_t strLen) {
switch (id) {
case SettingID::WIFI_SSID:
if (strLen >= sizeof(wifiSettings.ssid)) strLen = sizeof(wifiSettings.ssid) - 1;
memcpy(wifiSettings.ssid, str, strLen);
wifiSettings.ssid[strLen] = '\0';
break;
case SettingID::WIFI_PASSWORD:
if (strLen >= sizeof(wifiSettings.password)) strLen = sizeof(wifiSettings.password) - 1;
memcpy(wifiSettings.password, str, strLen);
wifiSettings.password[strLen] = '\0';
break;
case SettingID::WIFI_HOST:
if (strLen >= sizeof(wifiSettings.host)) strLen = sizeof(wifiSettings.host) - 1;
memcpy(wifiSettings.host, str, strLen);
wifiSettings.host[strLen] = '\0';
break;
case SettingID::WIFI_PATH:
if (strLen >= sizeof(wifiSettings.path)) strLen = sizeof(wifiSettings.path) - 1;
memcpy(wifiSettings.path, str, strLen);
wifiSettings.path[strLen] = '\0';
break;
default: return false;
}
return true;
}
// Helper: write one setting entry into a buffer at offset. Returns new offset.
// Format: [id:2][len:2][data:N]
static uint16_t writeDumpEntry(uint8_t* buf, uint16_t offset, uint16_t id, const uint8_t* data, uint16_t dataLen) {
buf[offset++] = id & 0xFF;
buf[offset++] = (id >> 8) & 0xFF;
buf[offset++] = dataLen & 0xFF;
buf[offset++] = (dataLen >> 8) & 0xFF;
memcpy(&buf[offset], data, dataLen);
offset += dataLen;
return offset;
}
void handleSettingsSet(const uint8_t* payload, uint16_t len) {
if (len == 0) {
// Dump all settings: respond with SSET containing [count:2][id:2][value:2]...
// Collect all focus settings
uint8_t buf[2 + SettingID::FOCUS_COUNT * 4]; // count(2) + N × (id(2) + value(2))
// Dump all settings: [count:2][id:2][len:2][data:N]...
// Max size: 2 + (FOCUS_COUNT + WIFI_COUNT) * (2+2+64) ≈ 1600 bytes, safe for stack
uint8_t buf[1600];
uint16_t count = 0;
uint16_t offset = 2; // Skip count bytes, fill in later
uint16_t offset = 2; // Skip count, fill later
// Dump all numeric focus settings
for (uint16_t id = SettingID::FOCUS_FIRST; id <= SettingID::FOCUS_LAST; id++) {
uint16_t value;
if (getSettingValue(id, value)) {
buf[offset++] = id & 0xFF;
buf[offset++] = (id >> 8) & 0xFF;
buf[offset++] = value & 0xFF;
buf[offset++] = (value >> 8) & 0xFF;
uint8_t vbuf[2] = { (uint8_t)(value & 0xFF), (uint8_t)((value >> 8) & 0xFF) };
offset = writeDumpEntry(buf, offset, id, vbuf, 2);
count++;
}
}
// Dump WiFi string settings
for (uint16_t id = SettingID::WIFI_FIRST; id <= SettingID::WIFI_LAST; id++) {
if (isStringSetting(id)) {
const char* str = getSettingString(id);
if (str) {
uint16_t slen = strlen(str);
offset = writeDumpEntry(buf, offset, id, (const uint8_t*)str, slen);
count++;
}
} else {
// Numeric WiFi setting (port)
uint16_t value;
if (getSettingValue(id, value)) {
uint8_t vbuf[2] = { (uint8_t)(value & 0xFF), (uint8_t)((value >> 8) & 0xFF) };
offset = writeDumpEntry(buf, offset, id, vbuf, 2);
count++;
}
}
}
// Write count at the start
buf[0] = count & 0xFF;
buf[1] = (count >> 8) & 0xFF;
@ -919,15 +1003,28 @@ void handleSettingsSet(const uint8_t* payload, uint16_t len) {
return;
}
if (len == 4) {
// Set a single setting: [id:2 LE][value:2 LE]
if (len >= 4) {
// Write a setting: [id:2 LE][data:N]
uint16_t id = payload[0] | (payload[1] << 8);
uint16_t value = payload[2] | (payload[3] << 8);
uint16_t dataLen = len - 2;
const uint8_t* data = &payload[2];
if (setSettingValue(id, value)) {
// Persist to flash
bool ok = false;
bool needsReconnect = false;
if (isStringSetting(id)) {
ok = setSettingString(id, (const char*)data, dataLen);
needsReconnect = true;
} else if (dataLen == 2) {
uint16_t value = data[0] | (data[1] << 8);
ok = setSettingValue(id, value);
if (ok && id == SettingID::WIFI_PORT) needsReconnect = true;
}
if (ok) {
config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior);
sendAck(Tag::SSET);
if (needsReconnect) websocketReconnect();
} else {
sendNack(Tag::SSET, "Unknown setting ID");
}

50
robotconfig.cpp
View File

@ -1,5 +1,6 @@
#include "robotconfig.h"
#include "behaviors.h"
#include "websocket_client.h"
#include <FFat.h>
uint16_t RobotConfig::getMotorPosition(uint8_t motorID) const {
@ -268,6 +269,21 @@ static float readFloat(File& f) {
return v;
}
static void writeStr(File& f, const char* s, uint8_t maxLen) {
uint8_t len = strlen(s);
if (len > maxLen) len = maxLen;
f.write(len);
f.write((const uint8_t*)s, len);
}
static void readStr(File& f, char* dst, uint8_t maxLen) {
uint8_t len = f.read();
if (len > maxLen) len = maxLen;
f.readBytes(dst, len);
dst[len] = '\0';
// Skip extra bytes if stored length exceeded maxLen
}
bool RobotConfig::saveToFFatV2(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior, FocusBehavior* focusBehavior) const {
File file = FFat.open(path, FILE_WRITE);
if (!file) return false;
@ -432,6 +448,21 @@ bool RobotConfig::saveToFFatV2(const char* path, BehaviorManager* behaviorManage
settingCount++;
}
// Setting 8: WiFi / WebSocket Settings
{
file.write((uint8_t)(KEY_WIFI_SETTINGS & 0xFF));
file.write((uint8_t)((KEY_WIFI_SETTINGS >> 8) & 0xFF));
file.write(TYPE_WIFI_SETTINGS);
writeStr(file, wifiSettings.ssid, 32);
writeStr(file, wifiSettings.password, 64);
writeStr(file, wifiSettings.host, 63);
writeU16(file, wifiSettings.port);
writeStr(file, wifiSettings.path, 31);
settingCount++;
}
// Write setting count at the beginning
size_t endPos = file.position();
file.seek(countPos);
@ -617,6 +648,25 @@ bool RobotConfig::loadFromFFatV2(const char* path, BehaviorManager* behaviorMana
break;
}
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);
Serial.println("[Config] WiFi settings loaded");
} else {
// Skip: 5 length-prefixed strings + 2 byte port - can't know exact size
// Best effort: skip based on stored lengths
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(); }
}
break;
}
default:
// Unknown key - skip based on type
switch (type) {

4
robotconfig.h
View File

@ -29,6 +29,9 @@ enum ConfigKey : uint16_t {
// Focus behavior settings
KEY_FOCUS_SETTINGS = 0x0500,
// WiFi / WebSocket settings
KEY_WIFI_SETTINGS = 0x0600,
// Future extensible settings
KEY_SERIAL_BAUD = 0x0400,
KEY_MOTOR_UPDATE_INTERVAL = 0x0401,
@ -49,6 +52,7 @@ enum ConfigType : uint8_t {
TYPE_BEHAVIOR_STATES = 0x0B, // Special type for behavior state array
TYPE_VISEME_ARRAY = 0x0C, // Special type for viseme array
TYPE_FOCUS_SETTINGS = 0x0D, // Focus behavior settings blob
TYPE_WIFI_SETTINGS = 0x0E, // WiFi/WebSocket settings blob
};
struct FirmwareVersion {

3
sensors.cpp
View File

@ -418,5 +418,4 @@ void SensorManager::sendFacePacket() {
uint8_t payload[64]; // 1 + (9 * FACE_MAX_FACES)
uint16_t len = faceDetect.packPayload(payload);
sendPacket(Tag::FACE, payload, len);
}
}

29
websocket_client.cpp
View File

@ -6,11 +6,17 @@
using namespace websockets;
WiFiSettings wifiSettings; // Global runtime instance
static WebsocketsClient client;
static bool s_connected = false;
static unsigned long lastReconnectAttempt = 0;
constexpr unsigned long RECONNECT_INTERVAL = 5000;
static String buildWsUrl() {
return "ws://" + String(wifiSettings.host) + ":" + String(wifiSettings.port) + String(wifiSettings.path);
}
// ============================================================================
// Packet parsing for WebSocket binary messages
// Uses the same protocol format: 0xA5 0x5A TAG(4) LEN(2) SEQ(2) PAYLOAD(N) CRC(2)
@ -120,8 +126,9 @@ static void onEvent(WebsocketsEvent event, String data) {
void websocketSetup() {
WiFi.mode(WIFI_STA);
WiFi.begin(WebSocketConfig::WIFI_SSID, WebSocketConfig::WIFI_PASSWORD);
Serial.print("[WebSocket] WiFi connecting");
WiFi.begin(wifiSettings.ssid, wifiSettings.password);
Serial.print("[WebSocket] WiFi connecting to ");
Serial.print(wifiSettings.ssid);
int attempts = 0;
while (WiFi.status() != WL_CONNECTED && attempts < 30) {
delay(500);
@ -139,7 +146,7 @@ void websocketSetup() {
client.onMessage(onMessage);
client.onEvent(onEvent);
String url = "ws://" + String(WebSocketConfig::HOST) + ":" + String(WebSocketConfig::PORT) + String(WebSocketConfig::PATH);
String url = buildWsUrl();
Serial.println("[WebSocket] Connecting to " + url);
if (client.connect(url)) {
s_connected = true;
@ -159,7 +166,7 @@ void websocketLoop() {
unsigned long now = millis();
if (now - lastReconnectAttempt >= RECONNECT_INTERVAL) {
lastReconnectAttempt = now;
String url = "ws://" + String(WebSocketConfig::HOST) + ":" + String(WebSocketConfig::PORT) + String(WebSocketConfig::PATH);
String url = buildWsUrl();
if (client.connect(url)) {
s_connected = true;
Serial.println("[WebSocket] Reconnected");
@ -175,3 +182,17 @@ void websocketLoop() {
bool websocketConnected() {
return s_connected && client.available();
}
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.println(wifiSettings.ssid);
lastReconnectAttempt = 0; // Force immediate reconnect attempt in loop
}

20
websocket_client.h
View File

@ -4,13 +4,16 @@
// WebSocket client: connect to remote device, receive bytes (e.g. FACE packets).
// Extension to the serial protocol - same packet concepts over WebSocket.
namespace WebSocketConfig {
constexpr const char* WIFI_SSID = "Police Surveillance Van";
constexpr const char* WIFI_PASSWORD = "ourpassword";
constexpr const char* HOST = "192.168.1.206"; // Change to remote device IP
constexpr uint16_t PORT = 5001; // Change to remote port
constexpr const char* PATH = "/"; // WebSocket path
}
// Runtime-configurable WiFi + WebSocket settings (persisted to NVM)
struct WiFiSettings {
char ssid[33] = "Police Surveillance Van";
char password[65] = "ourpassword";
char host[64] = "192.168.1.206";
uint16_t port = 5001;
char path[32] = "/";
};
extern WiFiSettings wifiSettings;
// Call once from setup() after Serial is ready
void websocketSetup();
@ -20,3 +23,6 @@ void websocketLoop();
// True when connected and ready to send/receive
bool websocketConnected();
// Force reconnect (call after settings change)
void websocketReconnect();