v1
This commit is contained in:
+509
@@ -0,0 +1,509 @@
|
|||||||
|
#include <Wire.h>
|
||||||
|
#include <SPI.h>
|
||||||
|
#include <Adafruit_BMP280.h>
|
||||||
|
#include <MPU6500_WE.h>
|
||||||
|
#include <Adafruit_HMC5883_U.h>
|
||||||
|
#include <Adafruit_Sensor.h>
|
||||||
|
#include <SPIMemory.h>
|
||||||
|
#include <freertos/FreeRTOS.h>
|
||||||
|
#include <freertos/task.h>
|
||||||
|
#include <freertos/queue.h>
|
||||||
|
|
||||||
|
// ── 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);
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user