Building an ESP32 Flight Controller: IMU Wiring, PID Code, and I2C Debugging

If you want to build a custom ESP32 flight controller, the direct answer is to pair an ESP32-WROOM-32 DevKit with an MPU6050 IMU, use the LEDC peripheral for ESC PWM generation, and run a complementary filter alongside a PID loop on Core 1 while reserving Core 0 for WiFi telemetry. However, before you solder a single pin, you must understand that the ESP32 is not a drop-in replacement for dedicated flight silicon like the STM32F4 or H7. The ESP32 excels at wireless telemetry and rapid prototyping, but its FreeRTOS background tasks and Wi-Fi/Bluetooth RF interrupts introduce microsecond-level jitter that will destabilize high-speed acrobatic flight.

This guide provides the exact hardware bill of materials, a data-dense pin mapping table, and fully compilable C++ code to get a basic quadcopter or self-balancing platform hovering. We will also cover the exact I2C bus lockups that brick 90% of ESP32 drone builds on the bench.

ESP32 vs STM32F405: Choosing Your Flight Silicon

Most commercial flight controllers (Pixhawk, Matek, SpeedyBee) rely on STM32 microcontrollers. Here is how the ESP32 stacks up when pressed into flight control duties.

Verdict: Choose the ESP32 when your project requires native wireless mesh networking (ESP-NOW) or IoT telemetry. Choose STM32 if you are building a 5-inch freestyle quadcopter that needs sub-millisecond gyro-to-motor latency.

Hardware BOM and Pin Mapping

The code and wiring below target the ESP32 DevKit V1 (30-pin variant) featuring the ESP32-WROOM-32 module. Do not use the 38-pin variant without adjusting the pin definitions, as the strapping pins differ.

Parts List

• MCU: ESP32 DevKit V1 (30-pin, ESP32-WROOM-32)
• IMU: GY-521 Breakout (MPU6050, 3.3V/5V tolerant with onboard LDO)
• ESCs: 4x 30A BLHeli_S Opto-isolated ESCs (requires separate 5V BEC)
• Power: 3S 1500mAh LiPo + 5V 3A UBEC (to power ESP32 and IMU independently of ESC BECs)
• Motors: 2205 2300KV Brushless (for micro-quad testing)

ESP32 Flight Controller Pin Mapping

Wiring and Power Distribution Steps

1. Establish Common Ground: Solder the black (GND) wires from the UBEC, the MPU6050 breakout, and all four ESC signal harnesses to a single ground pad on your power distribution board (PDB). A floating ground between the ESP32 and ESCs will result in erratic PWM signals.
2. Wire the I2C Bus: Connect GPIO 22 to SCL and GPIO 21 to SDA. Critical: The GY-521 breakout includes 4.7kΩ pull-up resistors tied to its VCC pin. Because we are powering the GY-521 with 3.3V, the pull-ups will correctly pull the ESP32 I2C lines to 3.3V. Do not add external pull-ups to 5V, or you will fry the ESP32 GPIO pins.
3. Route ESC Signals: Connect the ESC signal wires to GPIO 16, 17, 18, and 19. Keep these wires away from the 2.4GHz WiFi antenna on the ESP32 module to prevent RF-induced PWM jitter.
4. Power Injection: Connect the 5V UBEC output to the ESP32 VIN pin (not 5V, as VIN routes through the onboard diode for protection). Connect the UBEC ground to ESP32 GND.
5. Verify with Multimeter: Before plugging in the LiPo, set your multimeter to continuity mode. Verify there is no short between the 5V rail and GND. Then, power on and verify exactly 3.25V to 3.35V at the MPU6050 VCC pin.

Compilable PID Stabilization Code

This sketch targets the ESP32 DevKit V1 (30-pin). It reads raw accelerometer and gyroscope data, applies a complementary filter to calculate pitch/roll angles, and runs a basic P-term controller to stabilize a single axis (Pitch) for bench testing. It uses the ESP32Servo library to handle the LEDC PWM setup for standard 400Hz ESC protocols.

