Arduino + MPU6050 (Custom Flight Controller) Code

If you’re interested in building your own flight controller using Arduino + MPU6050, you’ll get deep insight into:

• Sensor Fusion
• PID Control
• Motor Thrust Management
• Receiver Signal Decoding

Here’s everything you’ll need, along with the code structure, libraries, and components.

---

🔩 Components Required for Arduino-Based Flight Controller

Component	Purpose
🧠 Arduino Nano / Uno / Mega	Brain of the system
📦 MPU6050 (Gyro + Accelerometer)	Senses orientation
📶 RC Receiver (6CH)	Gets commands from the transmitter
🌀 ESCs (4x)	Controls brushless motors
⚡ Brushless Motors (4x)	Main propulsion
🔋 LiPo Battery	Powers everything
🔌 Power Distribution Board	Distributes power
🔌 Servo Connectors / Dupont Wires	Connections between modules

---

🧠 Code Overview: What Will You Program?

To make the drone fly, you need to program these core functionalities in Arduino:

✅ 1. Read Data from MPU6050

• Use I2C to get gyro + accelerometer values
• Use Kalman filter or Complementary filter for sensor fusion

✅ 2. Decode RC Inputs

• Read PWM signals (throttle, pitch, roll, yaw)
• You can use pulseIn() or interrupts

✅ 3. PID Control

• Calculate errors between desired and current angle
• Adjust motor speed accordingly
• Use PID_v1.h or manual PID implementation

✅ 4. Generate PWM for ESCs

• Send updated throttle values to ESCs (1000–2000 microseconds)
• Use Servo.h or direct PWM with analogWrite() (but better precision with Servo)

---

📚 Libraries Required

Install these via Arduino Library Manager or GitHub:

1. MPU6050 by Jeff Rowberg
   #include <Wire.h>
#include <MPU6050.h>
2. PID_v1.h
   #include <PID_v1.h>
3. Servo.h (for ESC control)
   #include <Servo.h>
4. (Optional) Kalman Filter Library or Complementary Filter

Prerequisites

1. Hardware:
   • Arduino Nano / Uno
   • MPU6050 sensor (GY-521 module)
   • 4x ESC + Brushless Motors
   • RC Transmitter & Receiver (6CH)
   • LiPo battery (3S/4S)
2. Libraries to install (via Library Manager):
   • MPU6050 by Jeff Rowberg
   • I2Cdevlib (for MPU6050)
   • Servo.h (built-in)
   • PID_v1.h

---

🧠 Full Arduino Code for DIY Drone (Flight Controller)

cppCopyEdit#include <Wire.h>
#include <Servo.h>
#include <PID_v1.h>
#include "I2Cdev.h"
#include "MPU6050.h"

MPU6050 mpu;

Servo escFL, escFR, escBL, escBR; // ESCs for 4 motors

// PID variables
double inputRoll, outputRoll, setpointRoll = 0;
double inputPitch, outputPitch, setpointPitch = 0;

PID pidRoll(&inputRoll, &outputRoll, &setpointRoll, 1.5, 0.02, 1.0, DIRECT);
PID pidPitch(&inputPitch, &outputPitch, &setpointPitch, 1.5, 0.02, 1.0, DIRECT);

// Raw values
int16_t ax, ay, az, gx, gy, gz;

// RC Input Pins (adjust as per your receiver pins)
const int throttlePin = 2;
const int rollPin = 3;
const int pitchPin = 4;
const int yawPin = 5;

// RC Values
int throttle, rollInput, pitchInput, yawInput;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.initialize();

  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed");
    while (1);
  }

  // Attach ESCs to PWM pins
  escFL.attach(6);  // Front Left
  escFR.attach(9);  // Front Right
  escBL.attach(10); // Back Left
  escBR.attach(11); // Back Right

  // PID setup
  pidRoll.SetMode(AUTOMATIC);
  pidPitch.SetMode(AUTOMATIC);

  // Calibrate MPU6050
  delay(1000);
}

void loop() {
  // === Read RC Inputs ===
  throttle = pulseIn(throttlePin, HIGH, 25000); // 1000–2000 us
  rollInput = pulseIn(rollPin, HIGH, 25000);
  pitchInput = pulseIn(pitchPin, HIGH, 25000);
  yawInput = pulseIn(yawPin, HIGH, 25000);

  // === Read MPU6050 ===
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Convert raw values to angles (very basic method)
  inputRoll = atan2(ay, az) * 180 / PI;
  inputPitch = atan2(ax, az) * 180 / PI;

  // === Map RC Inputs to Setpoints ===
  setpointRoll = map(rollInput, 1000, 2000, -10, 10);  // ±10° target
  setpointPitch = map(pitchInput, 1000, 2000, -10, 10);

  // === PID Calculations ===
  pidRoll.Compute();
  pidPitch.Compute();

  // === Mix PID output with Throttle ===
  int baseThrottle = constrain(throttle, 1000, 2000);

  int motorFL = baseThrottle + outputPitch + outputRoll;
  int motorFR = baseThrottle + outputPitch - outputRoll;
  int motorBL = baseThrottle - outputPitch + outputRoll;
  int motorBR = baseThrottle - outputPitch - outputRoll;

  // Limit to ESC range
  motorFL = constrain(motorFL, 1000, 2000);
  motorFR = constrain(motorFR, 1000, 2000);
  motorBL = constrain(motorBL, 1000, 2000);
  motorBR = constrain(motorBR, 1000, 2000);

  // === Write to ESCs ===
  escFL.writeMicroseconds(motorFL);
  escFR.writeMicroseconds(motorFR);
  escBL.writeMicroseconds(motorBL);
  escBR.writeMicroseconds(motorBR);

  delay(10); // stabilize loop
}

---

🔌 RC Pin Notes

You must connect the RC receiver’s channels to Arduino pins:

• Throttle → Pin 2
• Roll (Aileron) → Pin 3
• Pitch (Elevator) → Pin 4
• Yaw (Rudder) → Pin 5

Make sure receiver is properly powered with 5V and GND.

---

⚠️ Important Tips

1. ESC Calibration: Before flying, calibrate each ESC individually or write an auto-calibration script.
2. MPU6050 Drift: This simple code doesn’t use Kalman or Complementary filters. You can improve accuracy by adding one later.
3. PID Tuning: Default values work okay but not perfectly. You’ll need to tune P, I, and D values manually.
4. Failsafe: This code has no failsafe. Add logic to cut motors if no signal.

---

🛸 Ready to Fly?

Once all 4 motors respond correctly to movement and you’ve tested motor directions:

• Install propellers (2 CW + 2 CCW)
• Power with LiPo battery
• Slowly increase throttle and test stability