Quadcopter stability control, No Radio controller, Android App

Projects General Guidance
1

I am doing a Quadcopter project and i need help!, i have done most of the part, reading sensors, controlling motors etc. I am totally stuck at the stabilization algorithm part. I tried PID but no go as i dont know how to set the output to servo speed. I tried my own code, that is given below

bool errState(int type) // returns true if the copter is not level
{
  if (type == PITCHPVE) // Positive pitch
  {
    if (getDeg(ypr[1]) > getDeg(statYPR[1]) + 3)
      return true;
    else
      return false;
  }
   if (type == PITCHNVE) // Negative Pitch
  {
    if (getDeg(ypr[1]) < getDeg(statYPR[1]) - 3)
      return true;
    else
      return false;
  }
  if (type == ROLLPVE) // +ve Roll
  {
    if (getDeg(ypr[2]) < getDeg(statYPR[2]) - 3)
      return true;
    else
      return false;
  }
   if (type == ROLLNVE)// -ve Roll 
  {
    if (getDeg(ypr[2]) < getDeg(statYPR[2]) - 3)
      return true;
    else
      return false;
  }
}

//comSpd holds the last common value written to all the motors using writeMicroseconds()
        //escSpeed holds current speeds of each motor
        // getIMUData() sets yaw, pitch and roll readings from imu in ypr[];
void autoLevel(int code, float error)
{
  switch (code)
  {
  case PITCH:
  {
    // runs for +ve pitch
    if (error > 0)
    {
      while (errState(PITCHPVE))
      {
        
        if(escSpeed[0] > comSpd-100 && escSpeed[2] < comSpd + 100)
        {
          esc[0].writeMicroseconds(--escSpeed[0]);
        esc[2].writeMicroseconds(++escSpeed[2]);
        }   
        getIMUData(false);
      }
    }
    if (error < 0)
    {
   
      while (errState(PITCHNVE))
      {
        //getIMUData(false); 
        if(escSpeed[2] > comSpd-100 && escSpeed[0] < comSpd + 100)
        {
          esc[2].writeMicroseconds(--escSpeed[2]);
        esc[0].writeMicroseconds(++escSpeed[0]);
        }
        getIMUData(false);
      }
   
    }
    break;
  }
  case ROLL:
  {
    if (error > 0)
    {  
      while (errState(ROLLPVE))
      {
        if(escSpeed[1] > comSpd-100 && escSpeed[3] < comSpd + 100)
        {
          esc[1].writeMicroseconds(--escSpeed[1]);
        esc[3].writeMicroseconds(++escSpeed[3]);
        }
        getIMUData(false);
       
      } 
    }
    if (error < 0)
    {
      while (errState(ROLLNVE))
      {
        if(escSpeed[3] > comSpd-100 && escSpeed[1] < comSpd + 100)
        {
          esc[3].writeMicroseconds(--escSpeed[3]);
        esc[1].writeMicroseconds(++escSpeed[1]);
        }
        
        getIMUData(false);   
      }
    }
    break;
  }
  case YAW:
  {

    break;
  }
  default:
    return;

  }
  

  return;
}

I didnt code for YAW yet. thought to finish the PITCH and ROLL first.

The quad copter oscillates violently when it lifts off, it pitches, it rolls simultaneously in either direction, but theoretically the code should be correct right?

I need some help urgently.

The components

Arduino Mega 2560

Sensors:

MPU6050
HMC5883L

Connectivity:

ESP8266

I have not used any radio controller or KK boards.

the fan speeds are sent to arduino via the wifi module as a HTTP request with an android app and it sets the speed of all the motors accordingly.

2

I did not look into your code, but when it comes to PID control, oscillation/overshoot means that your tuning-constants (kp, ki, kd) are not ideal (too high).
lg, couka

3

I imported the PID library, created variables as follows

double pIn, pOut=0, pSet, pKp=1, pKd=0.5, pKi=0.05;
double rIn, rOut=0, rSet, rKp=1, rKd=0.5, rKi=0.05;
PID pitch( &pIn, &pOut, &pSet, pKp, pKd, pKi,DIRECT), roll( &rIn, &rOut, &rSet, rKp, rKd, rKi,DIRECT);

