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

implemented websocket data, just face tracking now. focus behaviour uses face tracking instead of radar

websocket
Jake 2026-02-07 23:51:20 +08:00
parent b35a384974
commit cdd7a39e01
9 changed files with 530 additions and 176 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
PROTOCOL_MIGRATION.md
View File

@ -112,6 +112,8 @@ ushort Crc16Ccitt(byte[] data)
| `VSET` | Set motor positions for a viseme | | `VSET` | Set motor positions for a viseme |
| `VSME` | Trigger viseme (fire-and-forget) | | `VSME` | Trigger viseme (fire-and-forget) |
| `STAT` | System state/heartbeat | | `STAT` | System state/heartbeat |
| `FACE` | Face detection data (Radxa → host/robot, via WiFi) |
| `ALIV` | Remote component alive/heartbeat (component ID + status) |
| `ACK!` | Acknowledge (success) | | `ACK!` | Acknowledge (success) |
| `NACK` | Negative acknowledge (failure) | | `NACK` | Negative acknowledge (failure) |
| `BOOT` | Enter bootloader | | `BOOT` | Enter bootloader |
@ -277,6 +279,39 @@ For each of 3 targets:
``` ```
*Sent automatically when radar streaming is enabled* *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 ### Behaviors
@ -290,7 +325,7 @@ For each of 3 targets:
**Response:** `ACK!` on success, `NACK` on failure **Response:** `ACK!` on success, `NACK` on failure
**Behavior IDs:** **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) - `2` = Idle (perlin noise motion for all motors, ±500 range from center)
- `3` = Viseme (mouth motor control for speech) - `3` = Viseme (mouth motor control for speech)
@ -400,6 +435,8 @@ For each motor:
- Bit 2: Motor streaming active - Bit 2: Motor streaming active
- Bit 3: IMU streaming active - Bit 3: IMU streaming active
- Bit 4: Radar streaming active - Bit 4: Radar streaming active
- Bit 5: Face streaming active
- Bit 6: Face detection remote alive
*Sent automatically every 1 second* *Sent automatically every 1 second*

206
behaviors.cpp
View File

