first upload

esp32s3_drone.ino

@@ -0,0 +1,103 @@
+#include <Arduino.h>
+#include <Adafruit_MPU6050.h>
+#include <Adafruit_Sensor.h>
+#include <ESP32Servo.h>
+#include <Wire.h>
+
+// Shared variables between cores (use volatile for safety)
+volatile float pitch, roll;
+volatile int throttle = 1100; // Default, low throttle for safety
+volatile int commandYaw = 0, commandPitch = 0, commandRoll = 0;
+
+// Setup peripherals
+Adafruit_MPU6050 mpu;
+Servo esc1, esc2, esc3, esc4;
+
+void setup() {
+  Serial.begin(115200);
+  Wire.begin();
+
+  // Initialize IMU
+  if (!mpu.begin()) {
+    Serial.println("MPU6050 not found!");
+    while(1);
+  }
+
+  // Attach ESCs
+  esc1.attach(4);
+  esc2.attach(5);
+  esc3.attach(6);
+  esc4.attach(7);
+
+  // Create tasks on each core
+  xTaskCreatePinnedToCore(
+    stabilizationTask,    // Function to run
+    "StabilizationTask",  // Name
+    10000,                // Stack size (bytes)
+    NULL,                 // Parameter
+    1,                    // Priority (1 = higher than command task)
+    NULL,                 // Task handle
+    0                     // Core number (0 for stabilization)
+  );
+
+  xTaskCreatePinnedToCore(
+    commandTask,          // Function to run
+    "CommandTask",        // Name
+    10000,                // Stack size (bytes)
+    NULL,                 // Parameter
+    0,                    // Priority (0 = lower than stabilization)
+    NULL,                 // Task handle
+    1                     // Core number (1 for command)
+  );
+}
+
+void loop() {} // Not used in FreeRTOS tasks
+
+// Stabilization task (Core 0)
+void stabilizationTask(void *pvParameters) {
+  sensors_event_t a, g, temp;
+  float dt, last_time = micros();
+  while (1) {
+    mpu.getEvent(&a, &g, &temp);
+    dt = (micros() - last_time) * 1e-6;
+    last_time = micros();
+
+    // Calculate pitch and roll (simplified)
+    pitch = atan2(a.acceleration.y, sqrt(a.acceleration.x*a.acceleration.x + a.acceleration.z*a.acceleration.z));
+    roll = atan2(-a.acceleration.x, a.acceleration.z);
+
+    // PID calculation (simplified)
+    // ... Add your PID code here ...
+
+    // Motor mixing (simplified)
+    int m1 = throttle; // Example, adjust for your mixing logic
+    int m2 = throttle;
+    int m3 = throttle;
+    int m4 = throttle;
+
+    // Constrain and write to ESCs
+    esc1.writeMicroseconds(constrain(m1, 1100, 1900));
+    esc2.writeMicroseconds(constrain(m2, 1100, 1900));
+    esc3.writeMicroseconds(constrain(m3, 1100, 1900));
+    esc4.writeMicroseconds(constrain(m4, 1100, 1900));
+
+    vTaskDelay(1); // Yield to other tasks
+  }
+}
+
+// Command task (Core 1)
+void commandTask(void *pvParameters) {
+  while (1) {
+    // Example: Read commands from serial or WiFi
+    if (Serial.available()) {
+      String cmd = Serial.readStringUntil('\n');
+      // Parse command and update throttle, yaw, pitch, roll
+      // ... Add your command parsing code here ...
+    }
+
+    // Example: If no command, maintain hover (throttle = hover value)
+    // ... Add your logic for autonomous hover here ...
+
+    vTaskDelay(10); // Adjust as needed for command rate
+  }
+}