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

wearegogog1232026-03-24 11:16

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

c 复制代码 
#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);
  
  // 初始化I2C
  Wire.begin(21, 22);  // SDA=GPIO21, SCL=GPIO22
  
  // 初始化MPU6050
  mpu.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();
  
  // 初始化PID
  roll_pid.SetMode(AUTOMATIC);
  roll_pid.SetOutputLimits(-100, 100);
  roll_pid.SetSampleTime(10);  // 10ms
  
  pitch_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);
  
  // 校准IMU
  calibrateIMU();
  
  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. 传感器数据处理

c 复制代码 
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. 姿态解算（互补滤波）

c 复制代码 
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控制实现

c 复制代码 
// 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. 电机混控算法

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

  • 将PID输出转换为四个电机的速度指令

  • 考虑不同电机旋转方向（顺时针/逆时针）

  • 实现姿态控制与油门的混合

5. 安全机制

c 复制代码 
void emergencyStop() {
  stopMotors();
  while(1) { /* 停止所有操作 */ }
}

  • 紧急停止功能

  • 电机失控保护

  • 低电量检测（需电池监测电路）

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

扩展功能代码示例

1. 气压计定高功能

c 复制代码 
#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图传功能

c 复制代码 
#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. 光流定点功能

c 复制代码 
#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();
}

开发注意事项

  1. 安全优先

    • 始终在开阔空间测试

    • 首次飞行时移除螺旋桨

    • 实现失控保护机制

  2. 参数调试

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

  3. 电源管理

    • 使用单独的电调供电

    • 添加电压监测电路

    • 实现低电量自动降落

  4. 无线通信

    c 复制代码 
    #include <WiFi.h>
#include <WebServer.h>

WebServer server(80);

void setupWebControl() {
  WiFi.softAP("Drone_AP", "password");
  server.on("/", handleRoot);
  server.begin();
}

void handleRoot() {
  // 返回控制页面HTML
}
相关推荐
UQ_rookie16 小时前
【Unity3D】在URP渲染管线下使用liltoon插件出现粉色无法渲染情况的解决方案
unity·游戏引擎·shader·urp·着色器·vrchat·liltoon
金戈鐡馬1 天前
BetaFlight中的定时器引脚绑定详解
stm32·单片机·嵌入式硬件·无人机
weixin_Todd_Wong20101 天前
毫米波雷达传感器远距探测距离最远 260m 40 个多目标跟踪
无人机
强盛机器学习~2 天前
考虑异常天气和太阳辐射下基于强化学习的无人机三维路径规划
算法·matlab·无人机·强化学习·路径规划·无人机路径规划·q-learning
嵌入式Linux,2 天前
大疆，无人机霸主
无人机
南無忘码至尊2 天前
Unity学习90天-第1天-认识Transform + 坐标系
学习·unity·游戏引擎
南無忘码至尊2 天前
Unity学习90天-第1天-认识Unity并书写我们的第一个脚本
学习·unity·游戏引擎
风酥糖2 天前
Godot游戏练习01-第26节-轮次结束后弹出升级选项
游戏·游戏引擎·godot
雪域迷影2 天前
Hazel游戏引擎结构分析
c++·游戏引擎·hazel
weixin_409383122 天前
godot创建两种敌人僵尸 一种吐舌头 一种在角色脚下生成圆形伤害圈 两种僵尸均继承enemy脚本 理解继承
游戏引擎·godot
热门推荐
01GitHub 镜像站点022026年4月技术前沿：AI大模型爆发、智能体革命与量子安全新纪元03一周AI热点速览（2026.03.31-04.06）：GPT-6曝光、谷歌开源Gemma 4、资本狂飙与模型军备竞赛04基于 Docker 部署 Hermes Agent 并接入飞书机器人的完整指南05免费！不限量！用opencode接入英伟达（NVIDIA）大模型，轻松打造你的 AI 编程助手06Oh My Codex 快速使用指南07从限购到畅通：GLM-5.1 Coding Plan接入攻略08LLM Wiki：让大模型替你打理知识库的完整指南09CodeBuddy与WorkBuddy深度对比：腾讯两款AI工具差异及实操指南10AI Weekly | 2026年4月第二周 · GitHub热门项目与AI发展趋势深度解析