commit 7d4ec773aebf51e386bffaaf0fda31a936e10c03 Author: Donky Date: Sat Aug 29 15:02:50 2026 +0200 v1 diff --git a/IMU_mem_rkt.ino b/IMU_mem_rkt.ino new file mode 100644 index 0000000..69c8c90 --- /dev/null +++ b/IMU_mem_rkt.ino @@ -0,0 +1,509 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// ── I2C Pins ────────────────────────────────────────────── +#define SDA_PIN 8 +#define SCL_PIN 9 + +// ── SPI Pins ────────────────────────────────────────────── +#define FLASH_CS 7 +#define FLASH_SCK 4 +#define FLASH_MISO 5 +#define FLASH_MOSI 6 + +// ── I2C Addresses ───────────────────────────────────────── +#define BMP280_ADDRESS 0x76 +#define MPU6500_ADDRESS 0x68 + +// ── Timing ──────────────────────────────────────────────── +#define SENSOR_INTERVAL_MS 10 // 100Hz +#define LORA_INTERVAL_MS 100 // 10Hz + +// ── Flash layout ────────────────────────────────────────── +#define FLASH_META_ADDR 0x000000 // stores write pointer +#define FLASH_DATA_START 0x001000 // data starts after first sector +#define FLASH_SIZE 0x800000 // 8MB — adjust for your chip + +// ── Launch detection thresholds ─────────────────────────── +#define LAUNCH_ACCEL_THRESHOLD 2.5f // g — resultant G above this = launch +#define LANDING_STABLE_SECONDS 5 // seconds of stable altitude = landed + +// ── Objects ─────────────────────────────────────────────── +Adafruit_BMP280 bmp; +MPU6500_WE mpu(MPU6500_ADDRESS); +Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345); +SPIFlash flash(FLASH_CS); + +// ── Packet struct — 88 bytes ────────────────────────────── +struct SensorPacket { + uint32_t timestamp; + float bmp_temp, bmp_press, bmp_alt; + float accX, accY, accZ; + float gyrX, gyrY, gyrZ; + float pitch, roll, yaw; + float resultant, mpu_temp; + float magX, magY, magZ; + float heading, headingComp; + uint8_t flightPhase; // 0=standby 1=launch 2=coast 3=descent 4=landed + uint8_t padding[3]; // align to 4 bytes +}; // 88 bytes total + +// ── Queues ──────────────────────────────────────────────── +QueueHandle_t sensorQueue; +QueueHandle_t loraQueue; + +// ── Shared state (volatile = always re-read from RAM) ───── +volatile bool recording = false; +volatile bool playback = false; +volatile uint32_t writeAddr = FLASH_DATA_START; +volatile uint32_t packetCount = 0; + +// ── Flight state ────────────────────────────────────────── +volatile uint8_t flightPhase = 0; +float baseAltitude = 0; +float lastAltitude = 0; +uint32_t stableAltitudeTimer = 0; + +// ── Calibration ─────────────────────────────────────────── +float yawAngle = 0; +unsigned long lastYawTime = 0; +float initialHeading = -1; +float magOffsetX = 0, magOffsetY = 0, magOffsetZ = 0; + +// ══════════════════════════════════════════════════════════ +// SENSOR TASK — highest priority +// ══════════════════════════════════════════════════════════ +void sensorTask(void* pvParameters) { + TickType_t lastWake = xTaskGetTickCount(); + + for (;;) { + SensorPacket pkt; + pkt.timestamp = millis(); + + // ── MPU6500 ───────────────────────────────────────── + xyzFloat accelG = mpu.getGValues(); + xyzFloat gyro = mpu.getGyrValues(); + + pkt.accX = accelG.x * 9.81f; + pkt.accY = accelG.y * 9.81f; + pkt.accZ = accelG.z * 9.81f; + pkt.gyrX = gyro.x; + pkt.gyrY = gyro.y; + pkt.gyrZ = gyro.z; + pkt.mpu_temp = mpu.getTemperature(); + pkt.pitch = mpu.getPitch(); + pkt.roll = mpu.getRoll(); + pkt.resultant = mpu.getResultantG(accelG); + + // ── Yaw integration ────────────────────────────────── + unsigned long nowYaw = millis(); + float dtYaw = (nowYaw - lastYawTime) / 1000.0f; + lastYawTime = nowYaw; + yawAngle += gyro.z * dtYaw; + if (yawAngle > 180) yawAngle -= 360; + if (yawAngle < -180) yawAngle += 360; + pkt.yaw = yawAngle; + + // ── BMP280 ─────────────────────────────────────────── + pkt.bmp_temp = bmp.readTemperature(); + pkt.bmp_press = bmp.readPressure() / 100.0f; + pkt.bmp_alt = bmp.readAltitude(1013.25f); + + // ── HMC5883L ───────────────────────────────────────── + sensors_event_t magEvent; + mag.getEvent(&magEvent); + float rawX = magEvent.magnetic.x - magOffsetX; + float rawY = magEvent.magnetic.y - magOffsetY; + float rawZ = magEvent.magnetic.z - magOffsetZ; + pkt.magX = rawY; + pkt.magY = -rawX; + pkt.magZ = rawZ; + + float pitchRad = pkt.pitch * PI / 180.0f; + float rollRad = pkt.roll * PI / 180.0f; + float mXc = pkt.magX * cos(pitchRad) + pkt.magZ * sin(pitchRad); + float mYc = pkt.magX * sin(rollRad) * sin(pitchRad) + + pkt.magY * cos(rollRad) + - pkt.magZ * sin(rollRad) * cos(pitchRad); + pkt.heading = atan2(pkt.magY, pkt.magX) * 180.0f / PI; + if (pkt.heading < 0) pkt.heading += 360.0f; + pkt.headingComp = atan2(mYc, mXc) * 180.0f / PI; + if (pkt.headingComp < 0) pkt.headingComp += 360.0f; + + if (initialHeading < 0) initialHeading = pkt.headingComp; + + // ── Flight phase detection ──────────────────────────── + switch (flightPhase) { + case 0: // standby + if (pkt.resultant > LAUNCH_ACCEL_THRESHOLD) { + flightPhase = 1; + baseAltitude = pkt.bmp_alt; + recording = true; + Serial.println(F("[FLIGHT] Launch detected!")); + } + break; + + case 1: // launch / boost + if (pkt.resultant < 1.1f) { + flightPhase = 2; + Serial.println(F("[FLIGHT] Coasting...")); + } + break; + + case 2: // coasting up + if (pkt.bmp_alt < lastAltitude) { + flightPhase = 3; + Serial.println(F("[FLIGHT] Descending...")); + } + break; + + case 3: // descent + if (fabs(pkt.bmp_alt - lastAltitude) < 0.5f) { + if (millis() - stableAltitudeTimer > LANDING_STABLE_SECONDS * 1000) { + flightPhase = 4; + recording = false; + Serial.println(F("[FLIGHT] Landed.")); + } + } else { + stableAltitudeTimer = millis(); + } + break; + + case 4: // landed + break; + } + + lastAltitude = pkt.bmp_alt; + pkt.flightPhase = flightPhase; + + // ── Push to queues ──────────────────────────────────── + xQueueSend(sensorQueue, &pkt, 0); + xQueueSend(loraQueue, &pkt, 0); + + // ── Precise 10ms interval ───────────────────────────── + vTaskDelayUntil(&lastWake, pdMS_TO_TICKS(SENSOR_INTERVAL_MS)); + } +} + +// ══════════════════════════════════════════════════════════ +// STORAGE TASK — medium priority +// ══════════════════════════════════════════════════════════ +void storageTask(void* pvParameters) { + SensorPacket pkt; + + for (;;) { + if (xQueueReceive(sensorQueue, &pkt, portMAX_DELAY)) { + if (recording) { + // Check we haven't run out of flash + if (writeAddr + sizeof(SensorPacket) < FLASH_SIZE) { + flash.writeByteArray(writeAddr, (uint8_t*)&pkt, sizeof(SensorPacket)); + writeAddr += sizeof(SensorPacket); + packetCount++; + } else { + recording = false; + Serial.println(F("[FLASH] Memory full!")); + } + } + } + } +} + +// ══════════════════════════════════════════════════════════ +// LORA TASK — lowest priority +// ══════════════════════════════════════════════════════════ +void loraTask(void* pvParameters) { + SensorPacket pkt; + TickType_t lastWake = xTaskGetTickCount(); + + for (;;) { + // Drain queue, keep only latest packet + SensorPacket latest; + bool hasPacket = false; + while (xQueueReceive(loraQueue, &pkt, 0)) { + latest = pkt; + hasPacket = true; + } + + if (hasPacket) { + // ── TODO: replace with your LoRa library send call ── + // Example for RadioHead or LoRa.h: + // LoRa.beginPacket(); + // LoRa.write((uint8_t*)&latest, sizeof(latest)); + // LoRa.endPacket(); + + // For now, send DATA: line over serial as placeholder + Serial.print(F("DATA:")); + Serial.print(latest.bmp_temp, 2); Serial.print(F(",")); + Serial.print(latest.bmp_press, 2); Serial.print(F(",")); + Serial.print(latest.bmp_alt, 2); Serial.print(F(",")); + Serial.print(latest.accX, 4); Serial.print(F(",")); + Serial.print(latest.accY, 4); Serial.print(F(",")); + Serial.print(latest.accZ, 4); Serial.print(F(",")); + Serial.print(latest.gyrX, 2); Serial.print(F(",")); + Serial.print(latest.gyrY, 2); Serial.print(F(",")); + Serial.print(latest.gyrZ, 2); Serial.print(F(",")); + Serial.print(latest.pitch, 2); Serial.print(F(",")); + Serial.print(latest.roll, 2); Serial.print(F(",")); + Serial.print(latest.yaw, 2); Serial.print(F(",")); + Serial.print(latest.resultant, 4); Serial.print(F(",")); + Serial.print(latest.mpu_temp, 2); Serial.print(F(",")); + Serial.print(latest.magX, 4); Serial.print(F(",")); + Serial.print(latest.magY, 4); Serial.print(F(",")); + Serial.print(latest.magZ, 4); Serial.print(F(",")); + Serial.print(latest.heading, 2); Serial.print(F(",")); + Serial.print(latest.headingComp,2); Serial.print(F(",")); + Serial.println(latest.flightPhase); + + Serial.print(F("ORIENT:")); + Serial.print(latest.pitch, 2); Serial.print(F(",")); + Serial.print(latest.roll, 2); Serial.print(F(",")); + Serial.println(latest.yaw, 2); + } + + vTaskDelayUntil(&lastWake, pdMS_TO_TICKS(LORA_INTERVAL_MS)); + } +} + +// ══════════════════════════════════════════════════════════ +// SERIAL COMMAND TASK — handles user commands +// ══════════════════════════════════════════════════════════ +void serialCommandTask(void* pvParameters) { + for (;;) { + if (Serial.available()) { + char cmd = Serial.read(); + + switch (cmd) { + case 'r': + case 'R': + // Manual recording toggle + recording = !recording; + Serial.println(recording ? F("[REC] Started") : F("[REC] Stopped")); + break; + + case 'e': + case 'E': + // Erase flash + Serial.println(F("[FLASH] Erasing... this takes a few seconds")); + recording = false; + flash.eraseChip(); + writeAddr = FLASH_DATA_START; + packetCount = 0; + Serial.println(F("[FLASH] Erased.")); + break; + + case 'd': + case 'D': + // Dump all recorded data over serial + dumpFlash(); + break; + + case 's': + case 'S': + // Status + Serial.println(F("\n── Status ──────────────────")); + Serial.print(F("Recording : ")); Serial.println(recording ? F("YES") : F("NO")); + Serial.print(F("Packets : ")); Serial.println(packetCount); + Serial.print(F("Flash used: ")); + Serial.print((writeAddr - FLASH_DATA_START) / 1024); + Serial.println(F(" KB")); + Serial.print(F("Flight : ")); Serial.println(flightPhase); + Serial.println(F("────────────────────────────")); + Serial.println(F("Commands: R=record E=erase D=dump S=status")); + break; + } + } + vTaskDelay(pdMS_TO_TICKS(50)); + } +} + +// ══════════════════════════════════════════════════════════ +// FLASH DUMP — reads all packets and prints as CSV +// ══════════════════════════════════════════════════════════ +void dumpFlash() { + if (packetCount == 0) { + Serial.println(F("[DUMP] No data recorded.")); + return; + } + + Serial.println(F("[DUMP] Starting...")); + Serial.println(F("timestamp,bmp_temp,bmp_press,bmp_alt,accX,accY,accZ,gyrX,gyrY,gyrZ,pitch,roll,yaw,resultant,mpu_temp,magX,magY,magZ,heading,headingComp,phase")); + + uint32_t addr = FLASH_DATA_START; + uint32_t count = 0; + + while (addr < writeAddr && count < packetCount) { + SensorPacket pkt; + flash.readByteArray(addr, (uint8_t*)&pkt, sizeof(SensorPacket)); + + Serial.print(pkt.timestamp); Serial.print(F(",")); + Serial.print(pkt.bmp_temp, 2); Serial.print(F(",")); + Serial.print(pkt.bmp_press, 2); Serial.print(F(",")); + Serial.print(pkt.bmp_alt, 2); Serial.print(F(",")); + Serial.print(pkt.accX, 4); Serial.print(F(",")); + Serial.print(pkt.accY, 4); Serial.print(F(",")); + Serial.print(pkt.accZ, 4); Serial.print(F(",")); + Serial.print(pkt.gyrX, 2); Serial.print(F(",")); + Serial.print(pkt.gyrY, 2); Serial.print(F(",")); + Serial.print(pkt.gyrZ, 2); Serial.print(F(",")); + Serial.print(pkt.pitch, 2); Serial.print(F(",")); + Serial.print(pkt.roll, 2); Serial.print(F(",")); + Serial.print(pkt.yaw, 2); Serial.print(F(",")); + Serial.print(pkt.resultant, 4); Serial.print(F(",")); + Serial.print(pkt.mpu_temp, 2); Serial.print(F(",")); + Serial.print(pkt.magX, 4); Serial.print(F(",")); + Serial.print(pkt.magY, 4); Serial.print(F(",")); + Serial.print(pkt.magZ, 4); Serial.print(F(",")); + Serial.print(pkt.heading, 2); Serial.print(F(",")); + Serial.print(pkt.headingComp,2); Serial.print(F(",")); + Serial.println(pkt.flightPhase); + + addr += sizeof(SensorPacket); + count++; + } + + Serial.print(F("[DUMP] Done. ")); + Serial.print(count); + Serial.println(F(" packets.")); +} + +// ══════════════════════════════════════════════════════════ +// CALIBRATION FUNCTIONS +// ══════════════════════════════════════════════════════════ +void calibrateMag() { + float minX = 99999, maxX = -99999; + float minY = 99999, maxY = -99999; + float minZ = 99999, maxZ = -99999; + + Serial.println(F("[HMC5883L] Calibrating — rotate in all directions for 15 seconds...")); + + unsigned long start = millis(); + while (millis() - start < 15000) { + sensors_event_t e; + mag.getEvent(&e); + if (e.magnetic.x < minX) minX = e.magnetic.x; + if (e.magnetic.x > maxX) maxX = e.magnetic.x; + if (e.magnetic.y < minY) minY = e.magnetic.y; + if (e.magnetic.y > maxY) maxY = e.magnetic.y; + if (e.magnetic.z < minZ) minZ = e.magnetic.z; + if (e.magnetic.z > maxZ) maxZ = e.magnetic.z; + delay(10); + } + + magOffsetX = (maxX + minX) / 2.0f; + magOffsetY = (maxY + minY) / 2.0f; + magOffsetZ = (maxZ + minZ) / 2.0f; + + Serial.println(F("[HMC5883L] Done.")); + Serial.print(F(" Offsets: ")); + Serial.print(magOffsetX); Serial.print(F(", ")); + Serial.print(magOffsetY); Serial.print(F(", ")); + Serial.println(magOffsetZ); +} + +void scanI2C() { + Serial.println(F("Scanning I2C bus...")); + byte count = 0; + for (byte addr = 1; addr < 127; addr++) { + Wire.beginTransmission(addr); + if (Wire.endTransmission() == 0) { + Serial.print(F(" Found device at 0x")); + Serial.println(addr, HEX); + count++; + } + } + if (count == 0) Serial.println(F(" No devices found.")); +} + +// ══════════════════════════════════════════════════════════ +// SETUP +// ══════════════════════════════════════════════════════════ +void setup() { + Serial.begin(115200); + delay(1000); + + Wire.begin(SDA_PIN, SCL_PIN); + SPI.begin(FLASH_SCK, FLASH_MISO, FLASH_MOSI, FLASH_CS); + + Serial.println(F("╔══════════════════════════════╗")); + Serial.println(F("║ ESP32 Sensor System Boot ║")); + Serial.println(F("╚══════════════════════════════╝")); + + // ── BMP280 ──────────────────────────────────────────── + Serial.print(F("[BMP280] Initializing... ")); + if (!bmp.begin(BMP280_ADDRESS)) { + Serial.println(F("FAILED.")); scanI2C(); while (1) delay(1000); + } + bmp.setSampling( + Adafruit_BMP280::MODE_NORMAL, + Adafruit_BMP280::SAMPLING_X2, + Adafruit_BMP280::SAMPLING_X16, + Adafruit_BMP280::FILTER_X16, + Adafruit_BMP280::STANDBY_MS_63 + ); + Serial.println(F("OK")); + + // ── MPU6500 ─────────────────────────────────────────── + Serial.print(F("[MPU6500] Initializing... ")); + if (!mpu.init()) { + Serial.println(F("FAILED.")); scanI2C(); while (1) delay(1000); + } + mpu.setAccRange(MPU6500_ACC_RANGE_4G); + mpu.setGyrRange(MPU6500_GYRO_RANGE_250); + mpu.enableAccDLPF(true); + mpu.setAccDLPF(MPU6500_DLPF_6); + mpu.enableGyrDLPF(); + mpu.setGyrDLPF(MPU6500_DLPF_6); + mpu.setSampleRateDivider(5); + Serial.println(F("OK")); + + Serial.println(F("[MPU6500] Calibrating — keep still...")); + delay(1000); + mpu.autoOffsets(); + Serial.println(F("[MPU6500] Done.")); + + // ── HMC5883L ────────────────────────────────────────── + Serial.print(F("[HMC5883L] Initializing... ")); + if (!mag.begin()) { + Serial.println(F("FAILED.")); scanI2C(); while (1) delay(1000); + } + Serial.println(F("OK")); + calibrateMag(); + + // ── Flash ───────────────────────────────────────────── + Serial.print(F("[FLASH] Initializing... ")); + if (!flash.begin()) { + Serial.println(F("FAILED. Check SPI wiring.")); + while (1) delay(1000); + } + Serial.println(F("OK")); + Serial.print(F("[FLASH] Capacity: ")); + Serial.print(flash.getCapacity() / 1024); + Serial.println(F(" KB")); + + // ── Queues & Tasks ──────────────────────────────────── + sensorQueue = xQueueCreate(100, sizeof(SensorPacket)); + loraQueue = xQueueCreate(10, sizeof(SensorPacket)); + + xTaskCreate(sensorTask, "Sensors", 8192, NULL, 4, NULL); + xTaskCreate(storageTask, "Storage", 4096, NULL, 3, NULL); + xTaskCreate(loraTask, "LoRa", 4096, NULL, 2, NULL); + xTaskCreate(serialCommandTask,"Serial", 4096, NULL, 1, NULL); + + lastYawTime = millis(); + + Serial.println(F("\nSystem ready.")); + Serial.println(F("Commands: R=record E=erase D=dump S=status")); + Serial.println(F("Auto-recording starts on launch detection.\n")); +} + +void loop() { + vTaskDelay(portMAX_DELAY); +} \ No newline at end of file