当前位置：首页 > news >正文

ESP32迷你无人机开发代码详解

news 2026/3/26 18:27:46

ESP32迷你无人机的核心代码实现，涵盖传感器读取、姿态解算、PID控制和电机驱动等关键模块。代码基于Arduino框架开发，兼容ESP32系列芯片。

#include <Wire.h>
#include <MPU6050.h>
#include <PID_v1.h>
#include <ESP32Servo.h>// ===== 硬件配置 =====
#define MOTOR1_PIN 12
#define MOTOR2_PIN 13
#define MOTOR3_PIN 14
#define MOTOR4_PIN 15#define MPU6050_ADDR 0x68
#define BMP280_ADDR 0x76// ===== 飞行参数 =====
const float Kp_roll = 0.8;    // 横滚PID比例系数
const float Ki_roll = 0.1;    // 横滚PID积分系数
const float Kd_roll = 0.05;   // 横滚PID微分系数const float Kp_pitch = 0.8;   // 俯仰PID比例系数
const float Ki_pitch = 0.1;   // 俯仰PID积分系数
const float Kd_pitch = 0.05;  // 俯仰PID微分系数const float Kp_yaw = 1.2;     // 偏航PID比例系数
const float Ki_yaw = 0.01;    // 偏航PID积分系数
const float Kd_yaw = 0.1;     // 偏航PID微分系数// ===== 全局变量 =====
MPU6050 mpu;
float accel[3], gyro[3];  // 加速度计和陀螺仪数据
float roll, pitch, yaw;   // 姿态角(度)
float base_roll, base_pitch, base_yaw;  // 初始姿态double set_roll = 0, set_pitch = 0, set_yaw = 0;  // 目标角度
double pid_roll, pid_pitch, pid_yaw;              // PID输出Servo motor1, motor2, motor3, motor4;
int motor_speed[4] = {0, 0, 0, 0};  // 电机速度(0-180)// PID控制器实例
PID roll_pid(&roll, &pid_roll, &set_roll, Kp_roll, Ki_roll, Kd_roll, DIRECT);
PID pitch_pid(&pitch, &pid_pitch, &set_pitch, Kp_pitch, Ki_pitch, Kd_pitch, DIRECT);
PID yaw_pid(&yaw, &pid_yaw, &set_yaw, Kp_yaw, Ki_yaw, Kd_yaw, DIRECT);// ===== 初始化函数 =====
void setup() {Serial.begin(115200);// 初始化I2CWire.begin(21, 22);  // SDA=GPIO21, SCL=GPIO22// 初始化MPU6050mpu.initialize();if (!mpu.testConnection()) {Serial.println("MPU6050 connection failed!");while (1);}// 初始化电机motor1.attach(MOTOR1_PIN);motor2.attach(MOTOR2_PIN);motor3.attach(MOTOR3_PIN);motor4.attach(MOTOR4_PIN);stopMotors();// 初始化PIDroll_pid.SetMode(AUTOMATIC);roll_pid.SetOutputLimits(-100, 100);roll_pid.SetSampleTime(10);  // 10mspitch_pid.SetMode(AUTOMATIC);pitch_pid.SetOutputLimits(-100, 100);pitch_pid.SetSampleTime(10);yaw_pid.SetMode(AUTOMATIC);yaw_pid.SetOutputLimits(-100, 100);yaw_pid.SetSampleTime(10);// 校准IMUcalibrateIMU();Serial.println("Drone initialized successfully!");
}// ===== 主循环 =====
void loop() {static uint32_t last_time = 0;uint32_t now = millis();float dt = (now - last_time) / 1000.0;  // 转换为秒last_time = now;if (dt > 0.1) return;  // 防止长时间阻塞// 1. 读取传感器数据readIMU();// 2. 姿态解算computeAttitude(dt);// 3. 获取遥控输入getRCInput();// 4. 计算PID输出roll_pid.Compute();pitch_pid.Compute();yaw_pid.Compute();// 5. 计算电机速度computeMotorSpeeds();// 6. 更新电机输出updateMotors();// 7. 调试输出debugOutput();delay(5);  // 控制循环频率
}// ===== 传感器读取 =====
void readIMU() {int16_t ax, ay, az, gx, gy, gz;mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);// 转换为g和度/秒accel[0] = ax / 16384.0;  // X轴加速度 (g)accel[1] = ay / 16384.0;  // Y轴加速度 (g)accel[2] = az / 16384.0;  // Z轴加速度 (g)gyro[0] = gx / 131.0;  // X轴角速度 (°/s)gyro[1] = gy / 131.0;  // Y轴角速度 (°/s)gyro[2] = gz / 131.0;  // Z轴角速度 (°/s)
}// ===== 姿态解算(互补滤波) =====
void computeAttitude(float dt) {// 加速度计计算角度float accel_roll = atan2(accel[1], accel[2]) * RAD_TO_DEG;float accel_pitch = atan2(-accel[0], sqrt(accel[1]*accel[1] + accel[2]*accel[2])) * RAD_TO_DEG;// 陀螺仪积分roll += gyro[0] * dt;pitch += gyro[1] * dt;yaw += gyro[2] * dt;// 互补滤波融合roll = 0.98 * (roll + gyro[0] * dt) + 0.02 * accel_roll;pitch = 0.98 * (pitch + gyro[1] * dt) + 0.02 * accel_pitch;
}// ===== 遥控器输入处理 =====
void getRCInput() {// 实际项目中应替换为真实的遥控接收机代码// 这里使用模拟值代替set_roll = map(analogRead(36), 0, 4095, -30, 30);  // 横滚控制set_pitch = map(analogRead(39), 0, 4095, -30, 30); // 俯仰控制set_yaw = map(analogRead(34), 0, 4095, -45, 45);   // 偏航控制// 油门控制int throttle = map(analogRead(35), 0, 4095, 0, 100);// 更新目标角度（实际应用中应为PID设定值）set_roll = constrain(set_roll, -30, 30);set_pitch = constrain(set_pitch, -30, 30);set_yaw = constrain(set_yaw, -45, 45);
}// ===== 电机速度计算 =====
void computeMotorSpeeds() {// 基础油门（实际应用中应从遥控器获取）int throttle = 50;  // 50%油门// 电机布局（X型四轴）：// 电机1: 右前 (CW)// 电机2: 左后 (CW)// 电机3: 右后 (CCW)// 电机4: 左前 (CCW)// 计算每个电机的输出motor_speed[0] = throttle + pid_pitch + pid_roll - pid_yaw;  // 右前motor_speed[1] = throttle - pid_pitch + pid_roll + pid_yaw;  // 左后motor_speed[2] = throttle - pid_pitch - pid_roll - pid_yaw;  // 右后motor_speed[3] = throttle + pid_pitch - pid_roll + pid_yaw;  // 左前// 限制范围并转换为舵机信号for (int i = 0; i < 4; i++) {motor_speed[i] = constrain(motor_speed[i], 0, 100);motor_speed[i] = map(motor_speed[i], 0, 100, 0, 180);  // 转换为舵机信号(0-180)}
}// ===== 更新电机输出 =====
void updateMotors() {motor1.write(motor_speed[0]);motor2.write(motor_speed[1]);motor3.write(motor_speed[2]);motor4.write(motor_speed[3]);
}// ===== 停止所有电机 =====
void stopMotors() {motor1.write(0);motor2.write(0);motor3.write(0);motor4.write(0);
}// ===== IMU校准 =====
void calibrateIMU() {Serial.println("Calibrating IMU... Keep drone level");delay(2000);// 实际项目中应实现更精确的校准base_roll = 0;base_pitch = 0;base_yaw = 0;Serial.println("Calibration complete");
}// ===== 调试输出 =====
void debugOutput() {static uint32_t last_debug = 0;if (millis() - last_debug > 500) {last_debug = millis();Serial.print("Roll: ");Serial.print(roll);Serial.print("\tPitch: ");Serial.print(pitch);Serial.print("\tYaw: ");Serial.println(yaw);Serial.print("Motor1: ");Serial.print(motor_speed[0]);Serial.print("\tMotor2: ");Serial.print(motor_speed[1]);Serial.print("\tMotor3: ");Serial.print(motor_speed[2]);Serial.print("\tMotor4: ");Serial.println(motor_speed[3]);}
}// ===== 紧急停止 =====
void emergencyStop() {stopMotors();Serial.println("EMERGENCY STOP ACTIVATED");while (1) {// 闪烁LED或其他警示delay(500);}
}

关键代码模块解析

1. 传感器数据处理

void readIMU() {int16_t ax, ay, az, gx, gy, gz;mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);// 转换为物理量accel[0] = ax / 16384.0;  // ±2g量程gyro[0] = gx / 131.0;     // ±250°/s量程// ...其他轴类似
}