The input is the pitch,roll value from the IMU, The setPoint is the static YPR values obtained by Jeff Rowberg's code. Most of the time, the output remains zero, so please correct me if i am wrong. Also, how do i convert the PID output to corresponding Servo values between 1000,2000 if i do get an output?

Also i mentioned in my previous post that i didnt use PID, i used my own code. Acc to me, the code is pretty straightforward and should work. Please point me in the right direction

4

Do you understand the concept of a PID controller?

lg, couka

5

Yes, We set a point, and an input, it steers the input towards the setpoint, using constants kP(Proportional gain), kD(Deferential gain), kI(Integral Gain). Correct me if i'm wrong.

6

If i am wrong, what should be the input to the PID variables, i hope it is the current gyro angles, and the setpoint is 0 deg, if so, how do i convert an output that is in the range of degrees to servo speed?

7

Prominence:
what should be the input to the PID variables, i hope it is the current gyro angles

A gyro does NOT tell you the absolute angle of your quad, but the current gyro-rate (aka "how fast am I spinning around a certain axis")

Maybe you should show the whole code, not just a fraction.

lg, couka

8

I will post the whole code, but by gyro angles, i meant the YPR angles calculated by the internal DMP chip on the MPU6050 using its respective arduino library. It does its internal sensor fusion so by 'gyro angles' i meant the "ACTUAL" angles calculated by the DMP on the IMU. I am formatting the code so you can uderstand and i will post it soon.

9

while (errState(PITCHPVE))

You realise that you will never leave this loop? The pitch will never be exactly equal to the set point and it will always be hunting around a little positive or a little negative.

So that means you never even get to doing the roll stability.

You are stuck in a linear program. Trying to level one direction and then the other just won't work because even if you do hit exactly zero level in pitch, it will fall off level while you are trying to do the roll function. Think of your program as continuously checking all of the inputs and setting all of the outputs thousands of times per second. Your main loop should do every task on every loop but it should only do short tasks - nothing should take more than a few milliseconds to complete its actions, such as increasing or decreasing a motor.

Your main loop should look something like this:

void loop() {
  readDMP();
  setMotorOutputs();
  if(controlInputAvailable()) processControlInput();
}
10

MorganS:
You realise that you will never leave this loop? The pitch will never be exactly equal to the set point and it will always be hunting around a little positive or a little negative.

But the errState() function returns true only if the deviation is greater than +- 3 degrees from the setpoint, else if it is within 3 degrees negative and 3 degrees positive, it returns false, so the while loop should break according to me.

11

Thanks for all your replies ,I have Attached the entire sketch in the file below. i have tried the following so far.

ESC. NO Direction
1 FWD
2 LEFT
3 REVERSE
4 RIGHT

FIRST TRY:

Positive pitch, the motor with ESC1 raises, so i reduce its speed little by little unless its level.
Negative pitch, the motor with ESC3 raises, so i reduce its speed little by little unless its level.
Positive roll, Motor with ESC2 Raises, so i reduce its speed little by little unless its level.
Negative roll, Motor with ESC4 Raises, so i reduce its speed little by little unless its level.

The downside was, after it tries the level itself, the entire effective thrust gets reduced and the copter drops back to the ground.

SECOND TRY:

Positive pitch, the motor with ESC1 raises, so i reduce its speed little & increase ESC3 Speed a little unless its level.

Negative pitch, the motor with ESC3 raises, so i reduce its speed little & increase ESC1 Speed a little unless its level.

Positive roll, the motor with ESC2 raises, so i reduce its speed little & increase ESC4 Speed a little unless its level.

Negative roll, the motor with ESC4 raises, so i reduce its speed little & increase ESC2 Speed a little unless its level.

This makes the quad shake and oscillate violently in either direction.

Either manual control like this or i need help with PID, as i haven't tried it yet, since the i don't know what exactly has to be done in actually implementing PID for a quad copter.

Please Help.!

Freyr_Master.ino (10.2 KB)

12

Do you still need help with this? Have you made any progress since that last message?