Question about quadcopter serial code

Hello!

I’m making RC quadcopter with arduino and I want to control it over serial with apc220. Gyro and Serial.print and Serial.read works fine until I use this:

 if(Serial.available()) {
 char buffer[] = {' ',' '}; // Receive up to 2 bytes
 while(!Serial.available());
 Serial.readBytesUntil('\n', buffer, 2);
 int pin = atoi(buffer);
 char buffer1[] = {' ',' ',' '}; // Receive up to 3 bytes
 while(!Serial.available());
 Serial.readBytesUntil('\n', buffer1, 3);
 int pwm = atoi(buffer1);
 if(pin == 02) {
 pwm2 = pwm;
 Serial.print("2: ");
 Serial.println(pwm2);
 }
 if(pin == 03) {
 pwm3 = pwm;
 Serial.print("3: ");
 Serial.println(pwm3);
 }
 if(pin == 04) {
 pwm4 = pwm;
 Serial.print("4: ");
 Serial.println(pwm4);
 }
 if(pin == 05) {
 pwm5 = pwm;
 Serial.print("5: ");
 Serial.println(pwm5);
 }
 }

If I download that code to arduino it works fine, but if I download the whole quadcopter code with gyroscope and accellometer I can only send one time pwm value over serial to arduino.

My ready code look like this:

// MPU-6050 Accelerometer + Gyro
// -----------------------------
//
// By arduino.cc user "Krodal".
// June 2012
// Open Source / Public Domain
//
// Using Arduino 1.0.1
// It will not work with an older version,
// since Wire.endTransmission() uses a parameter
// to hold or release the I2C bus.
//
// Documentation:
// - The InvenSense documents:
// - "MPU-6000 and MPU-6050 Product Specification",
// PS-MPU-6000A.pdf
// - "MPU-6000 and MPU-6050 Register Map and Descriptions",
// RM-MPU-6000A.pdf or RS-MPU-6000A.pdf
// - "MPU-6000/MPU-6050 9-Axis Evaluation Board User Guide"
// AN-MPU-6000EVB.pdf
//
// The accuracy is 16-bits.
//
// Temperature sensor from -40 to +85 degrees Celsius
// 340 per degrees, -512 at 35 degrees.
//
// At power-up, all registers are zero, except these two:
// Register 0x6B (PWR_MGMT_2) = 0x40 (I read zero).
// Register 0x75 (WHO_AM_I) = 0x68.
//

#include <Wire.h>
// The name of the sensor is "MPU-6050".
// For program code, I omit the '-',
// therefor I use the name "MPU6050....".
// Register names according to the datasheet.
// According to the InvenSense document
// "MPU-6000 and MPU-6050 Register Map
// and Descriptions Revision 3.2", there are no registers
// at 0x02 ... 0x18, but according other information
// the registers in that unknown area are for gain
// and offsets.
//

&nbsp;