使用MPU6050库读取原始传感器数据
将原始数据转换为物理单位(g和°/s)

2. 姿态解算（互补滤波）

void computeAttitude(float dt) {// 加速度计计算角度float accel_roll = atan2(accel[1], accel[2]) * RAD_TO_DEG;// 陀螺仪积分roll += gyro[0] * dt;// 互补滤波融合roll = 0.98 * (roll + gyro[0] * dt) + 0.02 * accel_roll;
}

结合加速度计（静态精度高）和陀螺仪（动态响应好）数据
使用加权融合算法获得更准确的姿态估计

3. PID控制实现

// PID实例化
PID roll_pid(&roll, &pid_roll, &set_roll, Kp_roll, Ki_roll, Kd_roll, DIRECT);void loop() {// ...roll_pid.Compute();  // 计算PID输出// ...
}

使用Arduino PID库实现三轴独立控制
每个轴有独立的PID参数（Kp, Ki, Kd）
输出限幅防止过度控制

4. 电机混控算法

void computeMotorSpeeds() {motor_speed[0] = throttle + pid_pitch + pid_roll - pid_yaw;  // 右前motor_speed[1] = throttle - pid_pitch + pid_roll + pid_yaw;  // 左后// ...其他电机
}

将PID输出转换为四个电机的速度指令
考虑不同电机旋转方向（顺时针/逆时针）
实现姿态控制与油门的混合

