MPU6050/PID problem

Hello!
i’ve got a problem with my quadcopter project.
As you can see in the first picture (where the green is the ProcessValue from MPU6050 and the blue is the outsignal from the PID) the PIDs outsignal is fucking up.
And on the other picture the ProcessValue is generated by the code right under “valueread(pitch, roll, height);” (in the loop function) and the valueread function disabled by change it to valueread(pitchu, roll, height);" it doesn’t fuck up.
Any clues why??

I’ve taken the MPU6050 code from Jeff Rowberg’s example code https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/Examples/MPU6050_DMP6/MPU6050_DMP6.ino, so I’m not 100% sure how that works.

#include <Servo.h>
#include <PID_v1.h>
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu;
#define OUTPUT_READABLE_YAWPITCHROLL
#define OUTPUT_READABLE_REALACCEL

//Gyro stuff

// MPU control/status vars
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

// orientation/motion vars
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

// === INTERRUPT DETECTION ROUTINE ===
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}

Servo motorfront;
Servo motorback;
Servo motorright;
Servo motorleft;

int braap;
int first=0;
double pitch = 0;
double roll;
double pospitchkvot;
double negpitchkvot;
double firstnegpitchkvot;
double posrollkvot;
double negrollkvot;
double pitchkvot;
double notpitchkvot;
double rollkvot;
double notrollkvot;
double height;
double heightfilter;
double heightout;
double heightoutf;
double SPheight=4280;
double mfront;
double mback;
double mright;
double mleft;

PID pospitchPID(&pitch, &pospitchkvot, 0, 1, 0, 0, REVERSE);
PID negpitchPID(&pitch, &firstnegpitchkvot, 0, 1, 0, 0, DIRECT);
PID posrollPID(&roll, &posrollkvot, 0, 0.002, 0, 0, DIRECT);
PID negrollPID(&roll, &negrollkvot, 0, 0.002, 0, 0, REVERSE);
PID heightPID(&heightfilter, &heightout, &SPheight, 0.002, 0, 0, DIRECT);

void setup() { 
  //Start I2C and Serial
  Wire.begin();
  Serial.begin(115200);
  while(!Serial);
  
  // initialize   
  Serial.println(F("Initializing I2C devices..."));
  mpu.initialize();
  
  // verify connection
   Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
  
  // load and configure the DMP
  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();
    
  // make sure it worked (returns 0 if so)
  if (devStatus == 0) {
      // turn on the DMP, now that it's ready
      Serial.println(F("Enabling DMP..."));
      mpu.setDMPEnabled(true);

      // enable Arduino interrupt detection
      Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
      attachInterrupt(0, dmpDataReady, RISING);
      mpuIntStatus = mpu.getIntStatus();

      // set our DMP Ready flag so the main loop() function knows it's okay to use it
      Serial.println(F("DMP ready! Waiting for first interrupt..."));
      dmpReady = true;

      // get expected DMP packet size for later comparison
      packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
      Serial.print(F("DMP Initialization failed (code "));
      Serial.print(devStatus);
      Serial.println(F(")"));
      error();
    }

 //Set up all motors
 motorfront.attach(3);
 motorback.attach(5);
 motorright.attach(6);
 motorleft.attach(9);
 delay(2000);
 
 //Set output limit so it'll calculate right
 pospitchPID.SetOutputLimits(0,100 );
 negpitchPID.SetOutputLimits(0,1);
 posrollPID.SetOutputLimits(1,2);
 negrollPID.SetOutputLimits(0,1);
 heightPID.SetOutputLimits(60,170);

 //Set LED-pin as output
 pinMode(13, OUTPUT);
 
 //Call motor begin function to initiate the motors
 Serial.println("MOTORS STARTING!!");
 Motor_begin();
 
 //Activate PID regulators
 heightPID.SetMode(AUTOMATIC);
 pospitchPID.SetMode(AUTOMATIC);
 negpitchPID.SetMode(AUTOMATIC);
 posrollPID.SetMode(AUTOMATIC);
 negrollPID.SetMode(AUTOMATIC);
}

