131 lines
3.7 KiB
C++
131 lines
3.7 KiB
C++
#pragma once
|
|
#include <Arduino.h>
|
|
#include <Wire.h>
|
|
|
|
// ============================================================================
|
|
// Pin Configuration
|
|
// ============================================================================
|
|
|
|
namespace SensorPins {
|
|
// Radar (RD-03D) on Serial2
|
|
constexpr int RADAR_RX = 4;
|
|
constexpr int RADAR_TX = 5;
|
|
|
|
// IMU (BNO055) on I2C
|
|
constexpr int IMU_SDA = 8;
|
|
constexpr int IMU_SCL = 9;
|
|
}
|
|
|
|
// ============================================================================
|
|
// Radar - RD-03D mmWave Sensor
|
|
// ============================================================================
|
|
|
|
constexpr int RADAR_MAX_TARGETS = 3;
|
|
constexpr uint32_t RADAR_BAUD = 256000;
|
|
|
|
struct RadarTarget {
|
|
float x; // X position in cm (negative=left, positive=right)
|
|
float y; // Y distance in cm (forward)
|
|
float speed; // Speed in cm/s
|
|
float angle; // Angle in degrees (calculated from x,y using atan2)
|
|
bool valid; // Target is valid
|
|
};
|
|
|
|
class Radar {
|
|
public:
|
|
void init();
|
|
bool update(); // Returns true if new data parsed
|
|
|
|
const RadarTarget& getTarget(uint8_t index) const;
|
|
uint8_t getTargetCount() const;
|
|
|
|
// Pack all targets into a payload buffer, returns length
|
|
uint16_t packPayload(uint8_t* buffer) const;
|
|
|
|
private:
|
|
RadarTarget targets[RADAR_MAX_TARGETS] = {};
|
|
uint8_t rxBuf[64];
|
|
uint8_t bufIdx = 0;
|
|
uint8_t headerMatch = 0;
|
|
bool inFrame = false;
|
|
|
|
void parseFrame();
|
|
static int16_t decodeSignMag(uint16_t raw);
|
|
};
|
|
|
|
// ============================================================================
|
|
// IMU - ADXL345 3-axis Accelerometer
|
|
// ============================================================================
|
|
|
|
class ADXL345 {
|
|
public:
|
|
ADXL345(uint8_t addr = 0x53);
|
|
|
|
bool init();
|
|
bool update(); // Read latest values
|
|
|
|
// Get acceleration in g-forces
|
|
float getAccelX() const { return accelX; }
|
|
float getAccelY() const { return accelY; }
|
|
float getAccelZ() const { return accelZ; }
|
|
|
|
// Get tilt angles in degrees (approximated from gravity)
|
|
float getPitch() const; // Pitch from Y/Z acceleration (Y=front/back)
|
|
float getRoll() const; // Roll from X/Z acceleration (X=left/right)
|
|
|
|
// Get Euler angles (pitch and roll only, no heading without magnetometer)
|
|
void getEulerAngles(float& pitch, float& roll) const;
|
|
|
|
bool isReady() const { return ready; }
|
|
|
|
// Pack into payload buffer (10 bytes: x,y,z accel + pitch,roll angles)
|
|
uint16_t packPayload(uint8_t* buffer) const;
|
|
|
|
private:
|
|
uint8_t addr;
|
|
bool ready = false;
|
|
float accelX = 0, accelY = 0, accelZ = 0;
|
|
|
|
void write8(uint8_t reg, uint8_t value);
|
|
uint8_t read8(uint8_t reg);
|
|
void readAccelData();
|
|
};
|
|
|
|
// ============================================================================
|
|
// Global Instances
|
|
// ============================================================================
|
|
|
|
extern Radar radar;
|
|
extern ADXL345 adxl;
|
|
|
|
// ============================================================================
|
|
// Sensor Manager
|
|
// ============================================================================
|
|
|
|
class SensorManager {
|
|
public:
|
|
void init();
|
|
void update();
|
|
|
|
// Streaming control
|
|
void enableADXLStream(bool enable, uint16_t intervalMs = 100);
|
|
void enableRadarStream(bool enable, uint16_t intervalMs = 100);
|
|
|
|
bool isADXLStreamEnabled() const { return adxlStreamEnabled; }
|
|
bool isRadarStreamEnabled() const { return radarStreamEnabled; }
|
|
|
|
private:
|
|
bool adxlStreamEnabled = true;
|
|
bool radarStreamEnabled = true;
|
|
uint16_t adxlInterval = 500; // 500ms = 2 packets per second
|
|
uint16_t radarInterval = 10;
|
|
unsigned long lastADXLSend = 0;
|
|
unsigned long lastRadarSend = 0;
|
|
|
|
void sendADXLPacket();
|
|
void sendRadarPacket();
|
|
};
|
|
|
|
extern SensorManager sensors;
|
|
|