Required Library: Install ESP32Servo by Kevin Harrington via the Arduino Library Manager.

#include <Wire.h>
#include <ESP32Servo.h>

// --- PIN DEFINITIONS (DevKit V1 30-Pin) ---
#define PIN_MOTOR_FL 16
#define PIN_MOTOR_FR 17
#define PIN_MOTOR_RL 18
#define PIN_MOTOR_RR 19

// --- IMU & FILTER CONSTANTS ---
#define MPU_ADDR 0x68
#define DT 0.004 // 4ms loop time (250Hz)
#define ALPHA 0.98 // Complementary filter coefficient

Servo escFL, escFR, escRL, escRR;

float pitch = 0.0;
float targetPitch = 0.0; // Setpoint (level)
float Kp = 2.5;          // Proportional gain (Tune this on bench)

void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22);
  Wire.setClock(400000); // 400kHz Fast Mode

  // Initialize MPU6050
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B); // PWR_MGMT_1
  Wire.write(0);    // Wake up
  if (Wire.endTransmission() != 0) {
    Serial.println("FATAL: MPU6050 I2C Init Failed. Check wiring.");
    while(1) { delay(1000); } // Halt safely
  }

  // Set Gyro to 500deg/s, Accel to 2g
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x1B); Wire.write(0x08);
  Wire.write(0x1C); Wire.write(0x00);
  Wire.endTransmission();

  // Attach ESCs (ESP32Servo handles LEDC channel allocation)
  escFL.attach(PIN_MOTOR_FL, 1000, 2000);
  escFR.attach(PIN_MOTOR_FR, 1000, 2000);
  escRL.attach(PIN_MOTOR_RL, 1000, 2000);
  escRR.attach(PIN_MOTOR_RR, 1000, 2000);

  // ESC Arming Sequence
  Serial.println("Arming ESCs... Keep props clear!");
  for(int i=0; i<50; i++) {
    escFL.writeMicroseconds(1000); escFR.writeMicroseconds(1000);
    escRL.writeMicroseconds(1000); escRR.writeMicroseconds(1000);
    delay(20);
  }
  Serial.println("Armed. Entering control loop.");
}

void loop() {
  unsigned long startTime = micros();

  // 1. Read IMU Data (Accel X/Y, Gyro Y)
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B); // ACCEL_XOUT_H
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, 14, true);

  if (Wire.available() >= 14) {
    int16_t ax = Wire.read()<<8 | Wire.read();
    int16_t ay = Wire.read()<<8 | Wire.read();
    Wire.read(); Wire.read(); // Skip AZ
    Wire.read(); Wire.read(); // Skip Temp
    Wire.read(); Wire.read(); // Skip GX
    int16_t gy = Wire.read()<<8 | Wire.read(); // Gyro Y (Pitch rate)

    // 2. Calculate Angles
    float accelAngle = atan2(ay, ax) * 180.0 / PI;
    float gyroRate = gy / 65.5; // 500deg/s sensitivity
    
    // Complementary Filter
    pitch = ALPHA * (pitch + gyroRate * DT) + (1.0 - ALPHA) * accelAngle;

    // 3. PID Control (P-term only for stability demo)
    float error = targetPitch - pitch;
    float pidOutput = Kp * error;

    // 4. Motor Mixing (Base throttle 1300us + PID correction)
    int baseThrottle = 1300; 
    int mFL = constrain(baseThrottle + pidOutput, 1000, 1800);
    int mFR = constrain(baseThrottle + pidOutput, 1000, 1800);
    int mRL = constrain(baseThrottle - pidOutput, 1000, 1800);
    int mRR = constrain(baseThrottle - pidOutput, 1000, 1800);

    escFL.writeMicroseconds(mFL); escFR.writeMicroseconds(mFR);
    escRL.writeMicroseconds(mRL); escRR.writeMicroseconds(mRR);
    
  } else {
    // Error Handling: I2C Bus Lockup
    Serial.println("ERR: I2C Buffer Underflow. Resetting bus.");
    Wire.end();
    Wire.begin(21, 22);
    Wire.setClock(400000);
  }

  // Maintain strict 250Hz loop timing
  while(micros() - startTime < 4000) { delayMicroseconds(10); }
}