&nbsp;
#include<mpu6050.h>
// Default I2C address for the MPU-6050 is 0x68.
// But only if the AD0 pin is low.
// Some sensor boards have AD0 high, and the
// I2C address thus becomes 0x69.
#define MPU6050_I2C_ADDRESS 0x68
// Declaring an union for the registers and the axis values.
// The byte order does not match the byte order of
// the compiler and AVR chip.
// The AVR chip (on the Arduino board) has the Low Byte
// at the lower address.
// But the MPU-6050 has a different order: High Byte at
// lower address, so that has to be corrected.
// The register part "reg" is only used internally,
// and are swapped in code.
typedef union accel_t_gyro_union
{
 struct
 {
 uint8_t x_accel_h;
 uint8_t x_accel_l;
 uint8_t y_accel_h;
 uint8_t y_accel_l;
 uint8_t z_accel_h;
 uint8_t z_accel_l;
 uint8_t t_h;
 uint8_t t_l;
 uint8_t x_gyro_h;
 uint8_t x_gyro_l;
 uint8_t y_gyro_h;
 uint8_t y_gyro_l;
 uint8_t z_gyro_h;
 uint8_t z_gyro_l;
 } reg;
 struct
 {
 int x_accel;
 int y_accel;
 int z_accel;
 int temperature;
 int x_gyro;
 int y_gyro;
 int z_gyro;
 } value;
};
void setup()
{
 int error;
 uint8_t c;
 Serial.begin(9600);
 Serial.println(F("InvenSense MPU-6050"));
 Serial.println(F("June 2012"));

// Initialize the 'Wire' class for the I2C-bus.
 Wire.begin();
 // default at power-up:
 // Gyro at 250 degrees second
 // Acceleration at 2g
 // Clock source at internal 8MHz
 // The device is in sleep mode.
 //

error = MPU6050_read (MPU6050_WHO_AM_I, &c, 1);
 Serial.print(F("WHO_AM_I : "));
 Serial.print(c,HEX);
 Serial.print(F(", error = "));
 Serial.println(error,DEC);

// According to the datasheet, the 'sleep' bit
 // should read a '1'. But I read a '0'.
 // That bit has to be cleared, since the sensor
 // is in sleep mode at power-up. Even if the
 // bit reads '0'.
 error = MPU6050_read (MPU6050_PWR_MGMT_2, &c, 1);
 Serial.print(F("PWR_MGMT_2 : "));
 Serial.print(c,HEX);
 Serial.print(F(", error = "));
 Serial.println(error,DEC);
 // Clear the 'sleep' bit to start the sensor.
 MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0);

}

int speedo21 = 255;
int speedo31 = 255;
int speedo41 = 255;
int speedo51 = 255;

int speedo22 = 255;
int speedo32 = 255;
int speedo42 = 255;
int speedo52 = 255;

int readyspeedo2;
int readyspeedo3;
int readyspeedo4;
int readyspeedo5;

int pwm2 = 255;
int pwm3 = 255;
int pwm4 = 255;
int pwm5 = 255;

boolean serial = false;

void loop()
{
 int error;
 double dT;
 accel_t_gyro_union accel_t_gyro;
 // Read the raw values.
 // Read 14 bytes at once,
 // containing acceleration, temperature and gyro.
 // With the default settings of the MPU-6050,
 // there is no filter enabled, and the values
 // are not very stable.
 error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro, sizeof(accel_t_gyro));

// Swap all high and low bytes.
 // After this, the registers values are swapped,
 // so the structure name like x_accel_l does no
 // longer contain the lower byte.
 uint8_t swap;
 #define SWAP(x,y) swap = x; x = y; y = swap

SWAP (accel_t_gyro.reg.x_accel_h, accel_t_gyro.reg.x_accel_l);
 SWAP (accel_t_gyro.reg.y_accel_h, accel_t_gyro.reg.y_accel_l);
 SWAP (accel_t_gyro.reg.z_accel_h, accel_t_gyro.reg.z_accel_l);
 SWAP (accel_t_gyro.reg.t_h, accel_t_gyro.reg.t_l);
 SWAP (accel_t_gyro.reg.x_gyro_h, accel_t_gyro.reg.x_gyro_l);
 SWAP (accel_t_gyro.reg.y_gyro_h, accel_t_gyro.reg.y_gyro_l);
 SWAP (accel_t_gyro.reg.z_gyro_h, accel_t_gyro.reg.z_gyro_l);

