Power_Broker:
Looks like you're not reading the data from the FIFO fast enough. It seems that you're not using an interrupt to read the data from the MPU - you should try it if you haven't already.
Well this is the code i used now, but now it dosen't read any data coming from the mpu6050. I am posting the code until the loop. after that i didnt change anything.
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
#define OUTPUT_READABLE_YAWPITCHROLL
bool blinkState = false;
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
uint8_t teapotPacket[14] = { '
, 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0x00, 0x00, '\r', '\n' };
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
/////////////////////////////////////////////////////////////////////////
//libraries
#include <SoftwareSerial.h>
#include<Wire.h>
#include <TimerOne.h>
#include <Servo.h>//Using servo library to control ESC
//entra facility-----------------
int led = 13;
//---------------------------------
const int MPU_addr = 0x68;
double AcX, AcY, AcZ, GyX, GyY, GyZ, Tmp;
int minVal = 265;
int maxVal = 402;
double x_ang;
double y_ang;
double z_ang;
double x_rate;
double y_rate;
double z_rate;
double gxcal;
double gycal;
double gzcal;
//brushlkess motors-------------
Servo motor_fr;
Servo motor_br;
Servo motor_bl;
Servo motor_fl;
int motor_val_fr;
int motor_val_br;
int motor_val_bl;
int motor_val_fl;
int mint = 1050;
int maxt = 1900;
//transmitter input--------------
int throttle;
int pitch;
int roll;
int yaw;
int but_1;
int but_2;
String input;
int boundLow;
int boundHigh;
const char delimiter = ',';
SoftwareSerial mySerial(10, 9);
//PID-----------------------
int pid_output_pitch;
int pid_output_roll;
int pid_output_yaw = 0; //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//////////////////////////////////////////
double kp_pitch = 8; // PROPOTIONAL
double ki_pitch = 0; // INTEGRAL
double kd_pitch = 0; // DERIVATIVE
unsigned long currentTime_pitch, previousTime_pitch;
double elapsedTime_pitch;
double error_pitch;
double lastError_pitch;
double inputp_pitch, outputp_pitch, setPoint_pitch;
double cumError_pitch, rateError_pitch;
int Setpoint_pitch;
double kp_roll = 8; // PROPOTIONAL
double ki_roll = 0; // INTEGRAL
double kd_roll = 0; // DERIVATIVE
unsigned long currentTime_roll, previousTime_roll;
double elapsedTime_roll;
double error_roll;
double lastError_roll;
double inputp_roll, outputp_roll, setPoint_roll;
double cumError_roll, rateError_roll;
int Setpoint_roll;
////////////////////////////////////////////////////
/////////////////////////////////////////////////////
double battery;
int voltage = A3;
int butval;
#include <TimerOne.h>
void setup() {
pinMode(led, OUTPUT);
Serial.begin(115200);
mySerial.begin(9600);
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
motor_fr.attach(4);
motor_br.attach(5);
motor_bl.attach(6);
motor_fl.attach(7);
mpu_calibrate();
Timer1.initialize(4000);
Timer1.attachInterrupt(mpu6050);
/*
for (int n = 0; n < 1000; n++) {
mpu6050_reading();
gxcal = gxcal + GyX;
gycal = gycal + GyY;
gzcal = gzcal + GyZ;
if (n % 100 == 0)Serial.println(n / 100);
delay(2);
}
gxcal /= 1000;
gycal /= 1000;
gzcal /= 1000;
*/
}
void loop() {
//variables of HC-12 anf mpu6050
// throttle
// roll
// pitch
// yaw
// but1 (button 1)
// but2 (button 1)
// x_ang
// y_ang
// x_rate
// y_rate
// battery (not yet checked)
//mpu6050();
mpu6050_reading();
battery_voltage();
decoding_input();
inputp_roll = y_ang;
outputp_roll = computePID_roll(inputp_roll, Setpoint_roll);
pid_output_roll = outputp_roll + throttle;
inputp_pitch = x_ang;
outputp_pitch = computePID_pitch(inputp_pitch, Setpoint_pitch);
pid_output_pitch = outputp_pitch + throttle;
output_motor();
}