Help me stabilize a quadcopter

The goal is to stabilze the quad using MPU6050 and arduino hovering at 50cm from ground. [I do not want to control it, just hovering]
I have the pitch roll yaw filtered and they are in degrees. Im not really sure what to do next? Bascially the speed of the motors is always 1000 even when the IMU moves around. There is something wrong with the PID.
Any ideas? thank you.

Main.ino

#include <Servo.h>#include <config.h>
#include <Wire.h>
#include <I2Cdev.h>
#include <PID_v1.h>
#include <IMU.h>   
#include <MotorController.h>


IMU                   imu;
MotorController       motors;




double yawOut, yawSet = 0;
double pitchOut, pitchSet = 0;
double rollOut, rollSet = 0;


double yawIn = imu.ypr[0];
double pitchIn = imu.ypr[1];
double rollIn = imu.ypr[2];


PID* yawPID   = new PID(&yawIn,   &yawOut,   &yawSet,    2.0,   0.0,  0.0, DIRECT);
PID* pitchPID = new PID(&rollIn,  &pitchOut, &pitchSet,  7,     0.0,  1.1, REVERSE);
PID* rollPID  = new PID(&pitchIn, &rollOut,  &rollSet,   10.0,  0.06, 1.5, REVERSE);


int initSpeed = 1000;




// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================
void setup() {
  Wire.begin();
  Serial.begin(115200);
  imu.init();
  motors.init();
  initControllers();
  pitchPID->SetOutputLimits(-1000, 1000);
  pitchPID->SetMode(AUTOMATIC);
  rollPID->SetOutputLimits(-1000, 1000);
  rollPID->SetMode(AUTOMATIC);
  yawPID->SetOutputLimits(-300, 300);
  yawPID->SetMode(AUTOMATIC);
}






// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================
void loop() {
  imu.update(); //Its gets the raw data from MPU6050 filters and converts to yaw pitch roll -> degrees and stores them into an array. imu.ypr[3]. so imu.ypr[0] is yaw and so on


  // Check for user input
  if(Serial.available()){
    byte inpt = Serial.read();
    
    if(inpt == 'w') 
      initSpeed += 10;  //Throttle up
    else if(inpt == 's')
      initSpeed -= 10;  //Throttle down
    else if(inpt == 'x'){
      Serial.println("Stopping...");
      motors.arm();
    }
  }


  pitchPID->Compute();
  rollPID->Compute();
  yawPID->Compute();


  motors.setPitch(initSpeed + yawOut, pitchOut);
  motors.setRoll(initSpeed - yawOut, rollOut);
}

MotorController

// Set the north and south motor speeds in terms of a base speed and an offset// A positive offset means a greater value to the north motor.
void MotorController::setPitch(int base, int offset){
    int north = base + (offset / 2);
    int south = base - (offset / 2);
    if(offset % 2 != 0){ // if offset is odd
        if(offset > 0){
            north++;
        }else{ // (0 is even, so no else if)
            south++;
        }
    }
    M[0].writeMicroseconds(north); //Write speed to motor 0
    M[2].writeMicroseconds(south);//Write speed to motor 2
    
}

// Set the east and west motor speeds in terms of a base speed and an offset
// A positive offset means a greater value to the east motor.
void MotorController::setRoll(int base, int offset){
    int east = base + (offset / 2);
    int west = base - (offset / 2);
    if(offset % 2 != 0){ // if offset is odd
        if(offset > 0){
            east++;
        }else{ // (0 is even, so no else if)
            west++;
        }
    }
    M[1].writeMicroseconds(east);   //write speed to motor 1
    M[3].writeMicroseconds(west);  //Write speed to moto 3
   
}

There are already three or four published solutions to this sort of problem.