if(Serial.available()) {
 char buffer[] = {' ',' '}; // Receive up to 2 bytes
 while(!Serial.available());
 Serial.readBytesUntil('\n', buffer, 2);
 int pin = atoi(buffer);
 char buffer1[] = {' ',' ',' '}; // Receive up to 3 bytes
 while(!Serial.available());
 Serial.readBytesUntil('\n', buffer1, 3);
 int pwm = atoi(buffer1);
 if(pin == 02) {
 pwm2 = pwm;
 Serial.print("2: ");
 Serial.println(pwm2);
 }
 if(pin == 03) {
 pwm3 = pwm;
 Serial.print("3: ");
 Serial.println(pwm3);
 }
 if(pin == 04) {
 pwm4 = pwm;
 Serial.print("4: ");
 Serial.println(pwm4);
 }
 if(pin == 05) {
 pwm5 = pwm;
 Serial.print("5: ");
 Serial.println(pwm5);
 }
 }

if(accel_t_gyro.value.x_accel >= 1000) {
 int speedo = accel_t_gyro.value.x_accel;
 if(accel_t_gyro.value.x_accel < 9000) {
 speedo = map(speedo, 10000, 1000, 0, 255);

 speedo21 = speedo;
 speedo31 = speedo;
 }
 else {
 speedo21 = 0;
 speedo31 = 0;
 }
 }
 else if (accel_t_gyro.value.x_accel <= -1000) {
 int speedo = accel_t_gyro.value.x_accel;
 if(accel_t_gyro.value.x_accel > -9000) {
 speedo = map(speedo, -10000, -1000, 0, 255);
 speedo41 = speedo;
 speedo51 = speedo;
 }
 else {
 speedo41 = 0;
 speedo51 = 0;
 }
 }
 else {
 speedo21 = 255;
 speedo31 = 255;
 speedo41 = 255;
 speedo51 = 255;
 }
 if(accel_t_gyro.value.y_accel >= 1000) {
 int speedo = accel_t_gyro.value.y_accel;
 if(accel_t_gyro.value.y_accel < 9000) {
 speedo = map(speedo, 10000, 1000, 0, 255);
 speedo42 = speedo;
 speedo22 = speedo;
 }
 else {
 speedo42 = 0;
 speedo22 = 0;
 }
 }
 else if (accel_t_gyro.value.y_accel <= -1000) {
 int speedo = accel_t_gyro.value.y_accel;
 if(accel_t_gyro.value.y_accel > -9000) {
 speedo = map(speedo, -10000, -1000, 0, 255);
 speedo32 = speedo;
 speedo52 = speedo;
}
else {
 speedo32 = 0;
 speedo52 = 0;
}
 }
 else {
 speedo22 = 255;
 speedo32 = 255;
 speedo42 = 255;
 speedo52 = 255;
 }
 if(serial == true) {
 Serial.println("Speedo21: ");
 Serial.println(speedo21);
 Serial.println("Speedo22: ");
 Serial.println(speedo22);
 Serial.println("Speedo31: ");
 Serial.println(speedo31);
 Serial.println("Speedo32: ");
 Serial.println(speedo32);
 Serial.println("Speedo41: ");
 Serial.println(speedo41);
 Serial.println("Speedo42: ");
 Serial.println(speedo42);
 Serial.println("Speedo51: ");
 Serial.println(speedo51);
 Serial.println("Speedo52: ");
 Serial.println(speedo52);
 }

 readyspeedo2 = (pwm2 - (255 - ((speedo21 + speedo22) / 2)));
 readyspeedo3 = (pwm3 - (255 - ((speedo31 + speedo32) / 2)));
 readyspeedo4 = (pwm4 - (255 - ((speedo41 + speedo42) / 2)));
 readyspeedo5 = (pwm5 - (255 - ((speedo51 + speedo52) / 2)));

 if(serial == true) {
 Serial.print("Readyspeedo2: ");
 Serial.println(readyspeedo2);
 Serial.print("Readyspeedo3: ");
 Serial.println(readyspeedo3);
 Serial.print("Readyspeedo4: ");
 Serial.println(readyspeedo4);
 Serial.print("Readyspeedo5: ");
 Serial.println(readyspeedo5);
 }

 if(readyspeedo2 >= 0) {
 analogWrite(2, readyspeedo2);
 }
 else {
 analogWrite(2, 0);
 }
 if(readyspeedo3 >= 0) {
 analogWrite(3, readyspeedo3);
 }
 else {
 analogWrite(3, 0);
 }
 if(readyspeedo4 >= 0) {
 analogWrite(4, readyspeedo4);
 }
 else {
 analogWrite(4, 0);
 }
 if(readyspeedo5 >= 0) {
 analogWrite(5, readyspeedo5);
 }
 else {
 analogWrite(5, 0);
 }
