make again for Arduino uno

/*
* Arduino Uno Hexacopter Flight Controller with Self-Leveling
* Components: Arduino Uno, MPU6050, 6x A2212 2200KV Motors, 6x 30A ESCs
* Flysky FS-i6 + FS-iA6B, Ublox Neo M8N GPS
* Features: Self-leveling mode activated by AUX1 switch
*
* READY TO USE - COPY PASTE CODE FOR ARDUINO UNO
* Upload to Arduino Uno and follow setup instructions
*/

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

// ===== PIN DEFINITIONS =====

// Motor ESC Pins (PWM Output)
#define MOTOR_1_PIN 3 // Front Motor
#define MOTOR_2_PIN 5 // Front Right Motor
#define MOTOR_3_PIN 6 // Rear Right Motor
#define MOTOR_4_PIN 9 // Rear Motor
#define MOTOR_5_PIN 10 // Rear Left Motor
#define MOTOR_6_PIN 11 // Front Left Motor

// Receiver Pins (PWM Input)

#define THROTTLE_PIN 2 // Channel 1 - Throttle
#define ROLL_PIN 4 // Channel 2 - Roll
#define PITCH_PIN 7 // Channel 3 - Pitch
#define YAW_PIN 8 // Channel 4 - Yaw
#define AUX1_PIN 12 // Channel 5 - Self-leveling switch
#define AUX2_PIN 13 // Channel 6 - GPS mode switch

// GPS Pins
#define GPS_RX_PIN A0
#define GPS_TX_PIN A1

// ===== MPU6050 CONFIGURATION =====

const int MPU6050_ADDR = 0x68;
float gyro_x, gyro_y, gyro_z;
float acc_x, acc_y, acc_z;
float angle_roll = 0, angle_pitch = 0, angle_yaw = 0;
float gyro_roll_input = 0, gyro_pitch_input = 0, gyro_yaw_input = 0;
float angle_roll_acc, angle_pitch_acc;
float acc_total_vector;
int temperature;

// Calibration values
float gyro_x_cal = 0, gyro_y_cal = 0, gyro_z_cal = 0;
// ===== ESC OBJECTS =====
Servo esc1, esc2, esc3, esc4, esc5, esc6;

// ===== RECEIVER VARIABLES =====

unsigned long receiver_input[7] = {0, 1500, 1500, 1000, 1500, 1000, 1000};
unsigned long timer_1, timer_2, timer_3, timer_4, timer_5, timer_6;
unsigned long current_time, loop_timer;
byte last_channel_1, last_channel_2, last_channel_3, last_channel_4, last_channel_5, last

int throttle = 1000;

int receiver_roll = 1500;
int receiver_pitch = 1500;
int receiver_yaw = 1500;
int receiver_aux1 = 1000;
int receiver_aux2 = 1000;

// ===== FLIGHT MODE VARIABLES =====

boolean self_leveling_mode = false;
boolean motors_armed = false;
int serial_counter = 0;

// ===== PID VARIABLES =====

float pid_error_temp;
float pid_i_mem_roll = 0, pid_roll_setpoint = 0, pid_output_roll = 0, pid_last_roll_d_err
float pid_p_gain_roll = 1.5;
float pid_i_gain_roll = 0.02;
float pid_d_gain_roll = 15.0;
int pid_max_roll = 400;

float pid_i_mem_pitch = 0, pid_pitch_setpoint = 0, pid_output_pitch = 0, pid_last_pitch_d

float pid_p_gain_pitch = 1.5;
float pid_i_gain_pitch = 0.02;
float pid_d_gain_pitch = 15.0;
int pid_max_pitch = 400;

float pid_i_mem_yaw = 0, pid_yaw_setpoint = 0, pid_output_yaw = 0, pid_last_yaw_d_error =

float pid_p_gain_yaw = 3.0;
float pid_i_gain_yaw = 0.01;
float pid_d_gain_yaw = 0.0;
int pid_max_yaw = 400;

// ===== MOTOR SPEED VARIABLES =====

