Need help on APM & Arduino

Hi, I am new in using APM 2.0 and Arduino MEGA, I am currently doing a small project of making an Arduino based QuadCopter by using APM.

My target is to pass the signal from RC to Arduino and pass it to APM.
Currently i am not sure where my problem is and seeking for help.
During flight test, My drone will not take off safety. it sometimes roll right and pitch back.

Here my code for Arduino:
#include <SPI.h>
#include <Servo.h>

//set up the pins for flight controller

int ledpin = 46;

int recAIL;
int recELE;
int recTHR;
int recRUD;

int readAIL;
int readELE;
int readTHR;
int readRUD;
int readArm = 0;

Servo AIL;
Servo ELE;
Servo THR;
Servo RUD;

void setup() {
Serial.begin(9600);
Serial.println(“Program begin…”);

//attaching servos
pinMode(7, OUTPUT);
pinMode(6, OUTPUT);
pinMode(5, OUTPUT);
pinMode(3, OUTPUT);
AIL.attach(7);
ELE.attach(6);
THR.attach(5);
RUD.attach(3);

pinMode(ledpin, OUTPUT);

Calibrate();
delay(500);
Arming();
}

void loop() {
Serial.println(“Program at LOOP”);

if(readArm != 0 )
{
//reading receiver value from remote control
recAIL = pulseIn(9, HIGH, 20000);
recELE = pulseIn(10, HIGH, 20000);
recTHR = pulseIn(11, HIGH, 20000);
recRUD = pulseIn(13, HIGH, 20000);

//Pass Receiver value to APM
AIL.writeMicroseconds(recAIL);
ELE.writeMicroseconds(recELE);
THR.writeMicroseconds(recTHR);
RUD.writeMicroseconds(recRUD);

//Read the OUTPUT from APM to ESC
readAIL = pulseIn(7, HIGH, 20000);
readELE = pulseIn(6, HIGH, 20000);
readTHR = pulseIn(5, HIGH, 20000);
readRUD = pulseIn(3, HIGH, 20000);

Serial.print("AIL: ");
Serial.print(recAIL);
Serial.print(’\t’);
Serial.print(readAIL);
Serial.print(’\n’);

Serial.print("ELE: ");
Serial.print(recELE);
Serial.print(’\t’);
Serial.print(readELE);
Serial.print(’\n’);

Serial.print("THR: ");
Serial.print(recTHR);
Serial.print(’\t’);
Serial.print(readTHR);
Serial.print(’\n’);

Serial.print("RUD: ");
Serial.print(recRUD);
Serial.print(’\t’);
Serial.print(readRUD);
Serial.print(’\n’);

if(recTHR < 1000)
{
DisArm();
}
}
}

void Calibrate()
{

AIL.writeMicroseconds(1978);
ELE.writeMicroseconds(1975);
RUD.writeMicroseconds(1966);
THR.writeMicroseconds(1966);
delay(1500);
AIL.writeMicroseconds(994);
ELE.writeMicroseconds(994);
RUD.writeMicroseconds(994);
THR.writeMicroseconds(994);
delay(500);

AIL.writeMicroseconds(1486);
delay(10);
ELE.writeMicroseconds(1485);
delay(10);
RUD.writeMicroseconds(1496);
delay(10);
THR.writeMicroseconds(994);
delay(10);
}

void Arming()
{
int count;

Serial.print(“Arming… \n”);

for(count = 0; count < 50; count++)
{
recTHR = pulseIn(11, HIGH, 20000);
recRUD = pulseIn(13, HIGH, 20000);

Serial.print("RUD: ");
Serial.print(recRUD);
Serial.print(’\t’);
Serial.print("THR: ");
Serial.print(recTHR);
Serial.print(’\n’);

if(recTHR < 1000 && recRUD > 1800)
{
Serial.print("In for loop… ");
Serial.print("RUD: ");
Serial.print(recRUD);
Serial.print(’\t’);
Serial.print("THR: ");
Serial.print(recTHR);
Serial.print(’\n’);
Serial.print("count: ");
Serial.print(count);
Serial.print(’\n’);

}
else
{
count = 0;
}
}
digitalWrite(ledpin,HIGH);
readArm = 1;
}

void DisArm()
{
int count = 0;
Serial.print(“Disarming… \n”);

for(count = 0 ; count < 200; count++)
{
Serial.print("count: ");
Serial.print(count);
Serial.print(’\n’);
Serial.print("THR: ");
Serial.print(recTHR);
Serial.print(’\n’);

recTHR = pulseIn(11, HIGH, 20000);
THR.writeMicroseconds(recTHR);

if(recTHR > 1000)
{
count = 0;
loop();
}
}
digitalWrite(ledpin,LOW);
readArm = 0;
Arming();
}

Need help ;x

Start by reading the “how to post” message, and edit the post to enclose your code in code tags ("</>" button).