Debugging: I2C Timeouts and Motor Jitter

The most common failure mode when building an ESP32 flight controller is the I2C bus locking up mid-flight, causing the watchdog to reset the board or the motors to freeze at their last PWM value. If your serial monitor outputs the following exact error string:

E (12345) i2c: i2c_master_cmd_begin(1451): i2c timeout
Guru Meditation Error: Core 1 panic'ed (Interrupt wdt timeout on CPU1)

This occurs because the ESP32's Wi-Fi/BT coexistence interrupt blocks the I2C hardware state machine, violating the I2C timing protocol and causing the MPU6050 to hold the SDA line low indefinitely. According to the official Espressif ESP32 Errata, this is a known silicon bug in earlier chip revisions when using I2C alongside active RF.

The First Three Things to Check When It Fails

1. Disable WiFi/BT if unused: If you are only bench-testing PID tuning and don't need telemetry, add WiFi.mode(WIFI_OFF); and btStop(); at the very beginning of your setup() function. This eliminates the RF interrupt contention entirely.
2. Check I2C Pull-Up Voltage: Verify with a multimeter that the SDA and SCL lines idle at 3.2V-3.3V. If they are floating around 1.5V, your pull-up resistors are too weak or tied to the wrong voltage rail. The ESP32 requires strong pull-ups (2.2kΩ to 3.3V) for 400kHz operation.
3. Implement Bus Recovery: Notice the else block in the code above. If Wire.requestFrom fails, you must physically re-initialize the Wire library. Simply retrying the read will not clear a locked SDA line.

Fixing ESC PWM Jitter

If your motors emit a "hunting" sound or twitch randomly while the ESP32 is armed but idle, you are experiencing PWM jitter. The ESP32's micros() function can drift slightly when FreeRTOS context switches occur. To fix this, ensure you are using the ESP32Servo library (which configures the hardware LEDC timers directly) rather than software-bit-banged PWM. Keep your control loop strictly timed using the while(micros() - startTime < 4000) blocking method shown in the code.

Extending and Simplifying the Build

Once you have the basic P-term stabilization running on the bench, you will need to adapt the project to your specific goals.

How to Simplify (For Beginners)

If tuning a quadcopter in 3D space is resulting in broken parts, simplify the build to a single-axis self-balancing robot. Mount the ESP32 and MPU6050 on a 2-wheeled chassis (like an N20 motor rover). This restricts the PID loop to managing only the Pitch axis. You can safely tune the Derivative (D) and Integral (I) terms on a 2D plane without the danger of a quadcopter flying into the ceiling.

How to Extend (For Advanced Makers)

• Add ESP-NOW Telemetry: Because the ESP32 has native 802.11, you can bypass traditional RC receivers. Implement the ESP-NOW protocol to receive joystick data from a secondary ESP32 transmitter with sub-5ms latency, freeing up UART pins and saving weight.
• Upgrade the IMU: The MPU6050 is obsolete and suffers from severe temperature drift. Upgrade to an ICM-20948 or BMI270. These require SPI communication rather than I2C, which completely bypasses the ESP32 I2C silicon bug and allows for DMA (Direct Memory Access) transfers, freeing up CPU cycles for more complex Kalman filtering.
• Implement ArduPilot: If you want full autonomous waypoint navigation, abandon the custom PID code and flash the ArduPilot ESP32 port. Note that ArduPilot on ESP32 requires specific board definitions and an external SPI IMU; it will not run on a raw DevKit with an I2C MPU6050.