int esc_1, esc_2, esc_3, esc_4, esc_5, esc_6;
float elapsed_time = 0.004; // 250Hz loop
unsigned long previous_time = 0;

// ===== SETUP FUNCTION =====

void setup() {
Serial.begin(57600);
Serial.println("Arduino Uno Hexacopter Flight Controller Starting...");

// Initialize I2C
Wire.begin();
TWBR = 12; // 400kHz I2C clock
// Initialize MPU6050
setup_mpu_6050_registers();

// Initialize ESCs
esc1.attach(MOTOR_1_PIN);
esc2.attach(MOTOR_2_PIN);
esc3.attach(MOTOR_3_PIN);
esc4.attach(MOTOR_4_PIN);
esc5.attach(MOTOR_5_PIN);
esc6.attach(MOTOR_6_PIN);

// Set motors to minimum

esc1.writeMicroseconds(1000);
esc2.writeMicroseconds(1000);
esc3.writeMicroseconds(1000);
esc4.writeMicroseconds(1000);
esc5.writeMicroseconds(1000);
esc6.writeMicroseconds(1000);

// Configure receiver interrupts

PCICR |= (1 << PCIE0);
PCICR |= (1 << PCIE2);
PCMSK0 |= (1 << PCINT0); // Pin 8
PCMSK0 |= (1 << PCINT4); // Pin 12
PCMSK0 |= (1 << PCINT5); // Pin 13
PCMSK2 |= (1 << PCINT18); // Pin 2
PCMSK2 |= (1 << PCINT20); // Pin 4
PCMSK2 |= (1 << PCINT23); // Pin 7

// Wait for receiver

Serial.println("Waiting for receiver signal...");
while(receiver_input[3] < 990 || receiver_input[3] > 1020) {
read_mpu_6050_data();
delay(4);
}
Serial.println("Receiver detected!");

// Calibrate gyroscope
Serial.println("Calibrating gyroscope - keep drone still...");
calibrate_gyro();
Serial.println("Gyroscope calibrated!");

// ESC Calibration
Serial.println("ESC Calibration:");
Serial.println("1. Set transmitter throttle to MAXIMUM");
Serial.println("2. Type 'max' and press Enter");

while(true) {
if(Serial.available()) {
String input = Serial.readString();
input.trim();
if(input == "max") break;
}
delay(100);
}
 // Send max throttle
for(int i = 0; i < 500; i++) {
esc1.writeMicroseconds(2000);
esc2.writeMicroseconds(2000);
esc3.writeMicroseconds(2000);
esc4.writeMicroseconds(2000);
esc5.writeMicroseconds(2000);
esc6.writeMicroseconds(2000);
delay(4);
}

Serial.println("3. Set transmitter throttle to MINIMUM");

Serial.println("4. Type 'min' and press Enter");

while(true) {
if(Serial.available()) {
String input = Serial.readString();
input.trim();
if(input == "min") break;
}
delay(100);
}

// Send min throttle

for(int i = 0; i < 500; i++) {
esc1.writeMicroseconds(1000);
esc2.writeMicroseconds(1000);
esc3.writeMicroseconds(1000);
esc4.writeMicroseconds(1000);
esc5.writeMicroseconds(1000);
esc6.writeMicroseconds(1000);
delay(4);
}

Serial.println("ESC Calibration Complete!");

Serial.println("Ready for flight!");
Serial.println("ARM: Throttle low + Yaw right");
Serial.println("DISARM: Throttle low + Yaw left");
Serial.println("Self-Level: AUX1 switch");

loop_timer = micros();
}

// ===== MAIN LOOP =====

