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/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