5. 安全机制

void emergencyStop() {stopMotors();while(1) { /* 停止所有操作 */ }
}

紧急停止功能
电机失控保护
低电量检测（需电池监测电路）

参考代码 ESP32开发-ESP32迷你无人机 www.youwenfan.com/contentcns/183132.html

扩展功能代码示例

1. 气压计定高功能

#include <Adafruit_BMP280.h>Adafruit_BMP280 bmp;
float target_altitude = 1.0;  // 目标高度(米)void setupAltimeter() {if (!bmp.begin(BMP280_ADDR)) {Serial.println("BMP280 not found!");while(1);}
}void maintainAltitude() {float pressure = bmp.readPressure();float altitude = bmp.readAltitude(1013.25);  // 海平面气压// 高度PID控制static PID alt_pid(&altitude, &throttle, &target_altitude, 2.0, 0.1, 0.5, DIRECT);alt_pid.Compute();
}

2. FPV图传功能

#include "esp_camera.h"void setupCamera() {camera_config_t config;// 配置摄像头参数config.ledc_channel = LEDC_CHANNEL_0;config.ledc_timer = LEDC_TIMER_0;// ...其他配置esp_err_t err = esp_camera_init(&config);if (err != ESP_OK) {Serial.printf("Camera init failed with error 0x%x", err);return;}
}void streamVideo() {camera_fb_t *fb = esp_camera_fb_get();if (!fb) return;// 通过WiFi发送图像帧sendFrameOverWiFi(fb->buf, fb->len);esp_camera_fb_return(fb);
}

3. 光流定点功能

#include <PMW3901.h>PMW3901 flowSensor(25, 26);  // CS, SCK引脚void setupOpticalFlow() {if (!flowSensor.begin()) {Serial.println("PMW3901 not found!");while(1);}
}void positionHold() {int16_t deltaX, deltaY;flowSensor.readMotionCount(&deltaX, &deltaY);// 转换为位移float dx = deltaX * 0.01;  // 根据实际测试调整系数float dy = deltaY * 0.01;// 位置PID控制static PID posX_pid(&dx, &pid_pitch, 0, 1.0, 0.01, 0.1, DIRECT);static PID posY_pid(&dy, &pid_roll, 0, 1.0, 0.01, 0.1, DIRECT);posX_pid.Compute();posY_pid.Compute();
}

开发注意事项

安全优先
- 始终在开阔空间测试
- 首次飞行时移除螺旋桨
- 实现失控保护机制

参数调试

// 使用串口调试助手调整PID参数
void tunePIDParameters() {if (Serial.available()) {String input = Serial.readStringUntil('\n');// 解析新参数并更新PIDsscanf(input.c_str(), "%f %f %f", &Kp_roll, &Ki_roll, &Kd_roll);roll_pid.SetTunings(Kp_roll, Ki_roll, Kd_roll);}
}

电源管理
- 使用单独的电调供电
- 添加电压监测电路
- 实现低电量自动降落

无线通信

#include <WiFi.h>
#include <WebServer.h>WebServer server(80);void setupWebControl() {WiFi.softAP("Drone_AP", "password");server.on("/", handleRoot);server.begin();
}void handleRoot() {// 返回控制页面HTML
}

查看全文

http://www.jsqmd.com/news/506782/