void loop() {
// Read MPU6050
read_mpu_6050_data();

// Process gyro data

gyro_roll_input = (gyro_roll_input * 0.7) + ((gyro_x / 65.5) * 0.3);
gyro_pitch_input = (gyro_pitch_input * 0.7) + ((gyro_y / 65.5) * 0.3);
gyro_yaw_input = (gyro_yaw_input * 0.7) + ((gyro_z / 65.5) * 0.3);

// Calculate angles for self-leveling

if(self_leveling_mode) {
calculate_angles();
 }

// Update receiver values and flight modes

update_receiver();
check_flight_modes();

// Calculate PID
calculate_pid();

// Calculate motor speeds

calculate_motor_speeds();

// Update motors
update_motors();

// Debug output
if(++serial_counter >= 50) { // Every 200ms
print_debug();
serial_counter = 0;
}

// 250Hz loop timing

while(micros() - loop_timer < 4000);
loop_timer = micros();
}

// ===== MPU6050 FUNCTIONS =====

void setup_mpu_6050_registers() {
Wire.beginTransmission(MPU6050_ADDR);
Wire.write(0x6B);
Wire.write(0x00);
Wire.endTransmission();

Wire.beginTransmission(MPU6050_ADDR);
Wire.write(0x1C);
Wire.write(0x10);
Wire.endTransmission();

Wire.beginTransmission(MPU6050_ADDR);
Wire.write(0x1B);
Wire.write(0x08);
Wire.endTransmission();
}

void read_mpu_6050_data() {
Wire.beginTransmission(MPU6050_ADDR);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(MPU6050_ADDR, 14, true);

while(Wire.available() < 14);

acc_x = Wire.read() << 8 | Wire.read();
acc_y = Wire.read() << 8 | Wire.read();
acc_z = Wire.read() << 8 | Wire.read();
temperature = Wire.read() << 8 | Wire.read();
gyro_x = Wire.read() << 8 | Wire.read();
 gyro_y = Wire.read() << 8 | Wire.read();
gyro_z = Wire.read() << 8 | Wire.read();

gyro_x -= gyro_x_cal;
gyro_y -= gyro_y_cal;
gyro_z -= gyro_z_cal;
}

void calibrate_gyro() {
for(int i = 0; i < 2000; i++) {
read_mpu_6050_data();
gyro_x_cal += gyro_x;
gyro_y_cal += gyro_y;
gyro_z_cal += gyro_z;
delay(3);
}
gyro_x_cal /= 2000;
gyro_y_cal /= 2000;
gyro_z_cal /= 2000;
}

void calculate_angles() {
// Convert accelerometer
acc_x = acc_x / 4096.0;
acc_y = acc_y / 4096.0;
acc_z = acc_z / 4096.0;

// Integrate gyro
angle_pitch += gyro_pitch_input * elapsed_time;
angle_roll += gyro_roll_input * elapsed_time;

// Yaw drift compensation

angle_pitch += angle_roll * sin(gyro_z * 0.000001066);
angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);

// Calculate accelerometer angles

acc_total_vector = sqrt((acc_x * acc_x) + (acc_y * acc_y) + (acc_z * acc_z));

if(abs(acc_x) < acc_total_vector) {

angle_pitch_acc = asin((float)acc_y / acc_total_vector) * 57.296;
}
if(abs(acc_y) < acc_total_vector) {
angle_roll_acc = asin((float)acc_x / acc_total_vector) * -57.296;
}

// Complementary filter
angle_pitch = angle_pitch * 0.998 + angle_pitch_acc * 0.002;
angle_roll = angle_roll * 0.998 + angle_roll_acc * 0.002;
}

// ===== RECEIVER FUNCTIONS =====

void update_receiver() {
throttle = receiver_input[3];
receiver_roll = receiver_input[1];
receiver_pitch = receiver_input[2];
receiver_yaw = receiver_input[4];
 receiver_aux1 = receiver_input[5];
receiver_aux2 = receiver_input[6];
}