@ -36,176 +36,124 @@ void Behavior::clearMotors() {
FocusBehavior::FocusBehavior() { FocusBehavior::FocusBehavior() {
isActive = false; isActive = false;
eyePosition = EYE_POSITION_CENTER; eyePosition = settings.eyeCenter;
neckPosition = NECK_POSITION_CENTER; neckPosition = settings.neckCenter;
targetEyePosition = EYE_POSITION_CENTER; neckNormalized = 0.0f;
targetNeckPosition = NECK_POSITION_CENTER; faceDetectedTime = 0;
targetDetectedTime = 0; faceWasPresent = false;
neckStartTime = 0;
neckRotating = false;
// Add motors 14, 15, and 27 to controlled list // Add motors to controlled list
addMotor(FOCUS_MOTOR_1); addMotor(settings.eyeMotor1);
addMotor(FOCUS_MOTOR_2); addMotor(settings.eyeMotor2);
addMotor(NECK_MOTOR); addMotor(settings.neckMotor);
} }
bool FocusBehavior::update() { bool FocusBehavior::update() {
// Check radar for valid targets uint8_t faceCount = faceDetect.getFaceCount();
uint8_t targetCount = radar.getTargetCount();
unsigned long now = millis(); unsigned long now = millis();
if (targetCount == 0 || !radar.getTarget(0).valid) { // ---- No face detected ----
// No target - return to center if (faceCount == 0 || !faceDetect.getFace(0).valid) {
isActive = false; isActive = false;
targetEyePosition = EYE_POSITION_CENTER; faceWasPresent = false;
targetNeckPosition = NECK_POSITION_CENTER;
targetDetectedTime = 0;
neckRotating = false;
// Smoothly interpolate eyes to center // Smoothly return eyes and neck to center
eyePosition = lerp(eyePosition, EYE_POSITION_CENTER, INTERPOLATION_SPEED); eyePosition = lerp(eyePosition, settings.eyeCenter, settings.eyeCenteringSpeed);
neckNormalized = lerpf(neckNormalized, 0.0f, settings.neckCenteringSpeed);
// Keep neck at center (no movement) neckPosition = normalizedToServo(
neckPosition = NECK_POSITION_CENTER; settings.neckInvert ? -neckNormalized : neckNormalized,
settings.neckCenter, settings.neckMin, settings.neckMax
);
return false; return false;
} }
// Get first valid target // ---- Face detected ----
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
isActive = true; isActive = true;
uint16_t targetEyePos = calculateEyePositionFromRadarAngle(target.angle); const DetectedFace& face = faceDetect.getFace(0);
// Eyes track immediately // Track when we first saw a face (for neck delay)
targetEyePosition = targetEyePos; if (!faceWasPresent) {
faceDetectedTime = now;
faceWasPresent = true;
}
// Neck disabled for now - keep it centered // Normalize face x to -1..+1
targetNeckPosition = NECK_POSITION_CENTER; float faceNorm = (float)face.x / settings.faceXMax;
neckRotating = false; if (faceNorm < -1.0f) faceNorm = -1.0f;
if (faceNorm > 1.0f) faceNorm = 1.0f;
// Smoothly interpolate eye position toward target // ---- Neck: smoothly follow the target after a delay ----
eyePosition = lerp(eyePosition, targetEyePosition, INTERPOLATION_SPEED); float neckTarget = faceNorm * settings.neckContribution;
// Keep neck at center (no movement) if (now - faceDetectedTime >= settings.neckDelayMs) {
neckPosition = NECK_POSITION_CENTER; // 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; return true;
} }
bool FocusBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) { bool FocusBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) {
// Provide position for eyes (motors 14 and 15) if (motorID == settings.eyeMotor1 || motorID == settings.eyeMotor2) {
if (motorID == FOCUS_MOTOR_1 || motorID == FOCUS_MOTOR_2) {
position = eyePosition; position = eyePosition;
return true; return true;
} }
if (motorID == settings.neckMotor) {
// Provide position for neck (motor 27)
if (motorID == NECK_MOTOR) {
position = neckPosition; position = neckPosition;
return true; return true;
} }
return false; return false;
} }
uint16_t FocusBehavior::calculateEyePositionFromRadarAngle(float radarAngle) { uint16_t FocusBehavior::normalizedToServo(float n, uint16_t center, uint16_t min, uint16_t max) const {
// Calculate eye motor position from radar angle (in degrees) // Map a normalized value (-1..+1) to servo range, handling asymmetric ranges
// Angle range: -50 to +50 degrees, mapped to full eye range (1700-2500, center 2200) float rangeNeg = (float)(center - min);
float rangePos = (float)(max - center);
constexpr float ANGLE_MIN = -50.0f; float posFloat;
constexpr float ANGLE_MAX = 50.0f; if (n < 0.0f) {
posFloat = (float)center + (n * rangeNeg);
// 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);
} else { } else {
// Positive angle: use right range (300 units) posFloat = (float)center + (n * rangePos);
positionFloat = (float)EYE_POSITION_CENTER + (normalized * rangeRight);
} }
// Convert to int16_t first to handle negative values, then clamp int16_t pos = (int16_t)posFloat;
int16_t position = (int16_t)positionFloat; if (pos < (int16_t)min) pos = (int16_t)min;
if (pos > (int16_t)max) pos = (int16_t)max;
// Clamp to valid range (1700 to 2500) return (uint16_t)pos;
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;
} }
uint16_t FocusBehavior::calculateNeckPositionFromRadarAngle(float radarAngle) { float FocusBehavior::lerpf(float current, float target, float t) {
// Calculate neck motor position from radar angle (in degrees) float diff = target - current;
// Angle range: approximately -45 to +45 degrees (typical radar FOV) if (fabs(diff) < 0.001f) return target;
// Map to neck motor position range: 1000 to 3000 (center 2000) return current + diff * t;
// 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;
} }
uint16_t FocusBehavior::lerp(uint16_t current, uint16_t target, float 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 diff = (int16_t)target - (int16_t)current;
int16_t delta = (int16_t)(diff * t); if (abs(diff) < 2) return target;
return (uint16_t)((int16_t)current + (int16_t)(diff * t));
// If difference is very small, snap to target
if (abs(diff) < 2) {
return target;
}
return (uint16_t)(current + delta);
} }
// ============================================================================ // ============================================================================

104
behaviors.h
View File

