digiflo/AeroAlign
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Extend AeroAlign with mixed CoG planning and telemetry base

Browse Source
This commit is contained in:
DIGIFLO - Florian Klaner
2026-03-11 23:14:33 +01:00
parent 538c3081bf
commit 56890272a0
28 changed files with 1631 additions and 1332 deletions
Download Patch File Download Diff File Expand all files Collapse all files
firmware/common/telemetry_protocol.h
+46
View File
@@ -0,0 +1,46 @@
// SkyLogic AeroAlign - Shared telemetry protocol
//
// Common ESP-NOW payload format shared by Master and all remote devices.
#ifndef TELEMETRY_PROTOCOL_H
#define TELEMETRY_PROTOCOL_H
#include <Arduino.h>
enum DeviceType : uint8_t {
DEVICE_TYPE_UNKNOWN = 0x00,
DEVICE_TYPE_IMU = 0x01,
DEVICE_TYPE_COG_SCALE = 0x02,
DEVICE_TYPE_HYBRID = 0x03,
};
inline const char* deviceTypeToString(DeviceType device_type) {
switch (device_type) {
case DEVICE_TYPE_IMU:
return "IMU";
case DEVICE_TYPE_COG_SCALE:
return "CoG Scale";
case DEVICE_TYPE_HYBRID:
return "Hybrid";
default:
return "Unknown";
}
}
// ESP-NOW packet structure shared across all node types.
// IMU nodes use pitch/roll/yaw normally.
// CoG Scale nodes map:
// pitch -> front support weight (g)
// roll -> rear support weight (g)
// yaw -> computed CoG position (mm)
struct __attribute__((packed)) ESPNowPacket {
uint8_t node_id;
uint8_t device_type;
float pitch;
float roll;
float yaw;
uint8_t battery;
uint8_t checksum;
};
#endif // TELEMETRY_PROTOCOL_H
firmware/master/data/index.html
+43 -16
View File
@@ -474,7 +474,7 @@
</div>
<div class="footer">
<p>SkyLogic AeroAlign v1.0.0 | Phase 4: Differential Measurement Complete</p>
<p>SkyLogic AeroAlign v1.0.0 | AeroAlign active, CoG integration base in progress</p>
<p style="margin-top: 10px;">Open source hardware & firmware | <a href="https://github.com" class="api-link">GitHub</a></p>
</div>
</div>
@@ -562,6 +562,12 @@
// Update status display
function updateStatusDisplay() {
const statusDiv = document.getElementById('status-bar');
const batteryItem = systemStatus.master_battery_available ? `
<div class="status-item">
<span class="status-label">Master Battery</span>
<span class="status-value">${systemStatus.master_battery_percent}%</span>
</div>
` : '';
statusDiv.innerHTML = `
<div class="status-item">
<span class="status-label">Uptime</span>
@@ -583,6 +589,7 @@
<span class="status-label">Free RAM</span>
<span class="status-value">${systemStatus.free_heap_kb} KB</span>
</div>
${batteryItem}
<div class="status-item">
<span class="status-label">Version</span>
<span class="status-value">${systemStatus.firmware_version}</span>
@@ -600,37 +607,57 @@
}
nodesDiv.innerHTML = nodes.map(node => {
const isWarning = node.battery_percent < 20;
const hasBattery = node.battery_available !== false;
const isWarning = hasBattery && node.battery_percent < 20;
const cardClass = !node.is_connected ? 'disconnected' : (isWarning ? 'warning' : '');
const connClass = node.is_connected ? '' : 'disconnected';
const isScaleNode = node.device_type === 2;
const batteryMetric = hasBattery ? `
<div class="metric">
<div class="metric-label">Battery</div>
<div class="metric-value">${node.battery_percent}%</div>
</div>
` : '';
const cardTitle = isScaleNode ? (node.label || 'CoG Scale ' + node.node_id) : (node.label || 'Sensor ' + node.node_id);
const mainValue = isScaleNode ? `${node.cog_position_mm.toFixed(1)} mm` : `${node.pitch.toFixed(2)}°`;
const mainLabel = isScaleNode ? 'CoG Position' : 'Pitch Angle';
const metricColumns = hasBattery ? 3 : 2;
const metricOneLabel = isScaleNode ? 'Front' : 'Roll';
const metricOneValue = isScaleNode ? `${node.front_weight_g.toFixed(0)} g` : `${node.roll.toFixed(2)}°`;
const metricTwoLabel = isScaleNode ? 'Rear' : 'Signal';
const metricTwoValue = isScaleNode ? `${node.rear_weight_g.toFixed(0)} g` : `${node.rssi} dBm`;
const signalMetric = isScaleNode ? '' : `
<div class="metric">
<div class="metric-label">Signal</div>
<div class="metric-value">${node.rssi} dBm</div>
</div>
`;
return `
<div class="node-card ${cardClass}">
<div class="connection-indicator ${connClass}"></div>
<div class="node-header">
<div class="node-label">${node.label || 'Sensor ' + node.node_id}</div>
<div class="node-label">${cardTitle}</div>
<div class="node-id">ID: ${node.node_id}</div>
</div>
<div class="angle-display">
<div class="angle-value">${node.pitch.toFixed(2)}°</div>
<div class="angle-label">Pitch Angle</div>
<div class="angle-value">${mainValue}</div>
<div class="angle-label">${mainLabel}</div>
</div>
<div class="node-metrics">
<div class="node-metrics" style="grid-template-columns: repeat(${isScaleNode ? metricColumns : metricColumns}, 1fr);">
<div class="metric">
<div class="metric-label">Roll</div>
<div class="metric-value">${node.roll.toFixed(2)}°</div>
<div class="metric-label">${metricOneLabel}</div>
<div class="metric-value">${metricOneValue}</div>
</div>
${batteryMetric}
<div class="metric">
<div class="metric-label">Battery</div>
<div class="metric-value">${node.battery_percent}%</div>
</div>
<div class="metric">
<div class="metric-label">Signal</div>
<div class="metric-value">${node.rssi} dBm</div>
<div class="metric-label">${metricTwoLabel}</div>
<div class="metric-value">${metricTwoValue}</div>
</div>
${signalMetric}
</div>
<button class="calibrate-btn" onclick="calibrateNode(${node.node_id})" ${!node.is_connected ? 'disabled' : ''}>
${!node.is_connected ? 'Disconnected' : '⚙ Calibrate (Zero)'}
${!node.is_connected ? 'Disconnected' : (isScaleNode ? '⚙ Tare / Calibrate' : '⚙ Calibrate (Zero)')}
</button>
</div>
`;
@@ -642,7 +669,7 @@
const select1 = document.getElementById('node1-select');
const select2 = document.getElementById('node2-select');
const options = nodes.filter(n => n.is_connected).map(n =>
const options = nodes.filter(n => n.is_connected && n.device_type !== 2).map(n =>
`<option value="${n.node_id}">${n.label || 'Node ' + n.node_id} (${n.node_id})</option>`
).join('');
firmware/master/platformio.ini
+2 -2
View File
@@ -2,7 +2,7 @@
;
; Master node hosts:
; - WiFi Access Point (SSID: SkyLogic-AeroAlign)
; - AsyncWebServer with React web UI
; - AsyncWebServer with browser-based web UI
; - ESP-NOW receiver for Slave node data
; - MPU6050/BNO055 IMU driver
;
@@ -55,7 +55,7 @@ monitor_filters = esp32_exception_decoder
; Build flags
build_flags =
-D ARDUINO_USB_CDC_ON_BOOT=1
-D ARDUINO_USB_CDC_ON_BOOT=0
-D CORE_DEBUG_LEVEL=3
-D CONFIG_ASYNC_TCP_RUNNING_CORE=0
-D CONFIG_ASYNC_TCP_USE_WDT=0
firmware/master/src/config.h
+23 -12
View File
@@ -33,22 +33,36 @@
#define WIFI_AP_SUBNET IPAddress(255, 255, 255, 0)
// ========================================
// GPIO Pin Definitions (ESP32-C3)
// GPIO Pin Definitions
// ========================================
// I2C pins for IMU (MPU6050/BNO055)
#if defined(CONFIG_IDF_TARGET_ESP32S3)
// ESP32-S3 defaults
#define IMU_I2C_SDA 4 // GPIO4 (SDA)
#define IMU_I2C_SCL 5 // GPIO5 (SCL)
#define IMU_I2C_FREQ 100000 // 100kHz for better signal margin on jumper wires
#define BATTERY_ADC_PIN 1 // GPIO1 (ADC1), avoids GPIO0 boot strap
#define BATTERY_MONITOR_ENABLED 0 // Set to 1 only if a divider is actually wired
#define STATUS_LED_PIN -1 // Board-dependent on S3 modules, disabled by default
#define HARDWARE_MODEL "ESP32-S3"
#else
#define IMU_I2C_SDA 4 // GPIO4 (SDA)
#define IMU_I2C_SCL 5 // GPIO5 (SCL)
#define IMU_I2C_FREQ 400000 // 400kHz (fast mode)
#define BATTERY_ADC_PIN 0 // GPIO0 (ADC1_CH0)
#define BATTERY_MONITOR_ENABLED 1
#define STATUS_LED_PIN 10 // GPIO10 (built-in LED on some boards)
#define HARDWARE_MODEL "ESP32-C3"
#endif
// Battery monitoring (ADC)
// Voltage divider: LiPo+ -> 10kΩ -> GPIO0 -> 10kΩ -> GND
#define BATTERY_ADC_PIN 0 // GPIO0 (ADC1_CH0)
// Voltage divider: LiPo+ -> 10kΩ -> BATTERY_ADC_PIN -> 10kΩ -> GND
#define BATTERY_VOLTAGE_DIVIDER 2.0 // 10kΩ + 10kΩ = 2:1 ratio
// Status LED (optional)
#define STATUS_LED_PIN 10 // GPIO10 (built-in LED on some boards)
// Power control (optional, for deep sleep)
#define POWER_ENABLE_PIN -1 // Not used (always on)
@@ -67,8 +81,8 @@
// If no packet received from Slave for this duration, mark as disconnected
#define ESPNOW_TIMEOUT_MS 1000
// Expected packet size (15 bytes: node_id + pitch + roll + yaw + battery + checksum)
#define ESPNOW_PACKET_SIZE 15
// Expected packet size (16 bytes: node_id + device_type + pitch + roll + yaw + battery + checksum)
#define ESPNOW_PACKET_SIZE 16
// ========================================
// IMU Configuration
@@ -132,9 +146,6 @@
// Firmware version
#define FIRMWARE_VERSION "1.0.0"
// Hardware model
#define HARDWARE_MODEL "ESP32-C3"
// System name
#define SYSTEM_NAME "SkyLogic AeroAlign"
firmware/master/src/espnow_master.cpp
+16 -2
View File
@@ -161,9 +161,18 @@ void ESPNowMaster::handleReceivedPacket(const uint8_t *mac, const uint8_t *data,
if (node) {
// Update node data
node->device_type = static_cast<DeviceType>(packet.device_type);
node->pitch = packet.pitch;
node->roll = packet.roll;
node->yaw = packet.yaw;
node->front_weight_g = 0.0f;
node->rear_weight_g = 0.0f;
node->cog_position_mm = 0.0f;
if (node->device_type == DEVICE_TYPE_COG_SCALE || node->device_type == DEVICE_TYPE_HYBRID) {
node->front_weight_g = packet.pitch;
node->rear_weight_g = packet.roll;
node->cog_position_mm = packet.yaw;
}
node->battery_percent = packet.battery;
node->is_connected = true;
node->last_update_ms = millis();
@@ -176,8 +185,9 @@ void ESPNowMaster::handleReceivedPacket(const uint8_t *mac, const uint8_t *data,
total_packets_received++;
#ifdef DEBUG_ESPNOW_PACKETS
Serial.printf("[ESP-NOW] RX from 0x%02X: pitch=%.2f° roll=%.2f° battery=%d%% RSSI=%ddBm\n",
packet.node_id, packet.pitch, packet.roll, packet.battery, node->rssi);
Serial.printf("[ESP-NOW] RX from 0x%02X (%s): a=%.2f b=%.2f c=%.2f battery=%d%% RSSI=%ddBm\n",
packet.node_id, deviceTypeToString(node->device_type),
packet.pitch, packet.roll, packet.yaw, packet.battery, node->rssi);
#endif
}
}
@@ -206,11 +216,15 @@ void ESPNowMaster::registerNode(uint8_t node_id, const uint8_t *mac) {
if (nodes[i].node_id == 0) {
// Initialize new node
nodes[i].node_id = node_id;
nodes[i].device_type = DEVICE_TYPE_UNKNOWN;
memcpy(nodes[i].mac_address, mac, 6);
snprintf(nodes[i].label, sizeof(nodes[i].label), "Sensor %d", node_id - 1);
nodes[i].pitch = 0.0;
nodes[i].roll = 0.0;
nodes[i].yaw = 0.0;
nodes[i].front_weight_g = 0.0;
nodes[i].rear_weight_g = 0.0;
nodes[i].cog_position_mm = 0.0;
nodes[i].pitch_offset = 0.0;
nodes[i].roll_offset = 0.0;
nodes[i].yaw_offset = 0.0;
firmware/master/src/espnow_master.h
+5 -11
View File
@@ -12,26 +12,20 @@
#include <Arduino.h>
#include <esp_now.h>
#include <WiFi.h>
// ESP-NOW packet structure (must match Slave's packet format)
// Total: 15 bytes
struct __attribute__((packed)) ESPNowPacket {
uint8_t node_id; // Sender node ID (0x02-0x09)
float pitch; // Pitch angle (degrees)
float roll; // Roll angle (degrees)
float yaw; // Yaw angle (degrees, unused)
uint8_t battery; // Battery percentage (0-100)
uint8_t checksum; // XOR checksum of bytes 0-13
};
#include "../../common/telemetry_protocol.h"
// Sensor node data structure
struct SensorNode {
uint8_t node_id;
DeviceType device_type;
uint8_t mac_address[6];
char label[32];
float pitch;
float roll;
float yaw;
float front_weight_g;
float rear_weight_g;
float cog_position_mm;
float pitch_offset;
float roll_offset;
float yaw_offset;
firmware/master/src/imu_driver.cpp
+137 -36
View File
@@ -7,10 +7,24 @@
#include "config.h"
#include <math.h>
namespace {
constexpr uint8_t MPU6050_REG_SMPLRT_DIV = 0x19;
constexpr uint8_t MPU6050_REG_CONFIG = 0x1A;
constexpr uint8_t MPU6050_REG_GYRO_CONFIG = 0x1B;
constexpr uint8_t MPU6050_REG_ACCEL_CONFIG = 0x1C;
constexpr uint8_t MPU6050_REG_PWR_MGMT_1 = 0x6B;
constexpr uint8_t MPU6050_REG_WHO_AM_I = 0x75;
constexpr uint8_t MPU6050_REG_ACCEL_XOUT_H = 0x3B;
constexpr uint8_t MPU6050_DEVICE_ID = 0x68;
constexpr float MPU6050_ACCEL_LSB_PER_G = 16384.0f;
constexpr float MPU6050_GYRO_LSB_PER_DEG_S = 131.0f;
constexpr float GRAVITY_M_S2 = 9.80665f;
}
IMU_Driver::IMU_Driver()
: pitch_offset(0.0), roll_offset(0.0), yaw_offset(0.0),
filtered_pitch(0.0), filtered_roll(0.0), last_update_us(0),
alpha(COMPLEMENTARY_FILTER_ALPHA), connected(false) {
alpha(COMPLEMENTARY_FILTER_ALPHA), connected(false), wire(&Wire) {
// Initialize data structure
memset(&data, 0, sizeof(IMU_Data));
}
@@ -21,11 +35,27 @@ bool IMU_Driver::begin(uint8_t sda_pin, uint8_t scl_pin, uint32_t i2c_freq) {
#endif
// Initialize I2C
Wire.begin(sda_pin, scl_pin, i2c_freq);
wire = &Wire;
wire->begin(sda_pin, scl_pin, i2c_freq);
wire->setTimeOut(50);
delay(20);
// Try to initialize MPU6050
if (!mpu.begin(IMU_I2C_ADDRESS, &Wire)) {
last_error = "MPU6050 not found at 0x68. Check wiring!";
// Probe device first so wiring / bus-speed failures are visible in logs.
wire->beginTransmission(IMU_I2C_ADDRESS);
uint8_t i2c_error = wire->endTransmission();
if (i2c_error != 0) {
last_error = "I2C probe failed for MPU6050 at 0x68";
#ifdef DEBUG_SERIAL_ENABLED
Serial.printf("[IMU] ERROR: %s (Wire err=%u, SDA=%u, SCL=%u, %luHz)\n",
last_error.c_str(), i2c_error, sda_pin, scl_pin, (unsigned long)i2c_freq);
#endif
connected = false;
return false;
}
uint8_t who_am_i = 0;
if (!readRegister(MPU6050_REG_WHO_AM_I, who_am_i)) {
last_error = "MPU6050 WHO_AM_I read failed";
#ifdef DEBUG_SERIAL_ENABLED
Serial.printf("[IMU] ERROR: %s\n", last_error.c_str());
#endif
@@ -33,19 +63,33 @@ bool IMU_Driver::begin(uint8_t sda_pin, uint8_t scl_pin, uint32_t i2c_freq) {
return false;
}
#ifdef DEBUG_SERIAL_ENABLED
Serial.printf("[IMU] WHO_AM_I = 0x%02X\n", who_am_i);
#endif
if (who_am_i != MPU6050_DEVICE_ID) {
#ifdef DEBUG_SERIAL_ENABLED
Serial.println("[IMU] WARNING: Unexpected WHO_AM_I, continuing with MPU6050-compatible init");
#endif
}
#ifdef DEBUG_SERIAL_ENABLED
Serial.printf("[IMU] MPU6050 initialized at 0x%02X\n", IMU_I2C_ADDRESS);
#endif
// Configure MPU6050 settings
// Accelerometer range: ±2g (sufficient for static measurement)
mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
// Gyroscope range: ±250 deg/s (low range for better resolution)
mpu.setGyroRange(MPU6050_RANGE_250_DEG);
// Filter bandwidth: 21Hz (balance noise reduction and responsiveness)
mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
// Wake sensor and configure ranges / filtering.
if (!writeRegister(MPU6050_REG_PWR_MGMT_1, 0x01) ||
!writeRegister(MPU6050_REG_SMPLRT_DIV, 0x07) ||
!writeRegister(MPU6050_REG_CONFIG, 0x04) ||
!writeRegister(MPU6050_REG_GYRO_CONFIG, 0x00) ||
!writeRegister(MPU6050_REG_ACCEL_CONFIG, 0x00)) {
last_error = "MPU6050 register configuration failed";
#ifdef DEBUG_SERIAL_ENABLED
Serial.printf("[IMU] ERROR: %s\n", last_error.c_str());
#endif
connected = false;
return false;
}
// Wait for IMU to stabilize
delay(100);
@@ -60,11 +104,16 @@ bool IMU_Driver::begin(uint8_t sda_pin, uint8_t scl_pin, uint32_t i2c_freq) {
int valid_samples = 0;
for (int i = 0; i < IMU_CALIBRATION_SAMPLES; i++) {
sensors_event_t accel, gyro, temp;
if (mpu.getEvent(&accel, &gyro, &temp)) {
uint8_t buffer[14];
if (readRegisters(MPU6050_REG_ACCEL_XOUT_H, buffer, sizeof(buffer))) {
int16_t raw_ax = (buffer[0] << 8) | buffer[1];
int16_t raw_ay = (buffer[2] << 8) | buffer[3];
int16_t raw_az = (buffer[4] << 8) | buffer[5];
float ax = (raw_ax / MPU6050_ACCEL_LSB_PER_G) * GRAVITY_M_S2;
float ay = (raw_ay / MPU6050_ACCEL_LSB_PER_G) * GRAVITY_M_S2;
float az = (raw_az / MPU6050_ACCEL_LSB_PER_G) * GRAVITY_M_S2;
float pitch_raw, roll_raw;
calculateAccelAngles(accel.acceleration.x, accel.acceleration.y, accel.acceleration.z,
pitch_raw, roll_raw);
calculateAccelAngles(ax, ay, az, pitch_raw, roll_raw);
pitch_sum += pitch_raw;
roll_sum += roll_raw;
valid_samples++;
@@ -72,15 +121,22 @@ bool IMU_Driver::begin(uint8_t sda_pin, uint8_t scl_pin, uint32_t i2c_freq) {
delay(10); // 100Hz sampling
}
if (valid_samples > 0) {
pitch_offset = pitch_sum / valid_samples;
roll_offset = roll_sum / valid_samples;
if (valid_samples == 0) {
last_error = "MPU6050 data read failed during calibration";
#ifdef DEBUG_SERIAL_ENABLED
Serial.printf("[IMU] Calibration complete. Offsets: pitch=%.2f°, roll=%.2f°\n",
pitch_offset, roll_offset);
Serial.printf("[IMU] ERROR: %s\n", last_error.c_str());
#endif
connected = false;
return false;
}
pitch_offset = pitch_sum / valid_samples;
roll_offset = roll_sum / valid_samples;
#ifdef DEBUG_SERIAL_ENABLED
Serial.printf("[IMU] Calibration complete. Offsets: pitch=%.2f°, roll=%.2f°\n",
pitch_offset, roll_offset);
#endif
connected = true;
last_update_us = micros();
return true;
@@ -91,15 +147,29 @@ bool IMU_Driver::update() {
return false;
}
// Get sensor events
sensors_event_t accel, gyro, temp;
if (!mpu.getEvent(&accel, &gyro, &temp)) {
uint8_t buffer[14];
if (!readRegisters(MPU6050_REG_ACCEL_XOUT_H, buffer, sizeof(buffer))) {
#ifdef DEBUG_SERIAL_ENABLED
Serial.println("[IMU] ERROR: Failed to read sensor data");
#endif
return false;
}
int16_t raw_ax = (buffer[0] << 8) | buffer[1];
int16_t raw_ay = (buffer[2] << 8) | buffer[3];
int16_t raw_az = (buffer[4] << 8) | buffer[5];
int16_t raw_temp = (buffer[6] << 8) | buffer[7];
int16_t raw_gx = (buffer[8] << 8) | buffer[9];
int16_t raw_gy = (buffer[10] << 8) | buffer[11];
int16_t raw_gz = (buffer[12] << 8) | buffer[13];
float ax = (raw_ax / MPU6050_ACCEL_LSB_PER_G) * GRAVITY_M_S2;
float ay = (raw_ay / MPU6050_ACCEL_LSB_PER_G) * GRAVITY_M_S2;
float az = (raw_az / MPU6050_ACCEL_LSB_PER_G) * GRAVITY_M_S2;
float gx_deg_s = raw_gx / MPU6050_GYRO_LSB_PER_DEG_S;
float gy_deg_s = raw_gy / MPU6050_GYRO_LSB_PER_DEG_S;
float gz_deg_s = raw_gz / MPU6050_GYRO_LSB_PER_DEG_S;
// Calculate time delta (dt) in seconds
uint32_t now_us = micros();
float dt = (now_us - last_update_us) / 1000000.0; // Convert to seconds
@@ -111,22 +181,21 @@ bool IMU_Driver::update() {
}
// Store raw sensor data
data.accel_x = accel.acceleration.x;
data.accel_y = accel.acceleration.y;
data.accel_z = accel.acceleration.z;
data.gyro_x = gyro.gyro.x;
data.gyro_y = gyro.gyro.y;
data.gyro_z = gyro.gyro.z;
data.temperature = temp.temperature;
data.accel_x = ax;
data.accel_y = ay;
data.accel_z = az;
data.gyro_x = gx_deg_s * M_PI / 180.0;
data.gyro_y = gy_deg_s * M_PI / 180.0;
data.gyro_z = gz_deg_s * M_PI / 180.0;
data.temperature = (raw_temp / 340.0f) + 36.53f;
data.timestamp = millis();
// Calculate pitch and roll from accelerometer (gravity vector)
float accel_pitch, accel_roll;
calculateAccelAngles(accel.acceleration.x, accel.acceleration.y, accel.acceleration.z,
accel_pitch, accel_roll);
calculateAccelAngles(ax, ay, az, accel_pitch, accel_roll);
// Apply complementary filter (fuse gyro + accel)
applyComplementaryFilter(accel_pitch, accel_roll, gyro.gyro.x, gyro.gyro.y, dt);
applyComplementaryFilter(accel_pitch, accel_roll, data.gyro_x, data.gyro_y, dt);
// Apply calibration offsets
data.pitch = constrainAngle(filtered_pitch - pitch_offset);
@@ -253,3 +322,35 @@ float IMU_Driver::constrainAngle(float angle) {
while (angle < -180.0) angle += 360.0;
return angle;
}
bool IMU_Driver::writeRegister(uint8_t reg, uint8_t value) {
wire->beginTransmission(IMU_I2C_ADDRESS);
wire->write(reg);
wire->write(value);
return wire->endTransmission() == 0;
}
bool IMU_Driver::readRegister(uint8_t reg, uint8_t &value) {
if (!readRegisters(reg, &value, 1)) {
return false;
}
return true;
}
bool IMU_Driver::readRegisters(uint8_t reg, uint8_t *buffer, size_t len) {
wire->beginTransmission(IMU_I2C_ADDRESS);
wire->write(reg);
if (wire->endTransmission(false) != 0) {
return false;
}
size_t received = wire->requestFrom((uint8_t)IMU_I2C_ADDRESS, (uint8_t)len, (uint8_t)true);
if (received != len) {
return false;
}
for (size_t i = 0; i < len; i++) {
buffer[i] = wire->read();
}
return true;
}
firmware/master/src/imu_driver.h
+7 -4
View File
@@ -8,8 +8,6 @@
#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
// IMU data structure
struct IMU_Data {
@@ -60,8 +58,8 @@ public:
String getLastError() const;
private:
// Adafruit MPU6050 driver instance
Adafruit_MPU6050 mpu;
// Active I2C bus for the IMU
TwoWire *wire;
// Current IMU data
IMU_Data data;
@@ -93,6 +91,11 @@ private:
// Constrain angle to -180 to +180 range
float constrainAngle(float angle);
// Low-level MPU6050 register access
bool writeRegister(uint8_t reg, uint8_t value);
bool readRegister(uint8_t reg, uint8_t &value);
bool readRegisters(uint8_t reg, uint8_t *buffer, size_t len);
};
#endif // IMU_DRIVER_H
firmware/master/src/main.cpp
+24 -14
View File
@@ -23,6 +23,8 @@ IMU_Driver imu;
ESPNowMaster espnow;
CalibrationManager calibration;
WebServerManager* webserver = nullptr;
uint8_t master_battery_percent = 0;
float master_battery_voltage = 0.0f;
// ========================================
// WiFi AP Setup
@@ -75,7 +77,12 @@ bool setupWiFiAP() {
// Battery Monitoring (Master)
// ========================================
uint8_t readBatteryPercent() {
bool readBatteryState(float &battery_voltage, uint8_t &battery_percent) {
#if !BATTERY_MONITOR_ENABLED || BATTERY_ADC_PIN < 0
battery_voltage = 0.0f;
battery_percent = 0;
return false;
#else
// Read battery voltage via ADC
int adc_value = analogRead(BATTERY_ADC_PIN);
@@ -83,7 +90,7 @@ uint8_t readBatteryPercent() {
float voltage_at_adc = (adc_value / 4095.0) * 3.3;
// Multiply by voltage divider ratio (2:1)
float battery_voltage = voltage_at_adc * BATTERY_VOLTAGE_DIVIDER;
battery_voltage = voltage_at_adc * BATTERY_VOLTAGE_DIVIDER;
// Convert to percentage (LiPo: 3.0V = 0%, 4.2V = 100%)
float percent = ((battery_voltage - BATTERY_VOLTAGE_MIN) /
@@ -93,7 +100,9 @@ uint8_t readBatteryPercent() {
if (percent < 0.0) percent = 0.0;
if (percent > 100.0) percent = 100.0;
return (uint8_t)percent;
battery_percent = (uint8_t)percent;
return true;
#endif
}
// ========================================
@@ -119,8 +128,10 @@ void setup() {
digitalWrite(STATUS_LED_PIN, LOW);
#endif
// Initialize battery ADC
// Initialize battery ADC only if a voltage divider is present.
#if BATTERY_MONITOR_ENABLED && BATTERY_ADC_PIN >= 0
pinMode(BATTERY_ADC_PIN, INPUT);
#endif
// ========================================
// Step 1: Setup WiFi AP
@@ -237,7 +248,7 @@ void setup() {
Serial.println("[Setup] Initializing Web Server...");
#endif
webserver = new WebServerManager(&espnow, &calibration, &imu);
webserver = new WebServerManager(&espnow, &calibration, &imu, &master_battery_percent, &master_battery_voltage);
if (!webserver->begin()) {
#ifdef DEBUG_SERIAL_ENABLED
@@ -277,8 +288,6 @@ void loop() {
static uint32_t last_espnow_update_ms = 0;
static uint32_t last_battery_read_ms = 0;
static uint32_t last_stats_print_ms = 0;
static uint8_t battery_percent = 100;
uint32_t now = millis();
// ========================================
@@ -312,13 +321,10 @@ void loop() {
if (now - last_battery_read_ms >= 1000) { // 1000ms = 1Hz
last_battery_read_ms = now;
// Read battery percentage
battery_percent = readBatteryPercent();
// Check for low battery
if (battery_percent <= BATTERY_WARNING_PERCENT) {
if (readBatteryState(master_battery_voltage, master_battery_percent) &&
master_battery_percent <= BATTERY_WARNING_PERCENT) {
#ifdef DEBUG_SERIAL_ENABLED
Serial.printf("[Battery] WARNING: Low battery (%d%%)\n", battery_percent);
Serial.printf("[Battery] WARNING: Low battery (%d%%)\n", master_battery_percent);
#endif
// Flash LED to warn user
@@ -358,7 +364,11 @@ void loop() {
Serial.println("Master Node Status Report");
Serial.println("========================================");
Serial.printf("Uptime: %lu seconds\n", now / 1000);
Serial.printf("Battery: %d%%\n", battery_percent);
#if BATTERY_MONITOR_ENABLED
Serial.printf("Battery: %d%% (%.2fV)\n", master_battery_percent, master_battery_voltage);
#else
Serial.println("Battery: not connected");
#endif
Serial.println("----------------------------------------");
Serial.printf("WiFi: %d clients connected\n", wifi_clients);
Serial.printf("WiFi: http://%s\n", WIFI_AP_IP.toString().c_str());
firmware/master/src/web_server.cpp
+38 -35
View File
@@ -5,10 +5,14 @@
#include "web_server.h"
#include "config.h"
#include <ArduinoJson.h>
#include <SPIFFS.h>
#include <math.h>
WebServerManager::WebServerManager(ESPNowMaster* espnow, CalibrationManager* calibration, IMU_Driver* imu)
: espnow(espnow), calibration(calibration), master_imu(imu), server(nullptr), pair_count(0) {
WebServerManager::WebServerManager(ESPNowMaster* espnow, CalibrationManager* calibration, IMU_Driver* imu,
const uint8_t* master_battery_percent, const float* master_battery_voltage)
: espnow(espnow), calibration(calibration), master_imu(imu), server(nullptr),
master_battery_percent(master_battery_percent), master_battery_voltage(master_battery_voltage),
pair_count(0) {
}
bool WebServerManager::begin() {
@@ -16,6 +20,14 @@ bool WebServerManager::begin() {
Serial.println("[WebServer] Initializing HTTP server...");
#endif
if (!SPIFFS.begin(true)) {
last_error = "SPIFFS mount failed";
#ifdef DEBUG_SERIAL_ENABLED
Serial.printf("[WebServer] ERROR: %s\n", last_error.c_str());
#endif
return false;
}
// Create server instance
server = new AsyncWebServer(HTTP_SERVER_PORT);
@@ -45,11 +57,13 @@ bool WebServerManager::begin() {
this->handleGetStatus(request);
});
// GET / - Serve web UI (placeholder for now)
// GET / - Serve web UI
server->on("/", HTTP_GET, [this](AsyncWebServerRequest *request) {
this->handleRoot(request);
});
server->serveStatic("/", SPIFFS, "/").setDefaultFile("index.html");
// 404 handler
server->onNotFound([this](AsyncWebServerRequest *request) {
this->handleNotFound(request);
@@ -93,15 +107,19 @@ void WebServerManager::handleGetNodes(AsyncWebServerRequest *request) {
JsonObject master_node = nodes_array.createNestedObject();
master_node["node_id"] = 1;
master_node["label"] = "Master";
master_node["device_type"] = DEVICE_TYPE_IMU;
master_node["device_type_label"] = deviceTypeToString(DEVICE_TYPE_IMU);
float pitch, roll, yaw;
master_imu->getAngles(pitch, roll, yaw);
master_node["pitch"] = pitch;
master_node["roll"] = roll;
master_node["yaw"] = yaw;
master_node["battery_percent"] = 85; // TODO: Implement Master battery monitoring
master_node["battery_voltage"] = 3.9;
master_node["battery_available"] = BATTERY_MONITOR_ENABLED;
if (BATTERY_MONITOR_ENABLED && master_battery_percent && master_battery_voltage) {
master_node["battery_percent"] = *master_battery_percent;
master_node["battery_voltage"] = *master_battery_voltage;
}
master_node["rssi"] = 0;
master_node["is_connected"] = true;
master_node["last_update_ms"] = millis();
@@ -116,9 +134,15 @@ void WebServerManager::handleGetNodes(AsyncWebServerRequest *request) {
JsonObject node_obj = nodes_array.createNestedObject();
node_obj["node_id"] = nodes[i].node_id;
node_obj["label"] = nodes[i].label;
node_obj["device_type"] = nodes[i].device_type;
node_obj["device_type_label"] = deviceTypeToString(nodes[i].device_type);
node_obj["pitch"] = nodes[i].pitch;
node_obj["roll"] = nodes[i].roll;
node_obj["yaw"] = nodes[i].yaw;
node_obj["front_weight_g"] = nodes[i].front_weight_g;
node_obj["rear_weight_g"] = nodes[i].rear_weight_g;
node_obj["cog_position_mm"] = nodes[i].cog_position_mm;
node_obj["battery_available"] = true;
node_obj["battery_percent"] = nodes[i].battery_percent;
node_obj["battery_voltage"] = nodes[i].battery_voltage;
node_obj["rssi"] = nodes[i].rssi;
@@ -158,7 +182,7 @@ void WebServerManager::handleGetDifferential(AsyncWebServerRequest *request) {
// (we'll handle this below)
} else {
node1 = espnow->getNode(node1_id);
if (!node1 || !node1->is_connected) {
if (!node1 || !node1->is_connected || node1->device_type == DEVICE_TYPE_COG_SCALE) {
request->send(404, "application/json", "{\"error\":\"Node 1 not found or disconnected\"}");
return;
}
@@ -169,7 +193,7 @@ void WebServerManager::handleGetDifferential(AsyncWebServerRequest *request) {
// Use Master IMU directly
} else {
node2 = espnow->getNode(node2_id);
if (!node2 || !node2->is_connected) {
if (!node2 || !node2->is_connected || node2->device_type == DEVICE_TYPE_COG_SCALE) {
request->send(404, "application/json", "{\"error\":\"Node 2 not found or disconnected\"}");
return;
}
@@ -323,8 +347,11 @@ void WebServerManager::handleGetStatus(AsyncWebServerRequest *request) {
// Build JSON response
DynamicJsonDocument doc(1024);
doc["master_battery_percent"] = 75; // TODO: Implement Master battery monitoring
doc["master_battery_voltage"] = 3.85;
doc["master_battery_available"] = BATTERY_MONITOR_ENABLED;
if (BATTERY_MONITOR_ENABLED && master_battery_percent && master_battery_voltage) {
doc["master_battery_percent"] = *master_battery_percent;
doc["master_battery_voltage"] = *master_battery_voltage;
}
doc["wifi_clients_connected"] = WiFi.softAPgetStationNum();
doc["wifi_channel"] = WIFI_CHANNEL;
doc["uptime_seconds"] = millis() / 1000;
@@ -344,31 +371,7 @@ void WebServerManager::handleRoot(AsyncWebServerRequest *request) {
#ifdef DEBUG_HTTP_REQUESTS
Serial.println("[WebServer] GET /");
#endif
// Serve web UI (placeholder - will be replaced with actual index.html)
String html = R"(
<!DOCTYPE html>
<html>
<head>
<title>SkyLogic AeroAlign</title>
<meta charset="utf-8">
<meta name="viewport" content="width=device-width, initial-scale=1">
</head>
<body>
<h1>SkyLogic AeroAlign</h1>
<p>Web UI placeholder - Full React interface will be implemented in next task</p>
<p>API Endpoints:</p>
<ul>
<li><a href="/api/nodes">/api/nodes</a></li>
<li><a href="/api/status">/api/status</a></li>
<li>/api/differential?node1=1&node2=2</li>
<li>POST /api/calibrate</li>
</ul>
</body>
</html>
)";
request->send(200, "text/html", html);
request->send(SPIFFS, "/index.html", "text/html");
}
void WebServerManager::handleNotFound(AsyncWebServerRequest *request) {
firmware/master/src/web_server.h
+4 -1
View File
@@ -20,7 +20,8 @@
class WebServerManager {
public:
// Constructor
WebServerManager(ESPNowMaster* espnow, CalibrationManager* calibration, IMU_Driver* imu);
WebServerManager(ESPNowMaster* espnow, CalibrationManager* calibration, IMU_Driver* imu,
const uint8_t* master_battery_percent, const float* master_battery_voltage);
// Initialize web server
bool begin();
@@ -36,6 +37,8 @@ private:
ESPNowMaster* espnow;
CalibrationManager* calibration;
IMU_Driver* master_imu;
const uint8_t* master_battery_percent;
const float* master_battery_voltage;
// Last error message
String last_error;
firmware/slave/platformio.ini
+30 -12
View File
@@ -27,8 +27,6 @@ build_flags =
; Library dependencies
lib_deps =
Wire ; I2C for IMU
adafruit/Adafruit MPU6050@^2.2.4 ; MPU6050 IMU driver
adafruit/Adafruit BNO055@^1.6.0 ; BNO055 IMU driver (optional)
; Partition scheme (minimal, no web server)
board_build.partitions = min_spiffs.csv
@@ -59,8 +57,6 @@ build_flags =
; Library dependencies (same as C3)
lib_deps =
Wire
adafruit/Adafruit MPU6050@^2.2.4
adafruit/Adafruit BNO055@^1.6.0
; Partition scheme
board_build.partitions = min_spiffs.csv
@@ -77,47 +73,69 @@ upload_speed = 921600
[env:slave1]
extends = env:esp32-c3
build_flags =
${env:esp32-c3.build_flags}
-D ARDUINO_USB_CDC_ON_BOOT=1
-D CORE_DEBUG_LEVEL=3
-D NODE_ID=0x02
[env:slave2]
extends = env:esp32-c3
build_flags =
${env:esp32-c3.build_flags}
-D ARDUINO_USB_CDC_ON_BOOT=1
-D CORE_DEBUG_LEVEL=3
-D NODE_ID=0x03
[env:slave3]
extends = env:esp32-c3
build_flags =
${env:esp32-c3.build_flags}
-D ARDUINO_USB_CDC_ON_BOOT=1
-D CORE_DEBUG_LEVEL=3
-D NODE_ID=0x04
[env:slave4]
extends = env:esp32-c3
build_flags =
${env:esp32-c3.build_flags}
-D ARDUINO_USB_CDC_ON_BOOT=1
-D CORE_DEBUG_LEVEL=3
-D NODE_ID=0x05
[env:slave5]
extends = env:esp32-c3
build_flags =
${env:esp32-c3.build_flags}
-D ARDUINO_USB_CDC_ON_BOOT=1
-D CORE_DEBUG_LEVEL=3
-D NODE_ID=0x06
[env:slave6]
extends = env:esp32-c3
build_flags =
${env:esp32-c3.build_flags}
-D ARDUINO_USB_CDC_ON_BOOT=1
-D CORE_DEBUG_LEVEL=3
-D NODE_ID=0x07
[env:slave7]
extends = env:esp32-c3
build_flags =
${env:esp32-c3.build_flags}
-D ARDUINO_USB_CDC_ON_BOOT=1
-D CORE_DEBUG_LEVEL=3
-D NODE_ID=0x08
[env:slave8]
extends = env:esp32-c3
build_flags =
${env:esp32-c3.build_flags}
-D ARDUINO_USB_CDC_ON_BOOT=1
-D CORE_DEBUG_LEVEL=3
-D NODE_ID=0x09
[env:slave1-s3]
extends = env:esp32-s3
build_flags =
-D ARDUINO_USB_CDC_ON_BOOT=1
-D CORE_DEBUG_LEVEL=3
-D NODE_ID=0x02
[env:slave2-s3]
extends = env:esp32-s3
build_flags =
-D ARDUINO_USB_CDC_ON_BOOT=1
-D CORE_DEBUG_LEVEL=3
-D NODE_ID=0x03
firmware/slave/src/config.cpp
+3
View File
@@ -0,0 +1,3 @@
#include <stdint.h>
uint8_t master_mac[6] = {0x24, 0x58, 0x7C, 0xDE, 0x81, 0x90};
firmware/slave/src/config.h
+26 -17
View File
@@ -17,15 +17,13 @@
// ========================================
// Master node MAC address
// **IMPORTANT**: Replace this with your Master's actual MAC address
// **IMPORTANT**: Set this to your Master's actual MAC address in config.cpp
// To find Master MAC:
// 1. Flash Master firmware
// 2. Connect Master to USB, open serial monitor (115200 baud)
// 3. Master prints MAC at boot: "Master MAC: 24:6F:28:12:34:56"
// 4. Copy MAC into this array, reflash Slave
//
// Format: {0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF}
uint8_t master_mac[6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; // REPLACE WITH ACTUAL MAC
// 4. Copy MAC into firmware/slave/src/config.cpp, then reflash Slave
extern uint8_t master_mac[6];
// Slave node ID (unique identifier for this Slave)
// Default: 0x02 (first Slave)
@@ -43,26 +41,40 @@ uint8_t master_mac[6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; // REPLACE WITH A
// 100ms = 10Hz update rate (balances latency and power consumption)
#define ESPNOW_SEND_INTERVAL_MS 100
// ESP-NOW packet size (15 bytes: node_id + pitch + roll + yaw + battery + checksum)
#define ESPNOW_PACKET_SIZE 15
// ESP-NOW packet size (16 bytes: node_id + device_type + pitch + roll + yaw + battery + checksum)
#define ESPNOW_PACKET_SIZE 16
// ========================================
// GPIO Pin Definitions (ESP32-C3)
// GPIO Pin Definitions
// ========================================
// I2C pins for IMU (MPU6050/BNO055)
#if defined(CONFIG_IDF_TARGET_ESP32S3)
// ESP32-S3 defaults
#define IMU_I2C_SDA 4 // GPIO4 (SDA)
#define IMU_I2C_SCL 5 // GPIO5 (SCL)
#define IMU_I2C_FREQ 100000 // 100kHz for better signal margin on jumper wires
#define BATTERY_ADC_PIN 1 // GPIO1 (ADC1), avoids GPIO0 boot strap
#define BATTERY_MONITOR_ENABLED 1
#define STATUS_LED_PIN -1 // Board-dependent on S3 modules, disabled by default
#define HARDWARE_MODEL "ESP32-S3"
#else
#define IMU_I2C_SDA 4 // GPIO4 (SDA)
#define IMU_I2C_SCL 5 // GPIO5 (SCL)
#define IMU_I2C_FREQ 400000 // 400kHz (fast mode)
#define BATTERY_ADC_PIN 0 // GPIO0 (ADC1_CH0)
#define BATTERY_MONITOR_ENABLED 1
#define STATUS_LED_PIN 10 // GPIO10 (built-in LED on some boards)
#define HARDWARE_MODEL "ESP32-C3"
#endif
// Battery monitoring (ADC)
// Voltage divider: LiPo+ -> 10kΩ -> GPIO0 -> 10kΩ -> GND
#define BATTERY_ADC_PIN 0 // GPIO0 (ADC1_CH0)
// Voltage divider: LiPo+ -> 10kΩ -> BATTERY_ADC_PIN -> 10kΩ -> GND
#define BATTERY_VOLTAGE_DIVIDER 2.0 // 10kΩ + 10kΩ = 2:1 ratio
// Status LED (optional)
#define STATUS_LED_PIN 10 // GPIO10 (built-in LED on some boards)
// Power control (optional, for deep sleep)
#define POWER_ENABLE_PIN -1 // Not used (always on)
@@ -101,9 +113,6 @@ uint8_t master_mac[6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; // REPLACE WITH A
// Firmware version
#define FIRMWARE_VERSION "1.0.0"
// Hardware model
#define HARDWARE_MODEL "ESP32-C3"
// System name
#define SYSTEM_NAME "SkyLogic AeroAlign Slave"
firmware/slave/src/espnow_slave.cpp
+28 -14
View File
@@ -77,31 +77,45 @@ bool ESPNowSlave::begin() {
}
bool ESPNowSlave::sendData(float pitch, float roll, float yaw, uint8_t battery) {
ESPNowPacket packet;
packet.node_id = node_id;
packet.device_type = DEVICE_TYPE_IMU;
packet.pitch = pitch;
packet.roll = roll;
packet.yaw = yaw;
packet.battery = battery;
packet.checksum = calculateChecksum((uint8_t*)&packet, sizeof(packet) - 1);
return sendPacket(packet);
}
bool ESPNowSlave::sendScaleData(float front_weight_g, float rear_weight_g, float cog_position_mm, uint8_t battery) {
ESPNowPacket packet;
packet.node_id = node_id;
packet.device_type = DEVICE_TYPE_COG_SCALE;
packet.pitch = front_weight_g;
packet.roll = rear_weight_g;
packet.yaw = cog_position_mm;
packet.battery = battery;
packet.checksum = calculateChecksum((uint8_t*)&packet, sizeof(packet) - 1);
return sendPacket(packet);
}
bool ESPNowSlave::sendPacket(const ESPNowPacket &packet) {
if (!paired) {
last_error = "Not paired with Master";
return false;
}
// Build packet
ESPNowPacket packet;
packet.node_id = node_id;
packet.pitch = pitch;
packet.roll = roll;
packet.yaw = yaw;
packet.battery = battery;
// Calculate checksum (XOR of bytes 0-13)
packet.checksum = calculateChecksum((uint8_t*)&packet, sizeof(packet) - 1);
// Send packet
esp_err_t result = esp_now_send(master_mac, (uint8_t*)&packet, sizeof(packet));
esp_err_t result = esp_now_send(master_mac, (const uint8_t*)&packet, sizeof(packet));
if (result == ESP_OK) {
total_packets_sent++;
#ifdef DEBUG_ESPNOW_PACKETS
Serial.printf("[ESP-NOW] TX: pitch=%.2f° roll=%.2f° battery=%d%% checksum=0x%02X\n",
pitch, roll, battery, packet.checksum);
Serial.printf("[ESP-NOW] TX (%s): a=%.2f b=%.2f c=%.2f battery=%d%% checksum=0x%02X\n",
deviceTypeToString(static_cast<DeviceType>(packet.device_type)),
packet.pitch, packet.roll, packet.yaw, packet.battery, packet.checksum);
#endif
return true;
firmware/slave/src/espnow_slave.h
+6 -11
View File
@@ -11,17 +11,7 @@
#include <Arduino.h>
#include <esp_now.h>
#include <WiFi.h>
// ESP-NOW packet structure (must match Master's packet format)
// Total: 15 bytes
struct __attribute__((packed)) ESPNowPacket {
uint8_t node_id; // Sender node ID (0x02-0x09)
float pitch; // Pitch angle (degrees)
float roll; // Roll angle (degrees)
float yaw; // Yaw angle (degrees, unused)
uint8_t battery; // Battery percentage (0-100)
uint8_t checksum; // XOR checksum of bytes 0-13
};
#include "../../common/telemetry_protocol.h"
// ESP-NOW Slave Manager class
class ESPNowSlave {
@@ -35,6 +25,9 @@ public:
// Send sensor data packet to Master
bool sendData(float pitch, float roll, float yaw, uint8_t battery);
// Send CoG scale packet to Master
bool sendScaleData(float front_weight_g, float rear_weight_g, float cog_position_mm, uint8_t battery);
// Get transmission statistics
void getStatistics(uint32_t &total_sent, uint32_t &total_failed, float &success_rate);
@@ -70,6 +63,8 @@ private:
// Calculate XOR checksum
uint8_t calculateChecksum(const uint8_t *data, int len);
bool sendPacket(const ESPNowPacket &packet);
};
#endif // ESPNOW_SLAVE_H
firmware/slave/src/imu_driver.cpp
+128 -38
View File
@@ -7,10 +7,24 @@
#include "config.h"
#include <math.h>
namespace {
constexpr uint8_t MPU6050_REG_SMPLRT_DIV = 0x19;
constexpr uint8_t MPU6050_REG_CONFIG = 0x1A;
constexpr uint8_t MPU6050_REG_GYRO_CONFIG = 0x1B;
constexpr uint8_t MPU6050_REG_ACCEL_CONFIG = 0x1C;
constexpr uint8_t MPU6050_REG_PWR_MGMT_1 = 0x6B;
constexpr uint8_t MPU6050_REG_WHO_AM_I = 0x75;
constexpr uint8_t MPU6050_REG_ACCEL_XOUT_H = 0x3B;
constexpr uint8_t MPU6050_DEVICE_ID = 0x68;
constexpr float MPU6050_ACCEL_LSB_PER_G = 16384.0f;
constexpr float MPU6050_GYRO_LSB_PER_DEG_S = 131.0f;
constexpr float GRAVITY_M_S2 = 9.80665f;
}
IMU_Driver::IMU_Driver()
: pitch_offset(0.0), roll_offset(0.0), yaw_offset(0.0),
filtered_pitch(0.0), filtered_roll(0.0), last_update_us(0),
alpha(COMPLEMENTARY_FILTER_ALPHA), connected(false) {
alpha(COMPLEMENTARY_FILTER_ALPHA), connected(false), wire(&Wire) {
// Initialize data structure
memset(&data, 0, sizeof(IMU_Data));
}
@@ -21,31 +35,46 @@ bool IMU_Driver::begin(uint8_t sda_pin, uint8_t scl_pin, uint32_t i2c_freq) {
#endif
// Initialize I2C
Wire.begin(sda_pin, scl_pin, i2c_freq);
wire = &Wire;
wire->begin(sda_pin, scl_pin, i2c_freq);
wire->setTimeOut(50);
delay(20);
// Try to initialize MPU6050
if (!mpu.begin(IMU_I2C_ADDRESS, &Wire)) {
last_error = "MPU6050 not found at 0x68. Check wiring!";
wire->beginTransmission(IMU_I2C_ADDRESS);
uint8_t i2c_error = wire->endTransmission();
if (i2c_error != 0) {
last_error = "I2C probe failed for MPU6050 at 0x68";
#ifdef DEBUG_SERIAL_ENABLED
Serial.printf("[IMU] ERROR: %s\n", last_error.c_str());
Serial.printf("[IMU] ERROR: %s (Wire err=%u, SDA=%u, SCL=%u, %luHz)\n",
last_error.c_str(), i2c_error, sda_pin, scl_pin, (unsigned long)i2c_freq);
#endif
connected = false;
return false;
}
#ifdef DEBUG_SERIAL_ENABLED
Serial.printf("[IMU] MPU6050 initialized at 0x%02X\n", IMU_I2C_ADDRESS);
uint8_t who_am_i = 0;
if (!readRegister(MPU6050_REG_WHO_AM_I, who_am_i)) {
last_error = "MPU6050 WHO_AM_I read failed";
Serial.printf("[IMU] ERROR: %s\n", last_error.c_str());
connected = false;
return false;
}
Serial.printf("[IMU] WHO_AM_I = 0x%02X\n", who_am_i);
#endif
// Configure MPU6050 settings
// Accelerometer range: ±2g (sufficient for static measurement)
mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
// Gyroscope range: ±250 deg/s (low range for better resolution)
mpu.setGyroRange(MPU6050_RANGE_250_DEG);
// Filter bandwidth: 21Hz (balance noise reduction and responsiveness)
mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
if (!writeRegister(MPU6050_REG_PWR_MGMT_1, 0x01) ||
!writeRegister(MPU6050_REG_SMPLRT_DIV, 0x07) ||
!writeRegister(MPU6050_REG_CONFIG, 0x04) ||
!writeRegister(MPU6050_REG_GYRO_CONFIG, 0x00) ||
!writeRegister(MPU6050_REG_ACCEL_CONFIG, 0x00)) {
last_error = "MPU6050 register configuration failed";
#ifdef DEBUG_SERIAL_ENABLED
Serial.printf("[IMU] ERROR: %s\n", last_error.c_str());
#endif
connected = false;
return false;
}
// Wait for IMU to stabilize
delay(100);
@@ -60,11 +89,16 @@ bool IMU_Driver::begin(uint8_t sda_pin, uint8_t scl_pin, uint32_t i2c_freq) {
int valid_samples = 0;
for (int i = 0; i < IMU_CALIBRATION_SAMPLES; i++) {
sensors_event_t accel, gyro, temp;
if (mpu.getEvent(&accel, &gyro, &temp)) {
uint8_t buffer[14];
if (readRegisters(MPU6050_REG_ACCEL_XOUT_H, buffer, sizeof(buffer))) {
int16_t raw_ax = (buffer[0] << 8) | buffer[1];
int16_t raw_ay = (buffer[2] << 8) | buffer[3];
int16_t raw_az = (buffer[4] << 8) | buffer[5];
float ax = (raw_ax / MPU6050_ACCEL_LSB_PER_G) * GRAVITY_M_S2;
float ay = (raw_ay / MPU6050_ACCEL_LSB_PER_G) * GRAVITY_M_S2;
float az = (raw_az / MPU6050_ACCEL_LSB_PER_G) * GRAVITY_M_S2;
float pitch_raw, roll_raw;
calculateAccelAngles(accel.acceleration.x, accel.acceleration.y, accel.acceleration.z,
pitch_raw, roll_raw);
calculateAccelAngles(ax, ay, az, pitch_raw, roll_raw);
pitch_sum += pitch_raw;
roll_sum += roll_raw;
valid_samples++;
@@ -72,15 +106,22 @@ bool IMU_Driver::begin(uint8_t sda_pin, uint8_t scl_pin, uint32_t i2c_freq) {
delay(10); // 100Hz sampling
}
if (valid_samples > 0) {
pitch_offset = pitch_sum / valid_samples;
roll_offset = roll_sum / valid_samples;
if (valid_samples == 0) {
last_error = "MPU6050 data read failed during calibration";
#ifdef DEBUG_SERIAL_ENABLED
Serial.printf("[IMU] Calibration complete. Offsets: pitch=%.2f°, roll=%.2f°\n",
pitch_offset, roll_offset);
Serial.printf("[IMU] ERROR: %s\n", last_error.c_str());
#endif
connected = false;
return false;
}
pitch_offset = pitch_sum / valid_samples;
roll_offset = roll_sum / valid_samples;
#ifdef DEBUG_SERIAL_ENABLED
Serial.printf("[IMU] Calibration complete. Offsets: pitch=%.2f°, roll=%.2f°\n",
pitch_offset, roll_offset);
#endif
connected = true;
last_update_us = micros();
return true;
@@ -91,15 +132,29 @@ bool IMU_Driver::update() {
return false;
}
// Get sensor events
sensors_event_t accel, gyro, temp;
if (!mpu.getEvent(&accel, &gyro, &temp)) {
uint8_t buffer[14];
if (!readRegisters(MPU6050_REG_ACCEL_XOUT_H, buffer, sizeof(buffer))) {
#ifdef DEBUG_SERIAL_ENABLED
Serial.println("[IMU] ERROR: Failed to read sensor data");
#endif
return false;
}
int16_t raw_ax = (buffer[0] << 8) | buffer[1];
int16_t raw_ay = (buffer[2] << 8) | buffer[3];
int16_t raw_az = (buffer[4] << 8) | buffer[5];
int16_t raw_temp = (buffer[6] << 8) | buffer[7];
int16_t raw_gx = (buffer[8] << 8) | buffer[9];
int16_t raw_gy = (buffer[10] << 8) | buffer[11];
int16_t raw_gz = (buffer[12] << 8) | buffer[13];
float ax = (raw_ax / MPU6050_ACCEL_LSB_PER_G) * GRAVITY_M_S2;
float ay = (raw_ay / MPU6050_ACCEL_LSB_PER_G) * GRAVITY_M_S2;
float az = (raw_az / MPU6050_ACCEL_LSB_PER_G) * GRAVITY_M_S2;
float gx_deg_s = raw_gx / MPU6050_GYRO_LSB_PER_DEG_S;
float gy_deg_s = raw_gy / MPU6050_GYRO_LSB_PER_DEG_S;
float gz_deg_s = raw_gz / MPU6050_GYRO_LSB_PER_DEG_S;
// Calculate time delta (dt) in seconds
uint32_t now_us = micros();
float dt = (now_us - last_update_us) / 1000000.0; // Convert to seconds
@@ -111,22 +166,21 @@ bool IMU_Driver::update() {
}
// Store raw sensor data
data.accel_x = accel.acceleration.x;
data.accel_y = accel.acceleration.y;
data.accel_z = accel.acceleration.z;
data.gyro_x = gyro.gyro.x;
data.gyro_y = gyro.gyro.y;
data.gyro_z = gyro.gyro.z;
data.temperature = temp.temperature;
data.accel_x = ax;
data.accel_y = ay;
data.accel_z = az;
data.gyro_x = gx_deg_s * M_PI / 180.0;
data.gyro_y = gy_deg_s * M_PI / 180.0;
data.gyro_z = gz_deg_s * M_PI / 180.0;
data.temperature = (raw_temp / 340.0f) + 36.53f;
data.timestamp = millis();
// Calculate pitch and roll from accelerometer (gravity vector)
float accel_pitch, accel_roll;
calculateAccelAngles(accel.acceleration.x, accel.acceleration.y, accel.acceleration.z,
accel_pitch, accel_roll);
calculateAccelAngles(ax, ay, az, accel_pitch, accel_roll);
// Apply complementary filter (fuse gyro + accel)
applyComplementaryFilter(accel_pitch, accel_roll, gyro.gyro.x, gyro.gyro.y, dt);
applyComplementaryFilter(accel_pitch, accel_roll, data.gyro_x, data.gyro_y, dt);
// Apply calibration offsets
data.pitch = constrainAngle(filtered_pitch - pitch_offset);
@@ -253,3 +307,39 @@ float IMU_Driver::constrainAngle(float angle) {
while (angle < -180.0) angle += 360.0;
return angle;
}
bool IMU_Driver::writeRegister(uint8_t reg, uint8_t value) {
wire->beginTransmission(IMU_I2C_ADDRESS);
wire->write(reg);
wire->write(value);
return wire->endTransmission() == 0;
}
bool IMU_Driver::readRegister(uint8_t reg, uint8_t &value) {
if (!readRegisters(reg, &value, 1)) {
return false;
}
return true;
}
bool IMU_Driver::readRegisters(uint8_t reg, uint8_t *buffer, size_t len) {
wire->beginTransmission(IMU_I2C_ADDRESS);
wire->write(reg);
if (wire->endTransmission(false) != 0) {
return false;
}
size_t received = wire->requestFrom((int)IMU_I2C_ADDRESS, (int)len, (int)true);
if (received != len) {
return false;
}
for (size_t i = 0; i < len; i++) {
if (!wire->available()) {
return false;
}
buffer[i] = wire->read();
}
return true;
}
firmware/slave/src/imu_driver.h
+7 -4
View File
@@ -8,8 +8,6 @@
#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
// IMU data structure
struct IMU_Data {
@@ -60,8 +58,8 @@ public:
String getLastError() const;
private:
// Adafruit MPU6050 driver instance
Adafruit_MPU6050 mpu;
// Active I2C bus for the IMU
TwoWire *wire;
// Current IMU data
IMU_Data data;
@@ -93,6 +91,11 @@ private:
// Constrain angle to -180 to +180 range
float constrainAngle(float angle);
// Low-level MPU6050 register access
bool writeRegister(uint8_t reg, uint8_t value);
bool readRegister(uint8_t reg, uint8_t &value);
bool readRegisters(uint8_t reg, uint8_t *buffer, size_t len);
};
#endif // IMU_DRIVER_H
firmware/slave/src/main.cpp
+2 -2
View File
@@ -107,7 +107,7 @@ void setup() {
Serial.printf("[Setup] Master MAC: %02X:%02X:%02X:%02X:%02X:%02X\n",
master_mac[0], master_mac[1], master_mac[2],
master_mac[3], master_mac[4], master_mac[5]);
Serial.println("[Setup] **IMPORTANT**: Replace master_mac in config.h with your Master's MAC address!");
Serial.println("[Setup] Master MAC source: firmware/slave/src/config.cpp");
#endif
espnow = new ESPNowSlave(NODE_ID, master_mac);
@@ -115,7 +115,7 @@ void setup() {
if (!espnow->begin()) {
#ifdef DEBUG_SERIAL_ENABLED
Serial.printf("[Setup] ERROR: ESP-NOW initialization failed: %s\n", espnow->getLastError().c_str());
Serial.println("[Setup] Check Master MAC address in config.h");
Serial.println("[Setup] Check Master MAC address in config.cpp");
Serial.println("[Setup] HALTED - Cannot proceed without ESP-NOW");
#endif