PID tuning Quadrocopter

Hello!

We have big problems with the PID tuning parameters for our quadrocopter.

We have, filtered the acc and gyro readings and ploted them with processing.
A lot of testing but the only thing we can get is a oscillation.

Have missed something essential?

Posting the code.

Please give some feedback or tips, we have been testing for 35-40 hours. We are stuck here.

Any questions about the code, ask and I tell you…

int gyroPinX = 2; //compass.a.x             
int gyroPinY = 3; //compass.a.x             


float gyroVoltage = 5;         //Gyro is running at 5V
float gyroZeroVoltageX, gyroZeroVoltageY;//Gyro is zero ..setup
float gyroSensitivity = .00333;  //Our example gyro is 3,3mV/deg/sec  ungefär.
float rotationThreshold = 3;   //Minimum deg/sec to keep track of - helps with gyro drifting
float gyroRateX,gyroRateY;
int angleX,angleY,accZeroValueX,accZeroValueY;
float currentAngleX,currentAngleY;   //Keep track of our current angle
int inByte = 0;  
int ground =1150;
int incomingByte; 




//test int och float
float gyroAngle;

#include <Servo.h> 
#include <Wire.h>
#include <LSM303DLH.h>
#include <PID_v1.h>

LSM303DLH compass;

//Define Variables we'll be connecting to
double Setpoint, Input, Output;
double SetpointY, InputY, OutputY;
//Define the aggressive and conservative Tuning Parameters

double consKp=40.0, consKi=0.0, consKd=4.0;
double consKpY=2.0, consKiY=0.2, consKdY=0.4;


PID xPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT);
PID yPID(&InputY, &OutputY, &SetpointY, consKpY, consKiY, consKdY, DIRECT);

Servo esc1x,esc2x,esc1y,esc2y;

void setup() {
  Serial.begin (9600);
  gyroSetup();
   Wire.begin();
  compass.enable();
   accSetup(); 
  esc1x.attach(6);
  esc2x.attach(10);
  
  esc1y.attach(9);
  esc2y.attach(11);

  delay(10);
  
  esc1x.writeMicroseconds(0);
  esc2x.writeMicroseconds(0);
  esc1y.writeMicroseconds(0);
  esc2y.writeMicroseconds(0);
  delay(3000);
  esc1x.writeMicroseconds(1400);
  esc2x.writeMicroseconds(1400);
  esc1y.writeMicroseconds(1400);
  esc2y.writeMicroseconds(1400);
  esc1x.writeMicroseconds(1000);
esc2x.writeMicroseconds(1000);
  
  Setpoint=0;
  //myPID.SetControllerDirection(REVERSE);
    xPID.SetOutputLimits(-40, 40);
    xPID.SetMode(AUTOMATIC);
    xPID.SetTunings(consKp, consKi, consKd);

    yPID.SetOutputLimits(-60, 60);
    yPID.SetMode(AUTOMATIC);
    yPID.SetTunings(consKpY, consKiY, consKdY);

}

void loop() {
 
  


Xangle (); // return X angle as float currentAngleX
Input=currentAngleX;

xPID.Compute();


   
/*if (Output>=0)
{
esc1x.writeMicroseconds(ground);
esc2x.writeMicroseconds(ground+Output);

}else{
esc1x.writeMicroseconds(ground-Output); //--=+
esc2x.writeMicroseconds(ground);

}
*/

Yangle ();// return Y angle       -||-

InputY=currentAngleY;

if (InputY>3 || InputY<-3)
{
  yPID.SetTunings(consKpY, consKiY, consKdY); // felet större än...
}else{
  yPID.SetTunings(consKpY/2, consKiY, consKdY);
}

yPID.Compute();

if (OutputY>=0)
{
esc1y.writeMicroseconds(ground+OutputY);
esc2y.writeMicroseconds(ground);

}else{
esc1y.writeMicroseconds(ground); //--=+
esc2y.writeMicroseconds(ground-OutputY);

}

  
/*
esc1y.writeMicroseconds(1000);
esc2y.writeMicroseconds(1000);*/


 
serialRead(); 


int intprint=currentAngleY;
int intprint2=OutputY;
Serial.print(intprint); Serial.print(","); Serial.println(intprint2);


delay(10);


}

void gyroSetup(){
  delay(10);
  gyroZeroVoltageX= (analogRead(gyroPinX) * gyroVoltage) / 1023;
  delay(10);
  gyroZeroVoltageY= (analogRead(gyroPinY) * gyroVoltage) / 1023;
  delay(10);
}

void accSetup()
{
  compass.read();
  delay(5);
  accZeroValueX=compass.a.x/11.67;
  accZeroValueY=compass.a.y/11.67;
}

void Xangle () {
  gyroRateX = (analogRead(gyroPinX) * gyroVoltage ) / 1023;
  gyroRateX -= gyroZeroVoltageX;
  gyroRateX /= gyroSensitivity;
  
  if (gyroRateX >= rotationThreshold || gyroRateX <= -rotationThreshold) {
    //This line divides the value by 100 since we are running in a 10ms loop (1000ms/10ms)

    //Acc reading
    compass.read();
    angleX=(-compass.a.x/11.67)+accZeroValueX;

    gyroRateX /= 55; // konvertera till grafiskt korrekt
    currentAngleX = 0.98 * (currentAngleX + gyroRateX) + 0.02 * angleX;
    }   
}
void Yangle() { // copy of void Xangle
  
  gyroRateY = (analogRead(gyroPinY) * gyroVoltage ) / 1023;
  gyroRateY -= gyroZeroVoltageY;
  gyroRateY /= gyroSensitivity;
  
  if (gyroRateY >= rotationThreshold || gyroRateY <= -rotationThreshold) {
    //This line divides the value by 100 since we are running in a 10ms loop (1000ms/10ms)

    //Acc reading
    compass.read();
    angleY=(compass.a.y/11.67)-accZeroValueY;

    gyroRateY /= 55; // konvertera till grafiskt korrekt
    currentAngleY = 0.98 * (currentAngleY + gyroRateY) + 0.02 * angleY;
    }   
}


void serialRead() 
{
   if (Serial.available() > 0) {
    // read the oldest byte in the serial buffer:
    incomingByte = Serial.read();
    // if it's a capital H (ASCII 72), turn on the LED:
    if (incomingByte == 'H') {
      ground=1500;
esc1x.writeMicroseconds(1550);
esc2x.writeMicroseconds(1550);
/*    consKpY=consKpY+0.1;
   yPID.SetTunings(consKpY, consKiY, consKdY);  */
   delay(100);
  }
    
    
    if (incomingByte == 'L') {
      ground=1100;
esc1x.writeMicroseconds(1000);
esc2x.writeMicroseconds(1000);
      delay(50);
    }
    
    }
}

Two things.

When posting code, please enclose the code in [ code ] and [ / code ] tags (click the '#' button in the editor) to stop the forum from munging your code. Could you delete your posted code and re-post it in code tags?

Secondly, in order to diagnose problems with the PID settings you need to provide the input and output readings for the PID algorithm. I recommend you limit yourself to dealing with one degree of freedom at a time (restrain your vehicle on one axis while you tune the algorithm to control it on the other axis).