@ -10,7 +10,7 @@
// ============================================================================ // ============================================================================
enum BehaviorID : uint8_t { 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_IDLE = 2, // Idle behavior (perlin noise for all motors)
BEHAVIOR_VISEME = 3, // Viseme behavior (mouth motor positions) 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 { class FocusBehavior : public Behavior {
public: public:
FocusBehavior(); FocusBehavior();
// Update behavior - check radar for targets // Update behavior - check face detection for targets
bool update() override; bool update() override;
// Get motor position for a controlled motor // Get motor position for a controlled motor
bool getMotorPosition(uint8_t motorID, uint16_t& position) override; bool getMotorPosition(uint8_t motorID, uint16_t& position) override;
private: // Access settings for external tuning
bool isActive; FocusSettings& getSettings() { return settings; }
uint16_t eyePosition; // Current interpolated position for motors 14 and 15 const FocusSettings& getSettings() const { return settings; }
uint16_t neckPosition; // Current interpolated position for motor 27
// Target positions private:
uint16_t targetEyePosition; FocusSettings settings;
uint16_t targetNeckPosition;
bool isActive;
// 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 // Timing
unsigned long targetDetectedTime; // When target was first detected unsigned long faceDetectedTime; // When face was first seen (for neck delay)
unsigned long neckStartTime; // When neck rotation should start bool faceWasPresent; // Was a face present last frame?
bool neckRotating; // Whether neck is actively rotating
// Configuration // Map a normalized value (-1..+1) to an asymmetric servo range
static constexpr uint8_t FOCUS_MOTOR_1 = 14; // Eye motor 1 uint16_t normalizedToServo(float n, uint16_t center, uint16_t min, uint16_t max) const;
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) // Smooth interpolation helpers
static constexpr uint16_t EYE_POSITION_CENTER = 2200; static float lerpf(float current, float target, float t);
static constexpr uint16_t EYE_POSITION_MIN = 1700; static uint16_t lerp(uint16_t current, uint16_t target, float t);
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
// Calculate motor position from radar angle (in degrees)
uint16_t calculateEyePositionFromRadarAngle(float radarAngle);
uint16_t calculateNeckPositionFromRadarAngle(float radarAngle);
// Smooth interpolation helper (linear interpolation)
uint16_t lerp(uint16_t current, uint16_t target, float t);
}; };
// ============================================================================ // ============================================================================

13
ls_firmware.ino
View File