void check_flight_modes() {
// Self-leveling mode
self_leveling_mode = (receiver_aux1 > 1500);

// Motor arming
if(throttle < 1050 && receiver_yaw < 1050) {
motors_armed = false;
}
else if(throttle < 1050 && receiver_yaw > 1450) {
motors_armed = true;
}

// Calculate setpoints
if(self_leveling_mode) {
pid_roll_setpoint = 0;
pid_pitch_setpoint = 0;
pid_roll_setpoint -= (receiver_roll - 1500) / 3.0;
pid_pitch_setpoint -= (receiver_pitch - 1500) / 3.0;
} else {
pid_roll_setpoint = 0;
pid_pitch_setpoint = 0;
if(receiver_roll > 1508) pid_roll_setpoint = receiver_roll - 1508;
else if(receiver_roll < 1492) pid_roll_setpoint = receiver_roll - 1492;

if(receiver_pitch > 1508) pid_pitch_setpoint = receiver_pitch - 1508;

else if(receiver_pitch < 1492) pid_pitch_setpoint = receiver_pitch - 1492;
}

// Yaw setpoint
pid_yaw_setpoint = 0;
if(receiver_yaw > 1508) pid_yaw_setpoint = (receiver_yaw - 1508) / 3.0;
else if(receiver_yaw < 1492) pid_yaw_setpoint = (receiver_yaw - 1492) / 3.0;
}

// ===== PID CALCULATION =====

