103 lines
2.9 KiB
C++
103 lines
2.9 KiB
C++
#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
|
|
}
|
|
} |