@ -24,6 +24,7 @@
#include "protocol.h" #include "protocol.h"
#include "sensors.h" #include "sensors.h"
#include "behaviors.h" #include "behaviors.h"
#include "websocket_client.h"
#include <FFat.h> #include <FFat.h>
@ -527,6 +528,10 @@ void sendHeartbeat() {
flags |= 0x08; flags |= 0x08;
if (sensors.isRadarStreamEnabled()) if (sensors.isRadarStreamEnabled())
flags |= 0x10; flags |= 0x10;
if (sensors.isFaceStreamEnabled())
flags |= 0x20;
if (faceDetect.isAlive())
flags |= 0x40;
payload[0] = uptime & 0xFF; payload[0] = uptime & 0xFF;
payload[1] = (uptime >> 8) & 0xFF; payload[1] = (uptime >> 8) & 0xFF;
@ -554,6 +559,9 @@ void setup() {
delay(500); delay(500);
Serial.println("\n[HansonServo] Starting..."); Serial.println("\n[HansonServo] Starting...");
// WebSocket client (WiFi + connect to remote, receive FACE packets)
websocketSetup();
// Initialize servo manager // Initialize servo manager
servoManager.init(); servoManager.init();
Serial.println("[HansonServo] Servos initialized"); Serial.println("[HansonServo] Servos initialized");
@ -702,6 +710,9 @@ void loop() {
// Protocol handling // Protocol handling
handleProtocol(); handleProtocol();
// WebSocket client (receive bytes, FACE packets)
websocketLoop();
// Update behaviors // Update behaviors
behaviorManager.update(); behaviorManager.update();
@ -723,7 +734,7 @@ void loop() {
} }
//printMotorStats(); //printMotorStats();
// Sensor updates and streaming // Sensor updates and streaming
//sensors.update(); sensors.update();
// Heartbeat // Heartbeat
//sendHeartbeat(); //sendHeartbeat();

2
protocol.h
View File

@ -44,6 +44,8 @@ namespace Tag {
// Sensors // Sensors
constexpr char IMU[4] = {'I','M','U','0'}; // ADXL acceleration data (x,y,z) 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 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) constexpr char FRAME[4] = {'F','R','M','E'}; // Animation frame events (frame, mode, status)
// Behaviors // Behaviors

82
sensors.cpp
View File

@ -7,6 +7,7 @@
Radar radar; Radar radar;
ADXL345 adxl; ADXL345 adxl;
FaceDetect faceDetect;
SensorManager sensors; SensorManager sensors;
// ============================================================================ // ============================================================================
@ -277,6 +278,69 @@ void ADXL345::readAccelData() {
accelZ = z_raw * scale; 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 // Sensor Manager Implementation
// ============================================================================ // ============================================================================
@ -312,6 +376,12 @@ void SensorManager::update() {
sendRadarPacket(); sendRadarPacket();
lastRadarSend = now; lastRadarSend = now;
} }
if (faceStreamEnabled && faceDetect.hasNewData() && (now - lastFaceSend >= faceInterval)) {
sendFacePacket();
faceDetect.clearNewData();
lastFaceSend = now;
}
} }
void SensorManager::enableADXLStream(bool enable, uint16_t intervalMs) { void SensorManager::enableADXLStream(bool enable, uint16_t intervalMs) {
@ -326,6 +396,12 @@ void SensorManager::enableRadarStream(bool enable, uint16_t intervalMs) {
lastRadarSend = millis(); lastRadarSend = millis();
} }
void SensorManager::enableFaceStream(bool enable, uint16_t intervalMs) {
faceStreamEnabled = enable;
faceInterval = intervalMs;
lastFaceSend = millis();
}
void SensorManager::sendADXLPacket() { void SensorManager::sendADXLPacket() {
uint8_t payload[32]; // Buffer sized for current/future payload expansion uint8_t payload[32]; // Buffer sized for current/future payload expansion
uint16_t len = adxl.packPayload(payload); uint16_t len = adxl.packPayload(payload);
@ -338,3 +414,9 @@ void SensorManager::sendRadarPacket() {
sendPacket(Tag::RADAR, payload, len); 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);
}

47
sensors.h
View File

@ -91,12 +91,53 @@ private:
void readAccelData(); 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 // Global Instances
// ============================================================================ // ============================================================================
extern Radar radar; extern Radar radar;
extern ADXL345 adxl; extern ADXL345 adxl;
extern FaceDetect faceDetect;
// ============================================================================ // ============================================================================
// Sensor Manager // Sensor Manager
@ -110,20 +151,26 @@ public:
// Streaming control // Streaming control
void enableADXLStream(bool enable, uint16_t intervalMs = 100); void enableADXLStream(bool enable, uint16_t intervalMs = 100);
void enableRadarStream(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 isADXLStreamEnabled() const { return adxlStreamEnabled; }
bool isRadarStreamEnabled() const { return radarStreamEnabled; } bool isRadarStreamEnabled() const { return radarStreamEnabled; }
bool isFaceStreamEnabled() const { return faceStreamEnabled; }
private: private:
bool adxlStreamEnabled = true; bool adxlStreamEnabled = true;
bool radarStreamEnabled = true; bool radarStreamEnabled = true;
bool faceStreamEnabled = true;
uint16_t adxlInterval = 500; // 500ms = 2 packets per second uint16_t adxlInterval = 500; // 500ms = 2 packets per second
uint16_t radarInterval = 10; uint16_t radarInterval = 10;
uint16_t faceInterval = 50; // 20 Hz
unsigned long lastADXLSend = 0; unsigned long lastADXLSend = 0;
unsigned long lastRadarSend = 0; unsigned long lastRadarSend = 0;
unsigned long lastFaceSend = 0;
void sendADXLPacket(); void sendADXLPacket();
void sendRadarPacket(); void sendRadarPacket();
void sendFacePacket();
}; };
extern SensorManager sensors; extern SensorManager sensors;

177
websocket_client.cpp Normal file
View File

@ -0,0 +1,177 @@
#include "websocket_client.h"
#include "sensors.h"
#include "protocol.h"
#include <ArduinoWebsockets.h>
#include <WiFi.h>
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<const uint8_t*>(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();
}

22
websocket_client.h Normal file
View File

@ -0,0 +1,22 @@
#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.
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();