How to Build and Debug an Arduino Flight Controller Using MPU6050

Building a custom Arduino flight controller from scratch is the ultimate test of your embedded systems knowledge. You are combining high-speed I2C sensor polling, digital signal filtering, and a real-time PID control loop into a single ATmega328P microcontroller. While commercial flight controllers running Betaflight or ArduPilot are the standard for 2026, writing your own bare-metal stabilization code on an Arduino Nano teaches you the physics and math that black-box firmware hides.

This guide targets the Arduino Nano (ATmega328P, 5V/16MHz) paired with the ubiquitous MPU6050 6-axis IMU. We will bypass the bloated DMP (Digital Motion Processor) libraries and write a lean, raw-data complementary filter with a PID loop that runs reliably at 250Hz.

Spec Sheet & Parts List

Pin Mapping & Wiring the IMU

The most common point of failure in DIY flight controllers is I2C bus noise and logic-level mismatch. The MPU6050 operates at 3.3V. Feeding it 5V from the Arduino Nano's VCC pin will fry the internal voltage regulator, and feeding 5V logic into its SDA/SCL pins will eventually degrade the I2C transceivers. Use a BSS138 logic level converter for the data lines.

The PID Control Loop (Complete Code)

This code implements a complementary filter to merge the noisy but drift-free accelerometer with the smooth but drifting gyroscope. It then runs a basic Rate-mode PID controller. We use the standard Arduino Wire library to keep dependencies minimal and avoid the memory bloat of third-party DMP libraries.

#include <Wire.h>
#include <Servo.h>

// --- PIN DEFINITIONS ---
#define MOTOR_FR 9
#define MOTOR_FL 10
#define MOTOR_RR 11
#define MOTOR_RL 3

// --- IMU CONSTANTS ---
#define MPU_ADDR 0x68
#define ACCEL_FS_SEL 0 // +/- 2g
#define GYRO_FS_SEL 1  // +/- 500 deg/s

// --- PID GAINS (Tune these on the bench first!) ---
float Kp_roll = 1.2, Ki_roll = 0.02, Kd_roll = 15.0;
float Kp_pitch = 1.2, Ki_pitch = 0.02, Kd_pitch = 15.0;
float Kp_yaw = 2.0, Ki_yaw = 0.01, Kd_yaw = 0.0;

// --- VARIABLES ---
Servo escFR, escFL, escRR, escRL;
float angle_roll, angle_pitch, angle_yaw;
float pid_error_roll, pid_error_pitch, pid_error_yaw;
float pid_i_mem_roll, pid_i_mem_pitch, pid_i_mem_yaw;
float pid_output_roll, pid_output_pitch, pid_output_yaw;
int throttle, esc_pulse_fr, esc_pulse_fl, esc_pulse_rr, esc_pulse_rl;
unsigned long loop_timer;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  TWBR = 12; // Set I2C clock to 400kHz

  // Initialize MPU6050
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B); Wire.write(0x00); // Wake up
  Wire.endTransmission(true);
  
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x1B); Wire.write(GYRO_FS_SEL << 3);
  Wire.endTransmission(true);
  
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x1C); Wire.write(ACCEL_FS_SEL << 3);
  Wire.endTransmission(true);

  escFR.attach(MOTOR_FR); escFL.attach(MOTOR_FL);
  escRR.attach(MOTOR_RR); escRL.attach(MOTOR_RL);
  
  // ESC Calibration Sequence (Wait for user to plug in battery)
  Serial.println("Connect battery now... Waiting 5s.");
  delay(5000);
  loop_timer = micros();
}