delay(10);
}
// --------------------------------------------------------
// MPU6050_read
//
// This is a common function to read multiple bytes
// from an I2C device.
//
// It uses the boolean parameter for Wire.endTransMission()
// to be able to hold or release the I2C-bus.
// This is implemented in Arduino 1.0.1.
//
// Only this function is used to read.
// There is no function for a single byte.
//
int MPU6050_read(int start, uint8_t *buffer, int size)
{
 int i, n, error;

Wire.beginTransmission(MPU6050_I2C_ADDRESS);
 n = Wire.write(start);
 if (n != 1)
 return (-10);

n = Wire.endTransmission(false); // hold the I2C-bus
 if (n != 0)
 return (n);

// Third parameter is true: relase I2C-bus after data is read.
 Wire.requestFrom(MPU6050_I2C_ADDRESS, size, true);
 i = 0;
 while(Wire.available() && i<size)
 {
 buffer[i++]=Wire.read();
 }
 if ( i != size)
 return (-11);

return (0); // return : no error
}
// --------------------------------------------------------
// MPU6050_write
//
// This is a common function to write multiple bytes to an I2C device.
//
// If only a single register is written,
// use the function MPU_6050_write_reg().
//
// Parameters:
// start : Start address, use a define for the register
// pData : A pointer to the data to write.
// size : The number of bytes to write.
//
// If only a single register is written, a pointer
// to the data has to be used, and the size is
// a single byte:
// int data = 0; // the data to write
// MPU6050_write (MPU6050_PWR_MGMT_1, &c, 1);
//
int MPU6050_write(int start, const uint8_t *pData, int size)
{
 int n, error;

Wire.beginTransmission(MPU6050_I2C_ADDRESS);
 n = Wire.write(start); // write the start address
 if (n != 1)
 return (-20);

n = Wire.write(pData, size); // write data bytes
 if (n != size)
 return (-21);

error = Wire.endTransmission(true); // release the I2C-bus
 if (error != 0)
 return (error);

return (0); // return : no error
}

// --------------------------------------------------------
// MPU6050_write_reg
//
// An extra function to write a single register.
// It is just a wrapper around the MPU_6050_write()
// function, and it is only a convenient function
// to make it easier to write a single register.
//
int MPU6050_write_reg(int reg, uint8_t data)
{
 int error;

error = MPU6050_write(reg, &data, 1);

return (error);
}

So why doesn’t that code work fine?
But this code works:

// MPU-6050 Accelerometer + Gyro
// -----------------------------
//
// By arduino.cc user "Krodal".
// June 2012
// Open Source / Public Domain
//
// Using Arduino 1.0.1
// It will not work with an older version,
// since Wire.endTransmission() uses a parameter
// to hold or release the I2C bus.
//
// Documentation:
// - The InvenSense documents:
// - "MPU-6000 and MPU-6050 Product Specification",
// PS-MPU-6000A.pdf
// - "MPU-6000 and MPU-6050 Register Map and Descriptions",
// RM-MPU-6000A.pdf or RS-MPU-6000A.pdf
// - "MPU-6000/MPU-6050 9-Axis Evaluation Board User Guide"
// AN-MPU-6000EVB.pdf
//
// The accuracy is 16-bits.
//
// Temperature sensor from -40 to +85 degrees Celsius
// 340 per degrees, -512 at 35 degrees.
//
// At power-up, all registers are zero, except these two:
// Register 0x6B (PWR_MGMT_2) = 0x40 (I read zero).
// Register 0x75 (WHO_AM_I) = 0x68.
//