void loop() { 
  
  //double pitchu;
  valueread(pitch, roll, height);
  
  /*
  if (first == 0 && pitch < 100){
    pitch ++;
  }
  else if(first == 0 && pitch == 100){
    first = 1;
  }
  else if (first == 1 && pitch > 0){
    pitch --;
  }
  else if(first == 1 && pitch == 0){
    first = 0;
  }
  /*
  
  braap = braap++;
  
  //Compute PID regulators
  heightPID.Compute();
  pospitchPID.Compute();
  negpitchPID.Compute();
  posrollPID.Compute();
  negrollPID.Compute();
  
  //Flip negative quota
  negpitchkvot = (firstnegpitchkvot - 0) * (0 - 1) / (1 - 0) + 1;
  
  //Calculate quota
  pitchkvot = pospitchkvot * negpitchkvot;
  rollkvot = posrollkvot * negrollkvot;
  
  notpitchkvot = (1 - pitchkvot) + 1;
  notrollkvot = (1 - rollkvot) + 1;
  
  
  //saftey variable
  heightoutf = heightout;
  
  
  //test
  heightoutf = 80;
  

  //Multiply quota and height speed
  mfront = pitchkvot * heightoutf;
  mback = notpitchkvot * heightoutf;
  mright = rollkvot * heightoutf;
  mleft = notrollkvot * heightoutf;
  
  motorfront.write(mfront);
  motorback.write(mback);
  
  Serial.print(braap);
  Serial.print(";");
  Serial.print(pospitchkvot);
  Serial.print(";");
  Serial.print(negpitchkvot);
  Serial.print(";");
  Serial.println(pitch);
}

void Motor_begin() {
  
  //Show Motors are starting by blinking LED
  digitalWrite(13, HIGH);
  
  //Get motors to go to on mode
  motorfront.write(60);
  motorback.write(60);
  motorright.write(60);
  motorleft.write(60);
  delay(500);
  
  //Blink
  digitalWrite(13, LOW);
  delay(500);
  
  //Blink
  digitalWrite(13, HIGH);
  
  //Get motors to go to drive mode
  motorfront.write(70);
  motorback.write(70);
  motorright.write(70);
  motorleft.write(70);
  delay(500);
  
  //Blink
  digitalWrite(13, LOW);
  delay(500);
  
  //Blink
  digitalWrite(13, HIGH);
  
  //Test motors
  for(int x=60; x<=80; x++)
  {
    motorfront.write(x);
    motorback.write(x);
    motorright.write(x);
    motorleft.write(x);
    delay(100); 
  }
  
  //Blink
  digitalWrite(13, LOW);
  
  //Go back to 0 speed
  motorfront.write(60);
  motorback.write(60);
  motorright.write(60);
  motorleft.write(60);
  delay(500);
  
  //Blink
  digitalWrite(13, HIGH);
  delay(500);
  
  //End of test, shut LED off
  digitalWrite(13, LOW);
}

double valueread(double & x,double & z,double & y) {
  // if programming failed, don't try to do anything
  if (!dmpReady) {error();}
  // wait for MPU interrupt or extra packet(s) available
  while (!mpuInterrupt && fifoCount < packetSize) {}
  // reset interrupt flag and get INT_STATUS byte
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();

  // get current FIFO count
  fifoCount = mpu.getFIFOCount();

  // check for overflow (this should never happen unless our code is too inefficient)
  if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
      // reset so we can continue cleanly
      mpu.resetFIFO();
      //Serial.println(F("FIFO overflow!"));

  // otherwise, check for DMP data ready interrupt (this should happen frequently)
  } else if (mpuIntStatus & 0x02) {
      // wait for correct available data length, should be a VERY short wait
      while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

      // read a packet from FIFO
      mpu.getFIFOBytes(fifoBuffer, packetSize);
      
      // track FIFO count here in case there is > 1 packet available
      // (this lets us immediately read more without waiting for an interrupt)
      fifoCount -= packetSize;
	  
      #ifdef OUTPUT_READABLE_YAWPITCHROLL
          // display Euler angles in degrees
          mpu.dmpGetQuaternion(&q, fifoBuffer);
          mpu.dmpGetGravity(&gravity, &q);
          mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
          x = ypr[1] * 180/M_PI;
          z = ypr[2] * 180/M_PI;
      #endif

      #ifdef OUTPUT_READABLE_REALACCEL
          // display real acceleration, adjusted to remove gravity
          mpu.dmpGetQuaternion(&q, fifoBuffer);
          mpu.dmpGetAccel(&aa, fifoBuffer);
          mpu.dmpGetGravity(&gravity, &q);
          mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
          y = aaReal.z;
      #endif
}
}

void error() {
  while(0==0) {
    digitalWrite(13, HIGH);
    Serial.println("ERROR!!!!");
  }
}

Hello I'm doing a quadricopter and I'm stuck in the PID, you get your problem solved? O advances code if you want to contact me A greeting!

It looks like the standard library does not work when using interrupts