void loop() {
  // 1. Read IMU Data
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);
  byte status = Wire.endTransmission(false);
  
  if (status != 0) {
    Serial.print("ERR: I2C NACK on address (Code ");
    Serial.print(status);
    Serial.println("). Check wiring.");
    // Failsafe: Cut motors
    setMotors(1000);
    delay(100);
    return;
  }
  
  Wire.requestFrom(MPU_ADDR, 14, true);
  int16_t accel_x = Wire.read()<<8 | Wire.read();
  int16_t accel_y = Wire.read()<<8 | Wire.read();
  int16_t accel_z = Wire.read()<<8 | Wire.read();
  Wire.read(); Wire.read(); // Skip temp
  int16_t gyro_x = Wire.read()<<8 | Wire.read();
  int16_t gyro_y = Wire.read()<<8 | Wire.read();
  int16_t gyro_z = Wire.read()<<8 | Wire.read();

  // 2. Complementary Filter (0.9996 weight to gyro, 0.0004 to accel)
  float dt = 0.004; // 250Hz loop time
  float gyro_x_dps = gyro_x / 65.5;
  float gyro_y_dps = gyro_y / 65.5;
  float gyro_z_dps = gyro_z / 65.5;
  
  float accel_roll = atan2(accel_y, accel_z) * 57.296;
  float accel_pitch = atan2(accel_x, sqrt(accel_y*accel_y + accel_z*accel_z)) * 57.296;

  angle_roll = 0.9996 * (angle_roll + gyro_x_dps * dt) + 0.0004 * accel_roll;
  angle_pitch = 0.9996 * (angle_pitch + gyro_y_dps * dt) + 0.0004 * accel_pitch;
  angle_yaw += gyro_z_dps * dt;

  // 3. Receiver Input (Simulated via Serial for bench testing)
  throttle = 1400; // Base hover throttle
  float setpoint_roll = 0, setpoint_pitch = 0, setpoint_yaw = 0;
  if (Serial.available()) {
    char cmd = Serial.read();
    if (cmd == 'a') setpoint_roll = 15;
    if (cmd == 'd') setpoint_roll = -15;
    if (cmd == 'w') setpoint_pitch = 15;
    if (cmd == 's') setpoint_pitch = -15;
  }

  // 4. PID Calculations
  pid_error_roll = setpoint_roll - angle_roll;
  pid_i_mem_roll += Ki_roll * pid_error_roll;
  pid_i_mem_roll = constrain(pid_i_mem_roll, -400, 400);
  pid_output_roll = Kp_roll * pid_error_roll + pid_i_mem_roll + Kd_roll * (gyro_x_dps);

  pid_error_pitch = setpoint_pitch - angle_pitch;
  pid_i_mem_pitch += Ki_pitch * pid_error_pitch;
  pid_i_mem_pitch = constrain(pid_i_mem_pitch, -400, 400);
  pid_output_pitch = Kp_pitch * pid_error_pitch + pid_i_mem_pitch + Kd_pitch * (gyro_y_dps);

  pid_error_yaw = setpoint_yaw - angle_yaw;
  pid_i_mem_yaw += Ki_yaw * pid_error_yaw;
  pid_output_yaw = Kp_yaw * pid_error_yaw + pid_i_mem_yaw;

  // 5. Motor Mixing (X-Configuration)
  esc_pulse_fr = throttle - pid_output_pitch + pid_output_roll - pid_output_yaw;
  esc_pulse_fl = throttle - pid_output_pitch - pid_output_roll + pid_output_yaw;
  esc_pulse_rr = throttle + pid_output_pitch - pid_output_roll + pid_output_yaw;
  esc_pulse_rl = throttle + pid_output_pitch + pid_output_roll - pid_output_yaw;

  // Constrain and write to ESCs
  escFR.writeMicroseconds(constrain(esc_pulse_fr, 1000, 2000));
  escFL.writeMicroseconds(constrain(esc_pulse_fl, 1000, 2000));
  escRR.writeMicroseconds(constrain(esc_pulse_rr, 1000, 2000));
  escRL.writeMicroseconds(constrain(esc_pulse_rl, 1000, 2000));

  // Maintain 250Hz loop frequency
  while(micros() - loop_timer < 4000);
  loop_timer = micros();
}

void setMotors(int pulse) {
  escFR.writeMicroseconds(pulse);
  escFL.writeMicroseconds(pulse);
  escRR.writeMicroseconds(pulse);
  escRL.writeMicroseconds(pulse);
}

Debugging: First Three Things to Check When It Fails

Flight controllers fail in spectacular ways. Before you strap a LiPo to the frame and risk your workbench, run through this diagnostic sequence.

1. The I2C Bus Lockup

Exact Error String: ERR: I2C NACK on address (Code 2) or WHO_AM_I register returned 0x00

Ranked Causes:

1. Missing Pull-up Resistors: The GY-521 breakout has 4.7k pull-ups, but if you are using a bare MPU6050 chip or a defective clone board, the I2C lines will float. Add 4.7k resistors from SDA/SCL to 3.3V.
2. AD0 Pin Floating: The I2C address is determined by the AD0 pin. If it is floating, it will bounce between 0x68 and 0x69. Solder AD0 directly to GND to lock it to 0x68.
3. Capacitance Overload: If your I2C wires are longer than 10cm, the bus capacitance exceeds the MPU6050's drive strength. Shorten the wires or drop the I2C clock speed from 400kHz to 100kHz by changing TWBR = 12; to TWBR = 72;.

2. Motor Output Jitter and Twitching

Symptom: Motors randomly twitch or beep erratically while the Arduino is powered, even when the IMU is perfectly flat.

Ranked Causes:

1. Servo.h Timer Clash: The standard Servo.h library uses Timer1, which can occasionally interrupt the I2C hardware buffer on the ATmega328P if the PID loop runs too fast. Switch to the ServoTimer2 library or write direct OCR register manipulation for hardware PWM.
2. Vibration Aliasing: The accelerometer is picking up 100Hz+ motor harmonics. Check your foam dampening. If the raw accel Z-axis reads anything other than 16384 (+/- 200) at idle, your mount is too rigid.
3. Brownouts: The ESC BEC is failing to supply steady 5V to the Nano when the receiver or servos draw current. Power the Nano from a dedicated 5V UBEC, not the ESC's red wire.

3. The "Toilet Bowl" Effect (Circular Drift)

Symptom: The quadcopter takes off, but immediately begins flying in expanding circles.

Ranked Causes:

1. Incorrect Motor Mixing: You have a CW motor on a CCW pad, or your PID roll/pitch signs are inverted. Verify propeller directions and motor spin directions match the X-configuration mixing math in the code.
2. Untuned D-Gain: The derivative term (Kd) is reacting to high-frequency noise. Drop Kd_roll and Kd_pitch to 0.0, get the craft hovering on P and I alone, then slowly increase D until oscillations stop.

Extending and Simplifying the Build

If this is your first embedded control loop, a 4-axis quadcopter might be overwhelming. To simplify: Strip the code down to a 1-axis self-balancing robot or a single-rotor helicopter tail gyro. Comment out the pitch and yaw mixing, lock two motors, and tune the Roll PID on a fixed pivot rig. This isolates the math and keeps your hardware intact.

To extend: Once you have stable rate-mode flight, the next logical step is adding altitude hold. Wire an MS5611 barometric pressure sensor to the I2C bus (address 0x77). Implement a cascaded PID loop where the outer loop reads the barometer's Z-velocity and outputs a throttle offset, while the inner loop handles the angular stabilization. For advanced navigation, look into integrating a proven PID tuning framework and adding a UBlox M10 GPS via UART for position hold.

Arduino Flight Controller FAQ