#include <Wire.h>
// The name of the sensor is "MPU-6050".
// For program code, I omit the '-',
// therefor I use the name "MPU6050....".
// Register names according to the datasheet.
// According to the InvenSense document
// "MPU-6000 and MPU-6050 Register Map
// and Descriptions Revision 3.2", there are no registers
// at 0x02 ... 0x18, but according other information
// the registers in that unknown area are for gain
// and offsets.
//

&nbsp;

&nbsp;
#include<mpu6050.h>
// Default I2C address for the MPU-6050 is 0x68.
// But only if the AD0 pin is low.
// Some sensor boards have AD0 high, and the
// I2C address thus becomes 0x69.
#define MPU6050_I2C_ADDRESS 0x68
// Declaring an union for the registers and the axis values.
// The byte order does not match the byte order of
// the compiler and AVR chip.
// The AVR chip (on the Arduino board) has the Low Byte
// at the lower address.
// But the MPU-6050 has a different order: High Byte at
// lower address, so that has to be corrected.
// The register part "reg" is only used internally,
// and are swapped in code.
typedef union accel_t_gyro_union
{
 struct
 {
 uint8_t x_accel_h;
 uint8_t x_accel_l;
 uint8_t y_accel_h;
 uint8_t y_accel_l;
 uint8_t z_accel_h;
 uint8_t z_accel_l;
 uint8_t t_h;
 uint8_t t_l;
 uint8_t x_gyro_h;
 uint8_t x_gyro_l;
 uint8_t y_gyro_h;
 uint8_t y_gyro_l;
 uint8_t z_gyro_h;
 uint8_t z_gyro_l;
 } reg;
 struct
 {
 int x_accel;
 int y_accel;
 int z_accel;
 int temperature;
 int x_gyro;
 int y_gyro;
 int z_gyro;
 } value;
};
void setup()
{
 int error;
 uint8_t c;
 Serial.begin(9600);
 Serial.println(F("InvenSense MPU-6050"));
 Serial.println(F("June 2012"));

// Initialize the 'Wire' class for the I2C-bus.
 Wire.begin();
 // default at power-up:
 // Gyro at 250 degrees second
 // Acceleration at 2g
 // Clock source at internal 8MHz
 // The device is in sleep mode.
 //

error = MPU6050_read (MPU6050_WHO_AM_I, &c, 1);
 Serial.print(F("WHO_AM_I : "));
 Serial.print(c,HEX);
 Serial.print(F(", error = "));
 Serial.println(error,DEC);

// According to the datasheet, the 'sleep' bit
 // should read a '1'. But I read a '0'.
 // That bit has to be cleared, since the sensor
 // is in sleep mode at power-up. Even if the
 // bit reads '0'.
 error = MPU6050_read (MPU6050_PWR_MGMT_2, &c, 1);
 Serial.print(F("PWR_MGMT_2 : "));
 Serial.print(c,HEX);
 Serial.print(F(", error = "));
 Serial.println(error,DEC);
 // Clear the 'sleep' bit to start the sensor.
 MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0);

}

int speedo21 = 255;
int speedo31 = 255;
int speedo41 = 255;
int speedo51 = 255;

int speedo22 = 255;
int speedo32 = 255;
int speedo42 = 255;
int speedo52 = 255;

int readyspeedo2;
int readyspeedo3;
int readyspeedo4;
int readyspeedo5;

int pwm2 = 255;
int pwm3 = 255;
int pwm4 = 255;
int pwm5 = 255;

int speedo;

boolean serial = false;

int input;

void loop()
{
 int error;
 double dT;
 accel_t_gyro_union accel_t_gyro;
 // Read the raw values.
 // Read 14 bytes at once,
 // containing acceleration, temperature and gyro.
 // With the default settings of the MPU-6050,
 // there is no filter enabled, and the values
 // are not very stable.
 error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro, sizeof(accel_t_gyro));

