modified to use adxl345 instaad of bno055

protocolv2
Jake 2025-12-13 12:53:11 +08:00
parent db0297cea9
commit 4375fb283f
7 changed files with 224 additions and 125 deletions

View File

@ -206,10 +206,10 @@ void sendHeartbeat() {
uint16_t flags = 0;
// Build flags
if (imu.isReady()) flags |= 0x01;
if (adxl.isReady()) flags |= 0x01;
if (animState.current && animState.current->isActive()) flags |= 0x02;
if (motorStream.active) flags |= 0x04;
if (sensors.isIMUStreamEnabled()) flags |= 0x08;
if (sensors.isADXLStreamEnabled()) flags |= 0x08;
if (sensors.isRadarStreamEnabled()) flags |= 0x10;
payload[0] = uptime & 0xFF;

View File

@ -101,7 +101,7 @@ ushort Crc16Ccitt(byte[] data)
| Tag | Description |
|-----|-------------|
| `IMU0` | IMU data (heading, roll, pitch) |
| `IMU0` | IMU data (accel x,y,z) |
| `RDAR` | Radar target data |
| `STAT` | System state/heartbeat |
| `ACK!` | Acknowledge (success) |
@ -225,12 +225,24 @@ When enabled, device streams `MPOS` packets every 50ms.
#### `IMU0` - IMU Data (device → host)
**Payload:**
```
[heading: 2 bytes LE, signed] // degrees × 100
[roll: 2 bytes LE, signed] // degrees × 100
[accelX: 2 bytes LE, signed] // g-forces × 100
[accelY: 2 bytes LE, signed] // g-forces × 100
[accelZ: 2 bytes LE, signed] // g-forces × 100
[pitch: 2 bytes LE, signed] // degrees × 100
[roll: 2 bytes LE, signed] // degrees × 100
```
*Sent automatically when IMU streaming is enabled*
**Coordinate System:**
- X: left/right axis (affects roll)
- Y: front/back axis (affects pitch)
- Z: up/down axis
**Notes:**
- Acceleration values scaled by 100 (not 1000 as before)
- Euler angles calculated from accelerometer data
- Heading/yaw not available (accelerometer only)
#### `RDAR` - Radar Data (device → host)
**Payload:**
```

View File

@ -6,7 +6,7 @@
#include <cstring>
extern RobotConfig config;
extern IMU imu;
extern ADXL345 adxl;
extern Radar radar;
@ -65,29 +65,48 @@ void VariableNode::evaluate(uint32_t currentTick) {
outputValue = config.getMotorPosition(arg0);
break;
// IMU sources - scale to 0-4095 range
case VAR_IMU_HEADING:
// Heading: 0-360° → 0-4095
if (imu.isReady()) {
outputValue = (uint16_t)((imu.getHeading() / 360.0f) * 4095.0f);
// ADXL345 accelerometer sources - scale to 0-4095 range
case VAR_ACCEL_X:
// X acceleration: -2g to +2g → 0-4095 (2048 = 0g)
if (adxl.isReady()) {
float accel = constrain(adxl.getAccelX(), -2.0f, 2.0f);
outputValue = (uint16_t)(((accel + 2.0f) / 4.0f) * 4095.0f);
} else {
outputValue = 2048; // Center if not ready
outputValue = 2048;
}
break;
case VAR_IMU_PITCH:
// Pitch: -90° to +90° → 0-4095 (2048 = level)
if (imu.isReady()) {
float pitch = constrain(imu.getPitch(), -90.0f, 90.0f);
case VAR_ACCEL_Y:
// Y acceleration: -2g to +2g → 0-4095 (2048 = 0g)
if (adxl.isReady()) {
float accel = constrain(adxl.getAccelY(), -2.0f, 2.0f);
outputValue = (uint16_t)(((accel + 2.0f) / 4.0f) * 4095.0f);
} else {
outputValue = 2048;
}
break;
case VAR_ACCEL_Z:
// Z acceleration: -2g to +2g → 0-4095 (2048 = 0g)
if (adxl.isReady()) {
float accel = constrain(adxl.getAccelZ(), -2.0f, 2.0f);
outputValue = (uint16_t)(((accel + 2.0f) / 4.0f) * 4095.0f);
} else {
outputValue = 2048;
}
break;
case VAR_TILT_PITCH:
// Pitch angle: -90° to +90° → 0-4095 (2048 = level)
if (adxl.isReady()) {
float pitch = constrain(adxl.getPitch(), -90.0f, 90.0f);
outputValue = (uint16_t)(((pitch + 90.0f) / 180.0f) * 4095.0f);
} else {
outputValue = 2048;
}
break;
case VAR_IMU_ROLL:
// Roll: -180° to +180° → 0-4095 (2048 = level)
if (imu.isReady()) {
float roll = constrain(imu.getRoll(), -180.0f, 180.0f);
outputValue = (uint16_t)(((roll + 180.0f) / 360.0f) * 4095.0f);
case VAR_TILT_ROLL:
// Roll angle: -90° to +90° → 0-4095 (2048 = level)
if (adxl.isReady()) {
float roll = constrain(adxl.getRoll(), -90.0f, 90.0f);
outputValue = (uint16_t)(((roll + 90.0f) / 180.0f) * 4095.0f);
} else {
outputValue = 2048;
}

View File

@ -20,10 +20,12 @@ enum VariableSource {
VAR_SINE,
VAR_ANALOG,
VAR_SERVO_FEEDBACK,
// IMU sources (BNO055)
VAR_IMU_HEADING, // 0-360 degrees → 0-4095
VAR_IMU_PITCH, // -90 to +90 degrees → 0-4095 (2048 = level)
VAR_IMU_ROLL, // -180 to +180 degrees → 0-4095 (2048 = level)
// ADXL345 accelerometer sources
VAR_ACCEL_X, // X acceleration → 0-4095 (-2g to +2g, 2048 = 0g)
VAR_ACCEL_Y, // Y acceleration → 0-4095 (-2g to +2g, 2048 = 0g)
VAR_ACCEL_Z, // Z acceleration → 0-4095 (-2g to +2g, 2048 = 0g)
VAR_TILT_PITCH, // Pitch angle from accel → 0-4095 (-90° to +90°, 2048 = level)
VAR_TILT_ROLL, // Roll angle from accel → 0-4095 (-90° to +90°, 2048 = level)
// Radar sources (RD-03D) - primary target
VAR_RADAR_X, // X position (angle) → 0-4095 (2048 = center)
VAR_RADAR_Y // Y distance → 0-4095 (scaled 0-500cm)

View File

@ -41,7 +41,7 @@ namespace Tag {
constexpr char MSTRM[4] = {'M','S','T','M'}; // Motor stream control
// Sensors
constexpr char IMU[4] = {'I','M','U','0'}; // IMU data (heading, roll, pitch)
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 FRAME[4] = {'F','R','M','E'}; // Animation frame events (frame, mode, status)

View File

@ -6,7 +6,7 @@
// ============================================================================
Radar radar;
IMU imu;
ADXL345 adxl;
SensorManager sensors;
// ============================================================================
@ -128,79 +128,115 @@ uint16_t Radar::packPayload(uint8_t* buffer) const {
}
// ============================================================================
// IMU Implementation
// ADXL345 Implementation
// ============================================================================
IMU::IMU(uint8_t addr) : addr(addr) {}
ADXL345::ADXL345(uint8_t addr) : addr(addr) {}
bool IMU::init() {
bool ADXL345::init() {
Wire.begin(SensorPins::IMU_SDA, SensorPins::IMU_SCL);
delay(100);
uint8_t id = read8(0x00); // CHIP_ID register
if (id != 0xA0) {
uint8_t id = read8(0x00); // DEVID register (should be 0xE5)
if (id != 0xE5) {
ready = false;
return false;
}
// Enter config mode
write8(0x3D, 0x00);
delay(25);
// Set NDOF fusion mode
write8(0x3D, 0x0C);
delay(25);
// Enable measurement mode
write8(0x2D, 0x08); // POWER_CTL: measure mode
ready = true;
return true;
}
bool IMU::update() {
bool ADXL345::update() {
if (!ready) return false;
Wire.beginTransmission(addr);
Wire.write(0x1A); // Euler angles start register
Wire.endTransmission();
Wire.requestFrom(addr, (uint8_t)6);
if (Wire.available() < 6) return false;
int16_t rawHeading = Wire.read() | (Wire.read() << 8);
int16_t rawPitch = Wire.read() | (Wire.read() << 8);
int16_t rawRoll = Wire.read() | (Wire.read() << 8);
// Convert from 1/16 degree units
heading = rawHeading / 16.0f;
roll = -(rawRoll / 16.0f); // Inverted so right roll is positive
pitch = rawPitch / 16.0f;
readAccelData();
return true;
}
uint16_t IMU::packPayload(uint8_t* buffer) const {
// Format: heading(2, unsigned) + roll(2, signed) + pitch(2, signed), all *100
uint16_t h = (uint16_t)(heading * 100.0f); // 0..36000 fits in uint16
int16_t r = (int16_t)(roll * 100.0f);
int16_t p = (int16_t)(pitch * 100.0f);
buffer[0] = h & 0xFF;
buffer[1] = (h >> 8) & 0xFF;
buffer[2] = r & 0xFF;
buffer[3] = (r >> 8) & 0xFF;
buffer[4] = p & 0xFF;
buffer[5] = (p >> 8) & 0xFF;
return 6;
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;
}
void IMU::write8(uint8_t reg, uint8_t value) {
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 IMU::read8(uint8_t reg) {
uint8_t ADXL345::read8(uint8_t reg) {
Wire.beginTransmission(addr);
Wire.write(reg);
Wire.endTransmission();
@ -208,6 +244,28 @@ uint8_t IMU::read8(uint8_t reg) {
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;
}
// ============================================================================
// Sensor Manager Implementation
// ============================================================================
@ -215,10 +273,10 @@ uint8_t IMU::read8(uint8_t reg) {
void SensorManager::init() {
radar.init();
if (imu.init()) {
Serial.println("[Sensors] IMU initialized");
if (adxl.init()) {
Serial.println("[Sensors] ADXL345 initialized");
} else {
Serial.println("[Sensors] IMU not detected");
Serial.println("[Sensors] ADXL345 not detected");
}
Serial.println("[Sensors] Radar initialized");
@ -227,16 +285,16 @@ void SensorManager::init() {
void SensorManager::update() {
// Update sensors
radar.update();
if (imu.isReady()) {
imu.update();
if (adxl.isReady()) {
adxl.update();
}
// Handle streaming
unsigned long now = millis();
if (imuStreamEnabled && imu.isReady() && (now - lastIMUSend >= imuInterval)) {
sendIMUPacket();
lastIMUSend = now;
if (adxlStreamEnabled && adxl.isReady() && (now - lastADXLSend >= adxlInterval)) {
sendADXLPacket();
lastADXLSend = now;
}
if (radarStreamEnabled && (now - lastRadarSend >= radarInterval)) {
@ -245,10 +303,10 @@ void SensorManager::update() {
}
}
void SensorManager::enableIMUStream(bool enable, uint16_t intervalMs) {
imuStreamEnabled = enable;
imuInterval = intervalMs;
lastIMUSend = millis();
void SensorManager::enableADXLStream(bool enable, uint16_t intervalMs) {
adxlStreamEnabled = enable;
adxlInterval = intervalMs;
lastADXLSend = millis();
}
void SensorManager::enableRadarStream(bool enable, uint16_t intervalMs) {
@ -257,10 +315,10 @@ void SensorManager::enableRadarStream(bool enable, uint16_t intervalMs) {
lastRadarSend = millis();
}
void SensorManager::sendIMUPacket() {
uint8_t payload[6];
uint16_t len = imu.packPayload(payload);
sendPacket(Tag::IMU, payload, len);
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() {

View File

@ -53,33 +53,41 @@ private:
};
// ============================================================================
// IMU - BNO055 9-DOF Sensor
// IMU - ADXL345 3-axis Accelerometer
// ============================================================================
class IMU {
class ADXL345 {
public:
IMU(uint8_t addr = 0x29);
ADXL345(uint8_t addr = 0x53);
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; }
// 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 (6 bytes: h, r, p as int16 * 100)
// 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 heading = 0, roll = 0, pitch = 0;
float accelX = 0, accelY = 0, accelZ = 0;
void write8(uint8_t reg, uint8_t value);
uint8_t read8(uint8_t reg);
void readAccelData();
};
// ============================================================================
@ -87,7 +95,7 @@ private:
// ============================================================================
extern Radar radar;
extern IMU imu;
extern ADXL345 adxl;
// ============================================================================
// Sensor Manager
@ -99,21 +107,21 @@ public:
void update();
// Streaming control
void enableIMUStream(bool enable, uint16_t intervalMs = 100);
void enableADXLStream(bool enable, uint16_t intervalMs = 100);
void enableRadarStream(bool enable, uint16_t intervalMs = 100);
bool isIMUStreamEnabled() const { return imuStreamEnabled; }
bool isADXLStreamEnabled() const { return adxlStreamEnabled; }
bool isRadarStreamEnabled() const { return radarStreamEnabled; }
private:
bool imuStreamEnabled = true;
bool radarStreamEnabled = true;
uint16_t imuInterval = 10;
bool adxlStreamEnabled = true;
bool radarStreamEnabled = false;
uint16_t adxlInterval = 10;
uint16_t radarInterval = 10;
unsigned long lastIMUSend = 0;
unsigned long lastADXLSend = 0;
unsigned long lastRadarSend = 0;
void sendIMUPacket();
void sendADXLPacket();
void sendRadarPacket();
};