From cdd7a39e0118864e77f88e57c74c20094329130b Mon Sep 17 00:00:00 2001 From: Jake Date: Sat, 7 Feb 2026 23:51:20 +0800 Subject: [PATCH] implemented websocket data, just face tracking now. focus behaviour uses face tracking instead of radar --- PROTOCOL_MIGRATION.md | 39 +++++++- behaviors.cpp | 214 ++++++++++++++++-------------------------- behaviors.h | 110 ++++++++++++++-------- ls_firmware.ino | 13 ++- protocol.h | 2 + sensors.cpp | 82 ++++++++++++++++ sensors.h | 47 ++++++++++ websocket_client.cpp | 177 ++++++++++++++++++++++++++++++++++ websocket_client.h | 22 +++++ 9 files changed, 530 insertions(+), 176 deletions(-) create mode 100644 websocket_client.cpp create mode 100644 websocket_client.h diff --git a/PROTOCOL_MIGRATION.md b/PROTOCOL_MIGRATION.md index 452d65a..c891296 100644 --- a/PROTOCOL_MIGRATION.md +++ b/PROTOCOL_MIGRATION.md @@ -112,6 +112,8 @@ ushort Crc16Ccitt(byte[] data) | `VSET` | Set motor positions for a viseme | | `VSME` | Trigger viseme (fire-and-forget) | | `STAT` | System state/heartbeat | +| `FACE` | Face detection data (Radxa → host/robot, via WiFi) | +| `ALIV` | Remote component alive/heartbeat (component ID + status) | | `ACK!` | Acknowledge (success) | | `NACK` | Negative acknowledge (failure) | | `BOOT` | Enter bootloader | @@ -277,6 +279,39 @@ For each of 3 targets: ``` *Sent automatically when radar streaming is enabled* +#### `FACE` - Face Detection Data (Radxa → host/robot via WiFi) +**Payload:** +``` +[face_count: 1 byte] // 0–N faces +For each face: + [x: 2 bytes LE, signed] // center-relative pixels (0,0 = image center) + [y: 2 bytes LE, signed] + [w: 2 bytes LE] // bounding box width + [h: 2 bytes LE] // bounding box height + [conf: 1 byte] // confidence × 255 (0–255) +``` + +**Notes:** +- Sent from Radxa over WebSocket when faces are detected +- Radxa runs `face_server.py` (WebSocket server); robot/host connects to receive packets +- x, y are offset from image center: positive = right/down, negative = left/up +- Same packet format as serial (0xA5 0x5A + TAG + LENGTH + SEQ + PAYLOAD + CRC16) + +#### `ALIV` - Remote Component Alive (Radxa/other → host/robot via WiFi) +**Payload:** +``` +[component_id: 1 byte] // Assigned ID for each remote component +[alive: 1 byte] // 0 = not alive / stream disconnected, 1 = alive +// Future: extended payload for other devices +``` + +**Component IDs:** +- `3` = Face detection (Radxa `face_server.py` – alive = MJPEG stream connected) + +**Notes:** +- Sent every 2 seconds by each remote component +- Use `alive` to detect when a component has lost its connection and is retrying + --- ### Behaviors @@ -290,7 +325,7 @@ For each of 3 targets: **Response:** `ACK!` on success, `NACK` on failure **Behavior IDs:** -- `1` = Focus (radar tracking with eye motors 14 & 15) +- `1` = Focus (face tracking with eye motors 14 & 15) - `2` = Idle (perlin noise motion for all motors, ±500 range from center) - `3` = Viseme (mouth motor control for speech) @@ -400,6 +435,8 @@ For each motor: - Bit 2: Motor streaming active - Bit 3: IMU streaming active - Bit 4: Radar streaming active +- Bit 5: Face streaming active +- Bit 6: Face detection remote alive *Sent automatically every 1 second* diff --git a/behaviors.cpp b/behaviors.cpp index 16d555a..4b5d739 100644 --- a/behaviors.cpp +++ b/behaviors.cpp @@ -36,176 +36,124 @@ void Behavior::clearMotors() { FocusBehavior::FocusBehavior() { isActive = false; - eyePosition = EYE_POSITION_CENTER; - neckPosition = NECK_POSITION_CENTER; - targetEyePosition = EYE_POSITION_CENTER; - targetNeckPosition = NECK_POSITION_CENTER; - targetDetectedTime = 0; - neckStartTime = 0; - neckRotating = false; + eyePosition = settings.eyeCenter; + neckPosition = settings.neckCenter; + neckNormalized = 0.0f; + faceDetectedTime = 0; + faceWasPresent = false; - // Add motors 14, 15, and 27 to controlled list - addMotor(FOCUS_MOTOR_1); - addMotor(FOCUS_MOTOR_2); - addMotor(NECK_MOTOR); + // Add motors to controlled list + addMotor(settings.eyeMotor1); + addMotor(settings.eyeMotor2); + addMotor(settings.neckMotor); } bool FocusBehavior::update() { - // Check radar for valid targets - uint8_t targetCount = radar.getTargetCount(); + uint8_t faceCount = faceDetect.getFaceCount(); unsigned long now = millis(); - if (targetCount == 0 || !radar.getTarget(0).valid) { - // No target - return to center + // ---- No face detected ---- + if (faceCount == 0 || !faceDetect.getFace(0).valid) { isActive = false; - targetEyePosition = EYE_POSITION_CENTER; - targetNeckPosition = NECK_POSITION_CENTER; - targetDetectedTime = 0; - neckRotating = false; + faceWasPresent = false; - // Smoothly interpolate eyes to center - eyePosition = lerp(eyePosition, EYE_POSITION_CENTER, INTERPOLATION_SPEED); - - // Keep neck at center (no movement) - neckPosition = NECK_POSITION_CENTER; + // Smoothly return eyes and neck to center + eyePosition = lerp(eyePosition, settings.eyeCenter, settings.eyeCenteringSpeed); + neckNormalized = lerpf(neckNormalized, 0.0f, settings.neckCenteringSpeed); + neckPosition = normalizedToServo( + settings.neckInvert ? -neckNormalized : neckNormalized, + settings.neckCenter, settings.neckMin, settings.neckMax + ); return false; } - // Get first valid target - const RadarTarget& target = radar.getTarget(0); - - if (!target.valid) { - isActive = false; - targetEyePosition = EYE_POSITION_CENTER; - targetNeckPosition = NECK_POSITION_CENTER; - targetDetectedTime = 0; - neckRotating = false; - return false; - } - - // Active tracking - calculate target positions from radar angle + // ---- Face detected ---- isActive = true; - uint16_t targetEyePos = calculateEyePositionFromRadarAngle(target.angle); + const DetectedFace& face = faceDetect.getFace(0); + + // Track when we first saw a face (for neck delay) + if (!faceWasPresent) { + faceDetectedTime = now; + faceWasPresent = true; + } + + // Normalize face x to -1..+1 + float faceNorm = (float)face.x / settings.faceXMax; + if (faceNorm < -1.0f) faceNorm = -1.0f; + if (faceNorm > 1.0f) faceNorm = 1.0f; + + // ---- Neck: smoothly follow the target after a delay ---- + float neckTarget = faceNorm * settings.neckContribution; - // Eyes track immediately - targetEyePosition = targetEyePos; - - // Neck disabled for now - keep it centered - targetNeckPosition = NECK_POSITION_CENTER; - neckRotating = false; - - // Smoothly interpolate eye position toward target - eyePosition = lerp(eyePosition, targetEyePosition, INTERPOLATION_SPEED); - - // Keep neck at center (no movement) - neckPosition = NECK_POSITION_CENTER; + if (now - faceDetectedTime >= settings.neckDelayMs) { + // Neck is allowed to move - smoothly interpolate toward target + neckNormalized = lerpf(neckNormalized, neckTarget, settings.neckSpeed); + } + // else: neck stays where it is during the delay period + + // Convert neck normalized position to servo units + neckPosition = normalizedToServo( + settings.neckInvert ? -neckNormalized : neckNormalized, + settings.neckCenter, settings.neckMin, settings.neckMax + ); + + // ---- Eyes: dart to the remainder that the neck hasn't covered ---- + // The eyes compensate for whatever offset the neck hasn't reached yet + // As the neck catches up, this remainder shrinks toward 0 (eyes center) + float eyeNorm = faceNorm - neckNormalized; + // Clamp eye normalized to -1..+1 + if (eyeNorm < -1.0f) eyeNorm = -1.0f; + if (eyeNorm > 1.0f) eyeNorm = 1.0f; + + // Convert to servo position and interpolate quickly + uint16_t eyeTarget = normalizedToServo(eyeNorm, settings.eyeCenter, settings.eyeMin, settings.eyeMax); + eyePosition = lerp(eyePosition, eyeTarget, settings.eyeSpeed); + return true; } bool FocusBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) { - // Provide position for eyes (motors 14 and 15) - if (motorID == FOCUS_MOTOR_1 || motorID == FOCUS_MOTOR_2) { + if (motorID == settings.eyeMotor1 || motorID == settings.eyeMotor2) { position = eyePosition; return true; } - - // Provide position for neck (motor 27) - if (motorID == NECK_MOTOR) { + if (motorID == settings.neckMotor) { position = neckPosition; return true; } - return false; } -uint16_t FocusBehavior::calculateEyePositionFromRadarAngle(float radarAngle) { - // Calculate eye motor position from radar angle (in degrees) - // Angle range: -50 to +50 degrees, mapped to full eye range (1700-2500, center 2200) - - constexpr float ANGLE_MIN = -50.0f; - constexpr float ANGLE_MAX = 50.0f; - - // Clamp angle to -50 to +50 range - if (radarAngle < ANGLE_MIN) radarAngle = ANGLE_MIN; - if (radarAngle > ANGLE_MAX) radarAngle = ANGLE_MAX; - - // Normalize angle to -1.0 to 1.0 range - float normalized = radarAngle / 50.0f; - - // Calculate range from center in each direction - // Left range: 2200 - 1700 = 500, Right range: 2500 - 2200 = 300 - float rangeLeft = (float)(EYE_POSITION_CENTER - EYE_POSITION_MIN); // 500 - float rangeRight = (float)(EYE_POSITION_MAX - EYE_POSITION_CENTER); // 300 - - // Use different ranges for left (negative) and right (positive) to use full range - float positionFloat; - if (normalized < 0.0f) { - // Negative angle: use left range (500 units) - positionFloat = (float)EYE_POSITION_CENTER + (normalized * rangeLeft); +uint16_t FocusBehavior::normalizedToServo(float n, uint16_t center, uint16_t min, uint16_t max) const { + // Map a normalized value (-1..+1) to servo range, handling asymmetric ranges + float rangeNeg = (float)(center - min); + float rangePos = (float)(max - center); + + float posFloat; + if (n < 0.0f) { + posFloat = (float)center + (n * rangeNeg); } else { - // Positive angle: use right range (300 units) - positionFloat = (float)EYE_POSITION_CENTER + (normalized * rangeRight); + posFloat = (float)center + (n * rangePos); } - - // Convert to int16_t first to handle negative values, then clamp - int16_t position = (int16_t)positionFloat; - // Clamp to valid range (1700 to 2500) - if (position < (int16_t)EYE_POSITION_MIN) position = (int16_t)EYE_POSITION_MIN; - if (position > (int16_t)EYE_POSITION_MAX) position = (int16_t)EYE_POSITION_MAX; - - return (uint16_t)position; + int16_t pos = (int16_t)posFloat; + if (pos < (int16_t)min) pos = (int16_t)min; + if (pos > (int16_t)max) pos = (int16_t)max; + return (uint16_t)pos; } -uint16_t FocusBehavior::calculateNeckPositionFromRadarAngle(float radarAngle) { - // Calculate neck motor position from radar angle (in degrees) - // Angle range: approximately -45 to +45 degrees (typical radar FOV) - // Map to neck motor position range: 1000 to 3000 (center 2000) - // NOTE: Rotation is inverted for neck motor - - // Clamp angle to reasonable range (can extend later if needed) - constexpr float ANGLE_MIN = -45.0f; - constexpr float ANGLE_MAX = 45.0f; - - if (radarAngle < ANGLE_MIN) radarAngle = ANGLE_MIN; - if (radarAngle > ANGLE_MAX) radarAngle = ANGLE_MAX; - - // Normalize angle to -1.0 to 1.0 range, then invert for neck motor - float normalizedAngle = -(radarAngle / ANGLE_MAX); - - // Calculate range from center - float rangeLeft = NECK_POSITION_CENTER - NECK_POSITION_MIN; // 1000 - float rangeRight = NECK_POSITION_MAX - NECK_POSITION_CENTER; // 1000 - - uint16_t position; - if (normalizedAngle < 0.0f) { - // Left side: use left range - position = NECK_POSITION_CENTER + (uint16_t)(normalizedAngle * rangeLeft); - } else { - // Right side: use right range - position = NECK_POSITION_CENTER + (uint16_t)(normalizedAngle * rangeRight); - } - - // Clamp to valid range - if (position < NECK_POSITION_MIN) position = NECK_POSITION_MIN; - if (position > NECK_POSITION_MAX) position = NECK_POSITION_MAX; - - return position; +float FocusBehavior::lerpf(float current, float target, float t) { + float diff = target - current; + if (fabs(diff) < 0.001f) return target; + return current + diff * t; } uint16_t FocusBehavior::lerp(uint16_t current, uint16_t target, float t) { - // Linear interpolation with clamping to prevent overshoot int16_t diff = (int16_t)target - (int16_t)current; - int16_t delta = (int16_t)(diff * t); - - // If difference is very small, snap to target - if (abs(diff) < 2) { - return target; - } - - return (uint16_t)(current + delta); + if (abs(diff) < 2) return target; + return (uint16_t)((int16_t)current + (int16_t)(diff * t)); } // ============================================================================ diff --git a/behaviors.h b/behaviors.h index fadce30..01ea3a3 100644 --- a/behaviors.h +++ b/behaviors.h @@ -10,7 +10,7 @@ // ============================================================================ enum BehaviorID : uint8_t { - BEHAVIOR_FOCUS = 1, // Focus behavior (radar tracking) + BEHAVIOR_FOCUS = 1, // Focus behavior (face tracking) BEHAVIOR_IDLE = 2, // Idle behavior (perlin noise for all motors) BEHAVIOR_VISEME = 3, // Viseme behavior (mouth motor positions) }; @@ -49,59 +49,87 @@ protected: }; // ============================================================================ -// Focus Behavior - Tracks radar targets with eyes/neck +// Focus Behavior - Tracks faces with eyes/neck via FaceDetect sensor // ============================================================================ +// Tuneable settings - all values exposed for external adjustment +struct FocusSettings { + // Motor IDs + uint8_t eyeMotor1 = 14; + uint8_t eyeMotor2 = 15; + uint8_t neckMotor = 27; + + // Eye motor position range + uint16_t eyeCenter = 2200; + uint16_t eyeMin = 1700; + uint16_t eyeMax = 2500; + + // Neck motor position range + uint16_t neckCenter = 2000; + uint16_t neckMin = 1000; + uint16_t neckMax = 3000; + + // Face detection x range (pixels, center-relative) + float faceXMin = -140.0f; + float faceXMax = 140.0f; + + // Interpolation speeds (0-1 per update, higher = faster) + float eyeSpeed = 0.15f; // Eyes dart quickly to target + float neckSpeed = 0.02f; // Neck follows smoothly / slowly + float eyeReturnSpeed = 0.05f; // Speed eyes center as neck catches up + + // Neck starts moving after this delay (ms) from first detecting a target + unsigned long neckDelayMs = 500; + + // How much of the face offset the neck should try to cover (0-1) + // 1.0 = neck tries to fully face the target, 0.5 = neck covers half + float neckContribution = 0.7f; + + // Invert neck direction (set true if neck motor is wired backwards) + bool neckInvert = true; + + // Return-to-center speeds when no face detected + float eyeCenteringSpeed = 0.03f; + float neckCenteringSpeed = 0.02f; +}; + class FocusBehavior : public Behavior { public: FocusBehavior(); - // Update behavior - check radar for targets + // Update behavior - check face detection for targets bool update() override; // Get motor position for a controlled motor bool getMotorPosition(uint8_t motorID, uint16_t& position) override; -private: - bool isActive; - uint16_t eyePosition; // Current interpolated position for motors 14 and 15 - uint16_t neckPosition; // Current interpolated position for motor 27 - - // Target positions - uint16_t targetEyePosition; - uint16_t targetNeckPosition; - - // Timing - unsigned long targetDetectedTime; // When target was first detected - unsigned long neckStartTime; // When neck rotation should start - bool neckRotating; // Whether neck is actively rotating - - // Configuration - static constexpr uint8_t FOCUS_MOTOR_1 = 14; // Eye motor 1 - static constexpr uint8_t FOCUS_MOTOR_2 = 15; // Eye motor 2 - static constexpr uint8_t NECK_MOTOR = 27; // Neck motor - - // Eye motor position range (motors 14 and 15) - static constexpr uint16_t EYE_POSITION_CENTER = 2200; - static constexpr uint16_t EYE_POSITION_MIN = 1700; - static constexpr uint16_t EYE_POSITION_MAX = 2500; - - // Neck motor position range (motor 27) - static constexpr uint16_t NECK_POSITION_CENTER = 2000; - static constexpr uint16_t NECK_POSITION_MIN = 1000; - static constexpr uint16_t NECK_POSITION_MAX = 3000; - - static constexpr unsigned long NECK_DELAY_MS = 1000; // 1 second delay before neck moves - static constexpr float INTERPOLATION_SPEED = 0.05f; // Smooth interpolation factor for eyes (0-1, higher = faster) - static constexpr float NECK_INTERPOLATION_SPEED = 0.03f; // Slower interpolation for neck (smoother motion) - static constexpr float EYE_CENTERING_SPEED = 0.03f; // Speed at which eyes center when neck rotates + // Access settings for external tuning + FocusSettings& getSettings() { return settings; } + const FocusSettings& getSettings() const { return settings; } - // Calculate motor position from radar angle (in degrees) - uint16_t calculateEyePositionFromRadarAngle(float radarAngle); - uint16_t calculateNeckPositionFromRadarAngle(float radarAngle); +private: + FocusSettings settings; + + bool isActive; - // Smooth interpolation helper (linear interpolation) - uint16_t lerp(uint16_t current, uint16_t target, float t); + // Current smoothed positions (servo units) + uint16_t eyePosition; + uint16_t neckPosition; + + // Current normalized offset the neck has reached (-1 to +1) + // This tracks how far the neck has rotated toward the target + float neckNormalized; + + // Timing + unsigned long faceDetectedTime; // When face was first seen (for neck delay) + bool faceWasPresent; // Was a face present last frame? + + // Map a normalized value (-1..+1) to an asymmetric servo range + uint16_t normalizedToServo(float n, uint16_t center, uint16_t min, uint16_t max) const; + + // Smooth interpolation helpers + static float lerpf(float current, float target, float t); + static uint16_t lerp(uint16_t current, uint16_t target, float t); }; // ============================================================================ diff --git a/ls_firmware.ino b/ls_firmware.ino index b447801..ee53a2d 100644 --- a/ls_firmware.ino +++ b/ls_firmware.ino @@ -24,6 +24,7 @@ #include "protocol.h" #include "sensors.h" #include "behaviors.h" +#include "websocket_client.h" #include @@ -527,6 +528,10 @@ void sendHeartbeat() { flags |= 0x08; if (sensors.isRadarStreamEnabled()) flags |= 0x10; + if (sensors.isFaceStreamEnabled()) + flags |= 0x20; + if (faceDetect.isAlive()) + flags |= 0x40; payload[0] = uptime & 0xFF; payload[1] = (uptime >> 8) & 0xFF; @@ -554,6 +559,9 @@ 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"); @@ -702,6 +710,9 @@ void loop() { // Protocol handling handleProtocol(); + // WebSocket client (receive bytes, FACE packets) + websocketLoop(); + // Update behaviors behaviorManager.update(); @@ -723,7 +734,7 @@ void loop() { } //printMotorStats(); // Sensor updates and streaming - //sensors.update(); + sensors.update(); // Heartbeat //sendHeartbeat(); diff --git a/protocol.h b/protocol.h index fca7155..40052b0 100644 --- a/protocol.h +++ b/protocol.h @@ -44,6 +44,8 @@ namespace Tag { // Sensors constexpr char IMU[4] = {'I','M','U','0'}; // ADXL acceleration data (x,y,z) constexpr char RADAR[4] = {'R','D','A','R'}; // Radar targets + constexpr char FACE[4] = {'F','A','C','E'}; // Face detection data (via WiFi) + constexpr char ALIV[4] = {'A','L','I','V'}; // Remote component alive/heartbeat constexpr char FRAME[4] = {'F','R','M','E'}; // Animation frame events (frame, mode, status) // Behaviors diff --git a/sensors.cpp b/sensors.cpp index 42b8040..1803010 100644 --- a/sensors.cpp +++ b/sensors.cpp @@ -7,6 +7,7 @@ Radar radar; ADXL345 adxl; +FaceDetect faceDetect; SensorManager sensors; // ============================================================================ @@ -277,6 +278,69 @@ void ADXL345::readAccelData() { accelZ = z_raw * scale; } +// ============================================================================ +// Face Detection Implementation +// ============================================================================ + +static DetectedFace s_emptyFace = {0, 0, 0, 0, 0, false}; + +void FaceDetect::feedPayload(const uint8_t* payload, size_t len) { + // FACE payload: [face_count:1][x:2s][y:2s][w:2][h:2][conf:1] per face + if (len < 1) return; + + faceCount = payload[0]; + if (faceCount > FACE_MAX_FACES) faceCount = FACE_MAX_FACES; + + size_t offset = 1; + for (uint8_t i = 0; i < faceCount; i++) { + if (offset + 9 > len) { + // Truncated data + faces[i].valid = false; + continue; + } + faces[i].x = (int16_t)(payload[offset] | (payload[offset + 1] << 8)); + faces[i].y = (int16_t)(payload[offset + 2] | (payload[offset + 3] << 8)); + faces[i].w = payload[offset + 4] | (payload[offset + 5] << 8); + faces[i].h = payload[offset + 6] | (payload[offset + 7] << 8); + faces[i].conf = payload[offset + 8]; + faces[i].valid = true; + offset += 9; + } + + // Invalidate remaining slots + for (uint8_t i = faceCount; i < FACE_MAX_FACES; i++) { + faces[i].valid = false; + } + + newData = true; +} + +const DetectedFace& FaceDetect::getFace(uint8_t index) const { + if (index >= FACE_MAX_FACES) return s_emptyFace; + return faces[index]; +} + +uint16_t FaceDetect::packPayload(uint8_t* buffer) const { + // Same format as FACE protocol: [face_count:1][x:2s][y:2s][w:2][h:2][conf:1] per face + uint16_t offset = 0; + buffer[offset++] = faceCount; + + for (uint8_t i = 0; i < faceCount; i++) { + const DetectedFace& f = faces[i]; + buffer[offset++] = f.x & 0xFF; + buffer[offset++] = (f.x >> 8) & 0xFF; + buffer[offset++] = f.y & 0xFF; + buffer[offset++] = (f.y >> 8) & 0xFF; + buffer[offset++] = f.w & 0xFF; + buffer[offset++] = (f.w >> 8) & 0xFF; + buffer[offset++] = f.h & 0xFF; + buffer[offset++] = (f.h >> 8) & 0xFF; + buffer[offset++] = f.conf; + } + + return offset; +} + // ============================================================================ // Sensor Manager Implementation // ============================================================================ @@ -312,6 +376,12 @@ void SensorManager::update() { sendRadarPacket(); lastRadarSend = now; } + + if (faceStreamEnabled && faceDetect.hasNewData() && (now - lastFaceSend >= faceInterval)) { + sendFacePacket(); + faceDetect.clearNewData(); + lastFaceSend = now; + } } void SensorManager::enableADXLStream(bool enable, uint16_t intervalMs) { @@ -326,6 +396,12 @@ void SensorManager::enableRadarStream(bool enable, uint16_t intervalMs) { lastRadarSend = millis(); } +void SensorManager::enableFaceStream(bool enable, uint16_t intervalMs) { + faceStreamEnabled = enable; + faceInterval = intervalMs; + lastFaceSend = millis(); +} + void SensorManager::sendADXLPacket() { uint8_t payload[32]; // Buffer sized for current/future payload expansion uint16_t len = adxl.packPayload(payload); @@ -338,3 +414,9 @@ void SensorManager::sendRadarPacket() { sendPacket(Tag::RADAR, payload, len); } +void SensorManager::sendFacePacket() { + uint8_t payload[64]; // 1 + (9 * FACE_MAX_FACES) + uint16_t len = faceDetect.packPayload(payload); + sendPacket(Tag::FACE, payload, len); +} + diff --git a/sensors.h b/sensors.h index 3d343d1..aa71a8c 100644 --- a/sensors.h +++ b/sensors.h @@ -91,12 +91,53 @@ private: void readAccelData(); }; +// ============================================================================ +// Face Detection - via WebSocket from Radxa +// ============================================================================ + +constexpr int FACE_MAX_FACES = 5; + +struct DetectedFace { + int16_t x; // Center-relative pixels (0,0 = image center) + int16_t y; // Positive = right/down, negative = left/up + uint16_t w; // Bounding box width + uint16_t h; // Bounding box height + uint8_t conf; // Confidence 0-255 + bool valid; +}; + +class FaceDetect { +public: + // Feed raw FACE payload (after 4-byte tag) from WebSocket + void feedPayload(const uint8_t* payload, size_t len); + + const DetectedFace& getFace(uint8_t index) const; + uint8_t getFaceCount() const { return faceCount; } + bool hasNewData() const { return newData; } + void clearNewData() { newData = false; } + + // Remote alive state + void setAlive(bool alive) { remoteAlive = alive; lastAliveTime = millis(); } + bool isAlive() const { return remoteAlive && (millis() - lastAliveTime < 5000); } + + // Pack faces into payload buffer, returns length + uint16_t packPayload(uint8_t* buffer) const; + +private: + DetectedFace faces[FACE_MAX_FACES] = {}; + uint8_t faceCount = 0; + bool newData = false; + bool remoteAlive = false; + unsigned long lastAliveTime = 0; +}; + // ============================================================================ // Global Instances // ============================================================================ extern Radar radar; extern ADXL345 adxl; +extern FaceDetect faceDetect; // ============================================================================ // Sensor Manager @@ -110,20 +151,26 @@ public: // Streaming control void enableADXLStream(bool enable, uint16_t intervalMs = 100); void enableRadarStream(bool enable, uint16_t intervalMs = 100); + void enableFaceStream(bool enable, uint16_t intervalMs = 100); bool isADXLStreamEnabled() const { return adxlStreamEnabled; } bool isRadarStreamEnabled() const { return radarStreamEnabled; } + bool isFaceStreamEnabled() const { return faceStreamEnabled; } private: bool adxlStreamEnabled = true; bool radarStreamEnabled = true; + bool faceStreamEnabled = true; uint16_t adxlInterval = 500; // 500ms = 2 packets per second uint16_t radarInterval = 10; + uint16_t faceInterval = 50; // 20 Hz unsigned long lastADXLSend = 0; unsigned long lastRadarSend = 0; + unsigned long lastFaceSend = 0; void sendADXLPacket(); void sendRadarPacket(); + void sendFacePacket(); }; extern SensorManager sensors; diff --git a/websocket_client.cpp b/websocket_client.cpp new file mode 100644 index 0000000..346c3c5 --- /dev/null +++ b/websocket_client.cpp @@ -0,0 +1,177 @@ +#include "websocket_client.h" +#include "sensors.h" +#include "protocol.h" +#include +#include + +using namespace websockets; + +static WebsocketsClient client; +static bool s_connected = false; +static unsigned long lastReconnectAttempt = 0; +constexpr unsigned long RECONNECT_INTERVAL = 5000; + +// ============================================================================ +// Packet parsing for WebSocket binary messages +// Uses the same protocol format: 0xA5 0x5A TAG(4) LEN(2) SEQ(2) PAYLOAD(N) CRC(2) +// ============================================================================ + +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 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) + 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 + 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; + } + + // 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 + 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); + } + + pos += totalPacketLen; + } +} + +// ============================================================================ +// WebSocket callbacks +// ============================================================================ + +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 onEvent(WebsocketsEvent event, String data) { + switch (event) { + case WebsocketsEvent::ConnectionOpened: + s_connected = true; + Serial.println("[WebSocket] Connected"); + break; + case WebsocketsEvent::ConnectionClosed: + s_connected = false; + Serial.println("[WebSocket] Disconnected"); + break; + case WebsocketsEvent::GotPing: + break; + case WebsocketsEvent::GotPong: + break; + default: + break; + } +} + +// ============================================================================ +// Public API +// ============================================================================ + +void websocketSetup() { + WiFi.mode(WIFI_STA); + WiFi.begin(WebSocketConfig::WIFI_SSID, WebSocketConfig::WIFI_PASSWORD); + Serial.print("[WebSocket] WiFi connecting"); + int attempts = 0; + while (WiFi.status() != WL_CONNECTED && attempts < 30) { + delay(500); + Serial.print("."); + attempts++; + } + Serial.println(); + if (WiFi.status() != WL_CONNECTED) { + Serial.println("[WebSocket] WiFi failed"); + return; + } + Serial.print("[WebSocket] WiFi OK "); + Serial.println(WiFi.localIP()); + + client.onMessage(onMessage); + client.onEvent(onEvent); + + String url = "ws://" + String(WebSocketConfig::HOST) + ":" + String(WebSocketConfig::PORT) + String(WebSocketConfig::PATH); + Serial.println("[WebSocket] Connecting to " + url); + if (client.connect(url)) { + s_connected = true; + Serial.println("[WebSocket] Connected"); + } else { + Serial.println("[WebSocket] Connect failed (will retry in loop)"); + } +} + +void websocketLoop() { + if (WiFi.status() != WL_CONNECTED) { + s_connected = false; + return; + } + + if (!s_connected && !client.available()) { + unsigned long now = millis(); + if (now - lastReconnectAttempt >= RECONNECT_INTERVAL) { + lastReconnectAttempt = now; + String url = "ws://" + String(WebSocketConfig::HOST) + ":" + String(WebSocketConfig::PORT) + String(WebSocketConfig::PATH); + if (client.connect(url)) { + s_connected = true; + Serial.println("[WebSocket] Reconnected"); + } + } + } + + if (client.available()) { + client.poll(); + } +} + +bool websocketConnected() { + return s_connected && client.available(); +} diff --git a/websocket_client.h b/websocket_client.h new file mode 100644 index 0000000..000a652 --- /dev/null +++ b/websocket_client.h @@ -0,0 +1,22 @@ +#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. + +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 +} + +// Call once from setup() after Serial is ready +void websocketSetup(); + +// Call every loop() - handles connect, reconnect, and incoming messages +void websocketLoop(); + +// True when connected and ready to send/receive +bool websocketConnected();