// Swap all high and low bytes.
 // After this, the registers values are swapped,
 // so the structure name like x_accel_l does no
 // longer contain the lower byte.
 uint8_t swap;
 #define SWAP(x,y) swap = x; x = y; y = swap

SWAP (accel_t_gyro.reg.x_accel_h, accel_t_gyro.reg.x_accel_l);
 SWAP (accel_t_gyro.reg.y_accel_h, accel_t_gyro.reg.y_accel_l);
 SWAP (accel_t_gyro.reg.z_accel_h, accel_t_gyro.reg.z_accel_l);
 SWAP (accel_t_gyro.reg.t_h, accel_t_gyro.reg.t_l);
 SWAP (accel_t_gyro.reg.x_gyro_h, accel_t_gyro.reg.x_gyro_l);
 SWAP (accel_t_gyro.reg.y_gyro_h, accel_t_gyro.reg.y_gyro_l);
 SWAP (accel_t_gyro.reg.z_gyro_h, accel_t_gyro.reg.z_gyro_l);

input = Serial.read();
if(input == '0') {
 pwm2 = pwm2 + 1;
 pwm3 = pwm3 + 1;
 pwm4 = pwm4 + 1;
 pwm5 = pwm5 + 1;
}
if(input == '1') {
 pwm2 = pwm2 - 1;
 pwm3 = pwm3 - 1;
 pwm4 = pwm4 - 1;
 pwm5 = pwm5 - 1;
}
if(input == '2') {
 pwm2 = pwm2 - 1;
}
if(input == '3') {
 pwm3 = pwm3 - 1;
}
if(input == '4') {
 pwm4 = pwm4 - 1;
}
if(input == '5') {
 pwm5 = pwm5 - 1;
}
if(input == '6') {
 pwm2 = pwm2 + 1;
}
if(input == '7') {
 pwm3 = pwm3 + 1;
}
if(input == '8') {
 pwm4 = pwm4 + 1;
}
if(input == '9') {
 pwm5 = pwm5 + 1;
}

if(accel_t_gyro.value.x_accel >= 1000) {
 speedo = accel_t_gyro.value.x_accel;
 if(accel_t_gyro.value.x_accel < 9000) {
 speedo = map(speedo, 10000, 1000, 0, 255);

 speedo21 = speedo;
 speedo31 = speedo;
 }
 else {
 speedo21 = 0;
 speedo31 = 0;
 }
 }
 else if (accel_t_gyro.value.x_accel <= -1000) {
 speedo = accel_t_gyro.value.x_accel;
 if(accel_t_gyro.value.x_accel > -9000) {
 speedo = map(speedo, -10000, -1000, 0, 255);
 speedo41 = speedo;
 speedo51 = speedo;
 }
 else {
 speedo41 = 0;
 speedo51 = 0;
 }
 }
 else {
 speedo21 = 255;
 speedo31 = 255;
 speedo41 = 255;
 speedo51 = 255;
 }
 if(accel_t_gyro.value.y_accel >= 1000) {
 speedo = accel_t_gyro.value.y_accel;
 if(accel_t_gyro.value.y_accel < 9000) {
 speedo = map(speedo, 10000, 1000, 0, 255);
 speedo42 = speedo;
 speedo22 = speedo;
 }
 else {
 speedo42 = 0;
 speedo22 = 0;
 }
 }
 else if (accel_t_gyro.value.y_accel <= -1000) {
 speedo = accel_t_gyro.value.y_accel;
 if(accel_t_gyro.value.y_accel > -9000) {
 speedo = map(speedo, -10000, -1000, 0, 255);
 speedo32 = speedo;
 speedo52 = speedo;
}
else {
 speedo32 = 0;
 speedo52 = 0;
}
 }
 else {
 speedo22 = 255;
 speedo32 = 255;
 speedo42 = 255;
 speedo52 = 255;
 }

 readyspeedo2 = (pwm2 - (255 - ((speedo21 + speedo22) / 2)));
 readyspeedo3 = (pwm3 - (255 - ((speedo31 + speedo32) / 2)));
 readyspeedo4 = (pwm4 - (255 - ((speedo41 + speedo42) / 2)));
 readyspeedo5 = (pwm5 - (255 - ((speedo51 + speedo52) / 2)));

 if(readyspeedo2 >= 0) {
 analogWrite(2, readyspeedo2);
 }
 else {
 analogWrite(2, 0);
 }
 if(readyspeedo3 >= 0) {
 analogWrite(3, readyspeedo3);
 }
 else {
 analogWrite(3, 0);
 }
 if(readyspeedo4 >= 0) {
 analogWrite(4, readyspeedo4);
 }
 else {
 analogWrite(4, 0);
 }
 if(readyspeedo5 >= 0) {
 analogWrite(5, readyspeedo5);
 }
 else {
 analogWrite(5, 0);
 }

 Serial.print("Readyspeedo2: ");
 Serial.println(readyspeedo2);
 Serial.print("Readyspeedo3: ");
 Serial.println(readyspeedo3);
 Serial.print("Readyspeedo4: ");
 Serial.println(readyspeedo4);
 Serial.print("Readyspeedo5: ");
 Serial.println(readyspeedo5);

