421 lines
12 KiB
C++
421 lines
12 KiB
C++
#include "sensors.h"
|
||
#include "protocol.h"
|
||
|
||
// ============================================================================
|
||
// Global Instances
|
||
// ============================================================================
|
||
|
||
Radar radar;
|
||
ADXL345 adxl;
|
||
FaceDetect faceDetect;
|
||
SensorManager sensors;
|
||
|
||
// ============================================================================
|
||
// Radar Implementation
|
||
// ============================================================================
|
||
|
||
static const uint8_t RADAR_HEADER[] = {0xAA, 0xFF, 0x03, 0x00};
|
||
static const uint8_t RADAR_FOOTER[] = {0x55, 0xCC};
|
||
constexpr float RADAR_DISTANCE_SCALE = 0.1f; // Raw mm to cm
|
||
constexpr float RADAR_MIN_VALID_DIST = 30.0f; // Minimum valid distance in cm
|
||
|
||
int16_t Radar::decodeSignMag(uint16_t raw) {
|
||
int16_t magnitude = raw & 0x7FFF;
|
||
return (raw & 0x8000) ? magnitude : -magnitude;
|
||
}
|
||
|
||
void Radar::init() {
|
||
Serial2.begin(RADAR_BAUD, SERIAL_8N1, SensorPins::RADAR_RX, SensorPins::RADAR_TX);
|
||
}
|
||
|
||
bool Radar::update() {
|
||
bool newData = false;
|
||
|
||
while (Serial2.available()) {
|
||
uint8_t b = Serial2.read();
|
||
|
||
if (!inFrame) {
|
||
// Looking for header
|
||
if (b == RADAR_HEADER[headerMatch]) {
|
||
rxBuf[headerMatch] = b;
|
||
headerMatch++;
|
||
if (headerMatch == 4) {
|
||
inFrame = true;
|
||
bufIdx = 4;
|
||
headerMatch = 0;
|
||
}
|
||
} else if (b == RADAR_HEADER[0]) {
|
||
headerMatch = 1;
|
||
rxBuf[0] = b;
|
||
} else {
|
||
headerMatch = 0;
|
||
}
|
||
continue;
|
||
}
|
||
|
||
// In frame - collect bytes
|
||
if (bufIdx < sizeof(rxBuf)) {
|
||
rxBuf[bufIdx++] = b;
|
||
}
|
||
|
||
// Check for footer
|
||
if (bufIdx >= 6 && rxBuf[bufIdx - 2] == RADAR_FOOTER[0] && rxBuf[bufIdx - 1] == RADAR_FOOTER[1]) {
|
||
parseFrame();
|
||
newData = true;
|
||
inFrame = false;
|
||
bufIdx = 0;
|
||
}
|
||
|
||
// Overflow protection
|
||
if (bufIdx >= sizeof(rxBuf)) {
|
||
inFrame = false;
|
||
bufIdx = 0;
|
||
}
|
||
}
|
||
|
||
return newData;
|
||
}
|
||
|
||
void Radar::parseFrame() {
|
||
for (int i = 0; i < RADAR_MAX_TARGETS; i++) {
|
||
int offset = 4 + (i * 6);
|
||
|
||
uint16_t x_raw = rxBuf[offset] | (rxBuf[offset + 1] << 8);
|
||
uint16_t y_raw = rxBuf[offset + 2] | (rxBuf[offset + 3] << 8);
|
||
uint16_t spd_raw = rxBuf[offset + 4] | (rxBuf[offset + 5] << 8);
|
||
|
||
targets[i].x = decodeSignMag(x_raw) * RADAR_DISTANCE_SCALE;
|
||
targets[i].y = (int16_t)(y_raw - 0x8000) * RADAR_DISTANCE_SCALE;
|
||
targets[i].speed = decodeSignMag(spd_raw) * RADAR_DISTANCE_SCALE;
|
||
|
||
targets[i].valid = (y_raw != 0) && (y_raw != 0x8000) && (targets[i].y >= RADAR_MIN_VALID_DIST);
|
||
|
||
// Calculate angle from x and y (x is mm from center line, y is distance)
|
||
// atan2(x, y) gives angle in radians, convert to degrees
|
||
if (targets[i].valid && targets[i].y > 0) {
|
||
targets[i].angle = atan2(targets[i].x, targets[i].y) * 180.0f / PI;
|
||
} else {
|
||
targets[i].angle = 0.0f;
|
||
}
|
||
}
|
||
}
|
||
|
||
const RadarTarget& Radar::getTarget(uint8_t index) const {
|
||
if (index >= RADAR_MAX_TARGETS) index = 0;
|
||
return targets[index];
|
||
}
|
||
|
||
uint8_t Radar::getTargetCount() const {
|
||
uint8_t count = 0;
|
||
for (int i = 0; i < RADAR_MAX_TARGETS; i++) {
|
||
if (targets[i].valid) count++;
|
||
}
|
||
return count;
|
||
}
|
||
|
||
uint16_t Radar::packPayload(uint8_t* buffer) const {
|
||
// Format: count(1) + [valid(1), x(2), y(2), speed(2), angle(2)] * 3 = 28 bytes
|
||
buffer[0] = getTargetCount();
|
||
uint16_t offset = 1;
|
||
|
||
for (int i = 0; i < RADAR_MAX_TARGETS; i++) {
|
||
buffer[offset++] = targets[i].valid ? 1 : 0;
|
||
|
||
int16_t x = (int16_t)(targets[i].x * 10); // cm * 10 for precision
|
||
int16_t y = (int16_t)(targets[i].y * 10);
|
||
int16_t spd = (int16_t)(targets[i].speed * 10);
|
||
int16_t angle = (int16_t)(targets[i].angle * 10); // degrees * 10 for precision
|
||
|
||
buffer[offset++] = x & 0xFF;
|
||
buffer[offset++] = (x >> 8) & 0xFF;
|
||
buffer[offset++] = y & 0xFF;
|
||
buffer[offset++] = (y >> 8) & 0xFF;
|
||
buffer[offset++] = spd & 0xFF;
|
||
buffer[offset++] = (spd >> 8) & 0xFF;
|
||
buffer[offset++] = angle & 0xFF;
|
||
buffer[offset++] = (angle >> 8) & 0xFF;
|
||
}
|
||
|
||
return offset;
|
||
}
|
||
|
||
// ============================================================================
|
||
// ADXL345 Implementation
|
||
// ============================================================================
|
||
|
||
ADXL345::ADXL345(uint8_t addr) : addr(addr) {}
|
||
|
||
bool ADXL345::init() {
|
||
Wire.begin(SensorPins::IMU_SDA, SensorPins::IMU_SCL);
|
||
delay(100);
|
||
|
||
uint8_t id = read8(0x00); // DEVID register (should be 0xE5)
|
||
if (id != 0xE5) {
|
||
ready = false;
|
||
return false;
|
||
}
|
||
|
||
// Enable measurement mode
|
||
write8(0x2D, 0x08); // POWER_CTL: measure mode
|
||
|
||
ready = true;
|
||
return true;
|
||
}
|
||
|
||
bool ADXL345::update() {
|
||
if (!ready) return false;
|
||
|
||
readAccelData();
|
||
return true;
|
||
}
|
||
|
||
float ADXL345::getPitch() const {
|
||
// Approximate pitch from Y/Z acceleration (Y = front/back axis)
|
||
// This is not as accurate as a proper IMU with gyroscope
|
||
if (accelZ == 0) return 0;
|
||
return atan2(-accelY, sqrt(accelX * accelX + accelZ * accelZ)) * 180.0f / PI;
|
||
}
|
||
|
||
float ADXL345::getRoll() const {
|
||
// Approximate roll from X/Z acceleration (X = left/right axis)
|
||
if (accelZ == 0) return 0;
|
||
return atan2(accelX, accelZ) * 180.0f / PI;
|
||
}
|
||
|
||
void ADXL345::getEulerAngles(float& pitch, float& roll) const {
|
||
// Calculate Euler angles from accelerometer data
|
||
// Note: Heading (yaw) cannot be determined from accelerometer alone
|
||
// Coordinate system: X=left/right, Y=front/back, Z=up/down
|
||
|
||
if (!ready) {
|
||
pitch = 0.0f;
|
||
roll = 0.0f;
|
||
return;
|
||
}
|
||
|
||
// Pitch (front/back tilt) = atan2(-accelY, sqrt(accelX² + accelZ²))
|
||
// Roll (left/right tilt) = atan2(accelX, accelZ)
|
||
|
||
float accelMagnitude = sqrt(accelX * accelX + accelZ * accelZ);
|
||
if (accelMagnitude > 0.01f) { // Avoid division by very small numbers
|
||
pitch = atan2(-accelY, accelMagnitude) * 180.0f / PI;
|
||
} else {
|
||
pitch = 0.0f;
|
||
}
|
||
|
||
if (fabs(accelZ) > 0.01f) { // Avoid division by zero
|
||
roll = atan2(accelX, accelZ) * 180.0f / PI;
|
||
} else {
|
||
roll = 0.0f;
|
||
}
|
||
}
|
||
|
||
uint16_t ADXL345::packPayload(uint8_t* buffer) const {
|
||
// Format: accelX(2) + accelY(2) + accelZ(2) + pitch(2) + roll(2), all ×100
|
||
// Accelerations in g-forces ×100, angles in degrees ×100
|
||
|
||
// Pack acceleration data
|
||
int16_t x = (int16_t)(accelX * 100.0f);
|
||
int16_t y = (int16_t)(accelY * 100.0f);
|
||
int16_t z = (int16_t)(accelZ * 100.0f);
|
||
|
||
buffer[0] = x & 0xFF;
|
||
buffer[1] = (x >> 8) & 0xFF;
|
||
buffer[2] = y & 0xFF;
|
||
buffer[3] = (y >> 8) & 0xFF;
|
||
buffer[4] = z & 0xFF;
|
||
buffer[5] = (z >> 8) & 0xFF;
|
||
|
||
// Calculate and pack Euler angles
|
||
float pitch_deg, roll_deg;
|
||
getEulerAngles(pitch_deg, roll_deg);
|
||
|
||
int16_t pitch = (int16_t)(pitch_deg * 100.0f);
|
||
int16_t roll = (int16_t)(roll_deg * 100.0f);
|
||
|
||
buffer[6] = pitch & 0xFF;
|
||
buffer[7] = (pitch >> 8) & 0xFF;
|
||
buffer[8] = roll & 0xFF;
|
||
buffer[9] = (roll >> 8) & 0xFF;
|
||
|
||
return 10;
|
||
}
|
||
|
||
void ADXL345::write8(uint8_t reg, uint8_t value) {
|
||
Wire.beginTransmission(addr);
|
||
Wire.write(reg);
|
||
Wire.write(value);
|
||
Wire.endTransmission();
|
||
}
|
||
|
||
uint8_t ADXL345::read8(uint8_t reg) {
|
||
Wire.beginTransmission(addr);
|
||
Wire.write(reg);
|
||
Wire.endTransmission();
|
||
Wire.requestFrom(addr, (uint8_t)1);
|
||
return Wire.available() ? Wire.read() : 0xFF;
|
||
}
|
||
|
||
void ADXL345::readAccelData() {
|
||
// Read 6 bytes starting from DATAX0 register (0x32)
|
||
Wire.beginTransmission(addr);
|
||
Wire.write(0x32); // Start at DATAX0
|
||
Wire.endTransmission();
|
||
|
||
Wire.requestFrom(addr, (uint8_t)6);
|
||
if (Wire.available() < 6) return;
|
||
|
||
// ADXL345 outputs 10-bit values (2 bytes per axis)
|
||
int16_t x_raw = Wire.read() | (Wire.read() << 8);
|
||
int16_t y_raw = Wire.read() | (Wire.read() << 8);
|
||
int16_t z_raw = Wire.read() | (Wire.read() << 8);
|
||
|
||
// Convert to g-forces: ±2g range, 10-bit = ±512 LSB, 4mg/LSB
|
||
const float scale = 0.00390625f; // 4mg/LSB = 0.004g, but we use 1/256 for 10-bit
|
||
|
||
accelX = x_raw * scale;
|
||
accelY = y_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
|
||
// ============================================================================
|
||
|
||
void SensorManager::init() {
|
||
radar.init();
|
||
|
||
if (adxl.init()) {
|
||
Serial.println("[Sensors] ADXL345 initialized");
|
||
} else {
|
||
Serial.println("[Sensors] ADXL345 not detected");
|
||
}
|
||
|
||
Serial.println("[Sensors] Radar initialized");
|
||
}
|
||
|
||
void SensorManager::update() {
|
||
// Update sensors
|
||
radar.update();
|
||
if (adxl.isReady()) {
|
||
adxl.update();
|
||
}
|
||
|
||
// Handle streaming
|
||
unsigned long now = millis();
|
||
|
||
if (adxlStreamEnabled && adxl.isReady() && (now - lastADXLSend >= adxlInterval)) {
|
||
sendADXLPacket();
|
||
lastADXLSend = now;
|
||
}
|
||
|
||
if (radarStreamEnabled && (now - lastRadarSend >= radarInterval)) {
|
||
sendRadarPacket();
|
||
lastRadarSend = now;
|
||
}
|
||
|
||
if (faceStreamEnabled && faceDetect.hasNewData() && (now - lastFaceSend >= faceInterval)) {
|
||
sendFacePacket();
|
||
faceDetect.clearNewData();
|
||
lastFaceSend = now;
|
||
}
|
||
}
|
||
|
||
void SensorManager::enableADXLStream(bool enable, uint16_t intervalMs) {
|
||
adxlStreamEnabled = enable;
|
||
adxlInterval = intervalMs;
|
||
lastADXLSend = millis();
|
||
}
|
||
|
||
void SensorManager::enableRadarStream(bool enable, uint16_t intervalMs) {
|
||
radarStreamEnabled = enable;
|
||
radarInterval = 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);
|
||
sendPacket(Tag::IMU, payload, len); // Reuse IMU tag for compatibility
|
||
}
|
||
|
||
void SensorManager::sendRadarPacket() {
|
||
uint8_t payload[32]; // Updated size for angle field
|
||
uint16_t len = radar.packPayload(payload);
|
||
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);
|
||
} |