122 lines
3.2 KiB
C++
122 lines
3.2 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
|
|
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 - BNO055 9-DOF Sensor
|
|
// ============================================================================
|
|
|
|
class IMU {
|
|
public:
|
|
IMU(uint8_t addr = 0x29);
|
|
|
|
bool init();
|
|
bool update(); // Read latest values
|
|
|
|
// Get euler angles in degrees
|
|
float getHeading() const { return heading; }
|
|
float getRoll() const { return roll; }
|
|
float getPitch() const { return pitch; }
|
|
|
|
bool isReady() const { return ready; }
|
|
|
|
// Pack into payload buffer (6 bytes: h, r, p as int16 * 100)
|
|
uint16_t packPayload(uint8_t* buffer) const;
|
|
|
|
private:
|
|
uint8_t addr;
|
|
bool ready = false;
|
|
float heading = 0, roll = 0, pitch = 0;
|
|
|
|
void write8(uint8_t reg, uint8_t value);
|
|
uint8_t read8(uint8_t reg);
|
|
};
|
|
|
|
// ============================================================================
|
|
// Global Instances
|
|
// ============================================================================
|
|
|
|
extern Radar radar;
|
|
extern IMU imu;
|
|
|
|
// ============================================================================
|
|
// Sensor Manager
|
|
// ============================================================================
|
|
|
|
class SensorManager {
|
|
public:
|
|
void init();
|
|
void update();
|
|
|
|
// Streaming control
|
|
void enableIMUStream(bool enable, uint16_t intervalMs = 100);
|
|
void enableRadarStream(bool enable, uint16_t intervalMs = 100);
|
|
|
|
bool isIMUStreamEnabled() const { return imuStreamEnabled; }
|
|
bool isRadarStreamEnabled() const { return radarStreamEnabled; }
|
|
|
|
private:
|
|
bool imuStreamEnabled = true;
|
|
bool radarStreamEnabled = true;
|
|
uint16_t imuInterval = 10;
|
|
uint16_t radarInterval = 10;
|
|
unsigned long lastIMUSend = 0;
|
|
unsigned long lastRadarSend = 0;
|
|
|
|
void sendIMUPacket();
|
|
void sendRadarPacket();
|
|
};
|
|
|
|
extern SensorManager sensors;
|
|
|