delay(10);
}
// --------------------------------------------------------
// MPU6050_read
//
// This is a common function to read multiple bytes
// from an I2C device.
//
// It uses the boolean parameter for Wire.endTransMission()
// to be able to hold or release the I2C-bus.
// This is implemented in Arduino 1.0.1.
//
// Only this function is used to read.
// There is no function for a single byte.
//
int MPU6050_read(int start, uint8_t *buffer, int size)
{
 int i, n, error;

Wire.beginTransmission(MPU6050_I2C_ADDRESS);
 n = Wire.write(start);
 if (n != 1)
 return (-10);

n = Wire.endTransmission(false); // hold the I2C-bus
 if (n != 0)
 return (n);

// Third parameter is true: relase I2C-bus after data is read.
 Wire.requestFrom(MPU6050_I2C_ADDRESS, size, true);
 i = 0;
 while(Wire.available() && i<size)
 {
 buffer[i++]=Wire.read();
 }
 if ( i != size)
 return (-11);

return (0); // return : no error
}
// --------------------------------------------------------
// MPU6050_write
//
// This is a common function to write multiple bytes to an I2C device.
//
// If only a single register is written,
// use the function MPU_6050_write_reg().
//
// Parameters:
// start : Start address, use a define for the register
// pData : A pointer to the data to write.
// size : The number of bytes to write.
//
// If only a single register is written, a pointer
// to the data has to be used, and the size is
// a single byte:
// int data = 0; // the data to write
// MPU6050_write (MPU6050_PWR_MGMT_1, &c, 1);
//
int MPU6050_write(int start, const uint8_t *pData, int size)
{
 int n, error;

Wire.beginTransmission(MPU6050_I2C_ADDRESS);
 n = Wire.write(start); // write the start address
 if (n != 1)
 return (-20);

n = Wire.write(pData, size); // write data bytes
 if (n != size)
 return (-21);

error = Wire.endTransmission(true); // release the I2C-bus
 if (error != 0)
 return (error);

return (0); // return : no error
}

// --------------------------------------------------------
// MPU6050_write_reg
//
// An extra function to write a single register.
// It is just a wrapper around the MPU_6050_write()
// function, and it is only a convenient function
// to make it easier to write a single register.
//
int MPU6050_write_reg(int reg, uint8_t data)
{
 int error;

error = MPU6050_write(reg, &data, 1);

return (error);
}

Thanks for help!