void calculate_pid() {
// Roll PID
if(self_leveling_mode) {
pid_error_temp = angle_roll - pid_roll_setpoint;
} else {
pid_error_temp = gyro_roll_input - pid_roll_setpoint;
}

pid_i_mem_roll += pid_i_gain_roll * pid_error_temp;

if(pid_i_mem_roll > pid_max_roll) pid_i_mem_roll = pid_max_roll;
else if(pid_i_mem_roll < pid_max_roll * -1) pid_i_mem_roll = pid_max_roll * -1;

pid_output_roll = pid_p_gain_roll * pid_error_temp + pid_i_mem_roll + pid_d_gain_roll *

if(pid_output_roll > pid_max_roll) pid_output_roll = pid_max_roll;
else if(pid_output_roll < pid_max_roll * -1) pid_output_roll = pid_max_roll * -1;

pid_last_roll_d_error = pid_error_temp;
 // Pitch PID
if(self_leveling_mode) {
pid_error_temp = angle_pitch - pid_pitch_setpoint;
} else {
pid_error_temp = gyro_pitch_input - pid_pitch_setpoint;
}

pid_i_mem_pitch += pid_i_gain_pitch * pid_error_temp;

if(pid_i_mem_pitch > pid_max_pitch) pid_i_mem_pitch = pid_max_pitch;
else if(pid_i_mem_pitch < pid_max_pitch * -1) pid_i_mem_pitch = pid_max_pitch * -1;

pid_output_pitch = pid_p_gain_pitch * pid_error_temp + pid_i_mem_pitch + pid_d_gain_pit

if(pid_output_pitch > pid_max_pitch) pid_output_pitch = pid_max_pitch;
else if(pid_output_pitch < pid_max_pitch * -1) pid_output_pitch = pid_max_pitch * -1;

pid_last_pitch_d_error = pid_error_temp;

// Yaw PID
pid_error_temp = gyro_yaw_input - pid_yaw_setpoint;
pid_i_mem_yaw += pid_i_gain_yaw * pid_error_temp;
if(pid_i_mem_yaw > pid_max_yaw) pid_i_mem_yaw = pid_max_yaw;
else if(pid_i_mem_yaw < pid_max_yaw * -1) pid_i_mem_yaw = pid_max_yaw * -1;

pid_output_yaw = pid_p_gain_yaw * pid_error_temp + pid_i_mem_yaw + pid_d_gain_yaw * (pi

if(pid_output_yaw > pid_max_yaw) pid_output_yaw = pid_max_yaw;
else if(pid_output_yaw < pid_max_yaw * -1) pid_output_yaw = pid_max_yaw * -1;

pid_last_yaw_d_error = pid_error_temp;
}

// ===== MOTOR MIXING =====

void calculate_motor_speeds() {
if(throttle > 1800) throttle = 1800;

// Hexacopter motor mixing (corrected)

esc_1 = throttle - pid_output_pitch + pid_output_yaw;
esc_2 = throttle - pid_output_pitch * 0.5 + pid_output_roll * 0.866 - pid_output_yaw;
esc_3 = throttle + pid_output_pitch * 0.5 + pid_output_roll * 0.866 + pid_output_yaw;
esc_4 = throttle + pid_output_pitch - pid_output_yaw;
esc_5 = throttle + pid_output_pitch * 0.5 - pid_output_roll * 0.866 + pid_output_yaw;
esc_6 = throttle - pid_output_pitch * 0.5 - pid_output_roll * 0.866 - pid_output_yaw;

// Constrain values
esc_1 = constrain(esc_1, 1100, 2000);
esc_2 = constrain(esc_2, 1100, 2000);
esc_3 = constrain(esc_3, 1100, 2000);
esc_4 = constrain(esc_4, 1100, 2000);
esc_5 = constrain(esc_5, 1100, 2000);
esc_6 = constrain(esc_6, 1100, 2000);
}

void update_motors() {
if(motors_armed && throttle > 1100) {
esc1.writeMicroseconds(esc_1);
esc2.writeMicroseconds(esc_2);
 esc3.writeMicroseconds(esc_3);
esc4.writeMicroseconds(esc_4);
esc5.writeMicroseconds(esc_5);
esc6.writeMicroseconds(esc_6);
} else {
esc1.writeMicroseconds(1000);
esc2.writeMicroseconds(1000);
esc3.writeMicroseconds(1000);
esc4.writeMicroseconds(1000);
esc5.writeMicroseconds(1000);
esc6.writeMicroseconds(1000);
}
}

// ===== INTERRUPT HANDLERS =====

ISR(PCINT2_vect) {
current_time = micros();

// Channel 1 (Throttle) - Pin 2

if(PIND & B00000100) {
if(last_channel_1 == 0) {
last_channel_1 = 1;
timer_1 = current_time;
}
}
else if(last_channel_1 == 1) {
last_channel_1 = 0;
receiver_input[3] = current_time - timer_1;
}

// Channel 2 (Roll) - Pin 4

if(PIND & B00010000) {
if(last_channel_2 == 0) {
last_channel_2 = 1;
timer_2 = current_time;
}
}
else if(last_channel_2 == 1) {
last_channel_2 = 0;
receiver_input[1] = current_time - timer_2;
}

// Channel 3 (Pitch) - Pin 7

if(PIND & B10000000) {
if(last_channel_3 == 0) {
last_channel_3 = 1;
timer_3 = current_time;
}
}
else if(last_channel_3 == 1) {
last_channel_3 = 0;
receiver_input[2] = current_time - timer_3;
}
}

ISR(PCINT0_vect) {
 current_time = micros();

// Channel 4 (Yaw) - Pin 8

if(PINB & B00000001) {
if(last_channel_4 == 0) {
last_channel_4 = 1;
timer_4 = current_time;
}
}
else if(last_channel_4 == 1) {
last_channel_4 = 0;
receiver_input[4] = current_time - timer_4;
}

// Channel 5 (AUX1) - Pin 12

if(PINB & B00010000) {
if(last_channel_5 == 0) {
last_channel_5 = 1;
timer_5 = current_time;
}
}
else if(last_channel_5 == 1) {
last_channel_5 = 0;
receiver_input[5] = current_time - timer_5;
}

// Channel 6 (AUX2) - Pin 13

if(PINB & B00100000) {
if(last_channel_6 == 0) {
last_channel_6 = 1;
timer_6 = current_time;
}
}
else if(last_channel_6 == 1) {
last_channel_6 = 0;
receiver_input[6] = current_time - timer_6;
}
}

// ===== DEBUG FUNCTION =====

void print_debug() {
Serial.print("T:"); Serial.print(throttle);
Serial.print(" R:"); Serial.print(receiver_roll);
Serial.print(" P:"); Serial.print(receiver_pitch);
Serial.print(" Y:"); Serial.print(receiver_yaw);
Serial.print(" AUX1:"); Serial.print(receiver_aux1);
Serial.print(" | SelfLevel:"); Serial.print(self_leveling_mode ? "ON" : "OFF");
Serial.print(" Armed:"); Serial.print(motors_armed ? "YES" : "NO");
if(self_leveling_mode) {
Serial.print(" | Roll:"); Serial.print(angle_roll, 1);
Serial.print(" Pitch:"); Serial.print(angle_pitch, 1);
}
Serial.println();
}
ARDUINO UNO SETUP INSTRUCTIONS:
1. Arduino IDE Setup:
Select: Tools > Board > Arduino Uno
Select: Tools > Port > [Your COM Port]
No need to select processor (automatic for Uno)
2. Upload Process:
Copy-paste this entire code into Arduino IDE
Click Upload
Follow the serial monitor instructions for ESC calibration
3. First Time Setup:
Keep propellers OFF during setup
Connect to Serial Monitor (57600 baud)
Follow ESC calibration prompts
Test receiver inputs and motor directions
4. Flight Controls:
ARM: Throttle low + Yaw stick right
DISARM: Throttle low + Yaw stick left
Self-Level: AUX1 switch up (channel 5)
Rate Mode: AUX1 switch down
5. Motor Direction Check:
Motor 1,3,5 = Clockwise (CW)
Motor 2,4,6 = Counter-clockwise (CCW)
If wrong: swap any 2 motor wires

ARDUINO UNO WIRING CONNECTIONS:

ARDUINO UNO PIN CONNECTIONS

===========================

POWER PINS:
VIN ── 5V from ESC BEC (7-12V input)
5V ── 5V Output for Receiver and GPS
GND ── Common Ground (multiple connections)

DIGITAL PINS (PWM OUTPUT TO ESCs):

D3 ── ESC 1 Signal Wire (Front Motor)
D5 ── ESC 2 Signal Wire (Front Right Motor)
D6 ── ESC 3 Signal Wire (Rear Right Motor)
D9 ── ESC 4 Signal Wire (Rear Motor)
D10 ── ESC 5 Signal Wire (Rear Left Motor)
D11 ── ESC 6 Signal Wire (Front Left Motor)
 DIGITAL PINS (PWM INPUT FROM RECEIVER):
D2 ── FS-iA6B Channel 1 (Throttle)
D4 ── FS-iA6B Channel 2 (Roll)
D7 ── FS-iA6B Channel 3 (Pitch)
D8 ── FS-iA6B Channel 4 (Yaw)
D12 ── FS-iA6B Channel 5 (AUX1 - Self Level)
D13 ── FS-iA6B Channel 6 (AUX2 - GPS Mode)

ANALOG PINS:
A0 ── Ublox Neo M8N GPS TX (Arduino RX)
A1 ── Ublox Neo M8N GPS RX (Arduino TX)
A4 ── MPU6050 SDA (I2C Data)
A5 ── MPU6050 SCL (I2C Clock)

COMPONENT CONNECTIONS:

MPU6050 IMU:

MPU6050 → Arduino Uno

VCC → 5V
GND → GND
SDA → A4
SCL → A5

FS-iA6B Receiver:

FS-iA6B → Arduino Uno

CH1 (White) → D2 (Throttle)
CH2 (White) → D4 (Roll)
CH3 (White) → D7 (Pitch)
CH4 (White) → D8 (Yaw)
CH5 (White) → D12 (AUX1)
CH6 (White) → D13 (AUX2)
VCC (Red) → 5V
GND (Black) → GND

ESC Connections:
ESC 1 Signal → D3 (Front Motor)
ESC 2 Signal → D5 (Front Right Motor)
ESC 3 Signal → D6 (Rear Right Motor)
ESC 4 Signal → D9 (Rear Motor)
ESC 5 Signal → D10 (Rear Left Motor)
ESC 6 Signal → D11 (Front Left Motor)
⚠️SAFETY: Always remove propellers during testing and setup!
This code is ready to use with Arduino Uno - just copy, paste, and upload!