Coding my flight stabilizer function

[Nick]

Hello all,
Brand new to this quadcopter stuff. I've build a 450 quad using an Arduino Uno as the flight controller. Though the arduino has a PID library ( don't really understand how to integrate it into my code ), I am trying to code my own flight stabilizing algorithm but can't get it quite right.

Here is a snippet of code regarding the algorithm:

  float angle_x = alpha*gyro_angle_x + (1.0 - alpha)*accel_angle_x; // Current Pitch of quad
  float angle_y = alpha*gyro_angle_y + (1.0 - alpha)*accel_angle_y; // Current Roll of quad

  raw = joystick[0]/11.25; // The Throttle input from 0 to 90

  throttle[0] = raw+40-(stabilize(angle_x))-(stabilize(angle_y));  // Top Left motor
  throttle[1] = raw+40-(stabilize(angle_x))+(stabilize(angle_y)); // Top right Motor
  throttle[2] = raw+40+(stabilize(angle_x))+(stabilize(angle_y)); // Bottom Right Motor
  throttle[3] = raw+40+(stabilize(angle_x))-(stabilize(angle_y)); // Bottom Left Motor
 
  for (int motor = 0; motor <= 3; motor++) {
    throttle[motor] = manageSpeed(throttle[motor]);  // Run manageSpeed for each motor...
  }
 
  motor1.write(throttle[0]); //Write the throttle to the quad motors
  motor2.write(throttle[1]);
  motor3.write(throttle[2]);
  motor4.write(throttle[3]);

double stabilize (double angle) {     //function for the algorithm
  double afterAngle = angle + 16;

  if(angle<0){
    return(-(abs(pow(afterAngle,5)/20596298)+.5)); //      <---------- algorithm for negative angle!!!
  }else{
    return(abs(pow(afterAngle,5)/20596298)+.5); //      <---------- algorithm for positive angle!!!
  }
}

double manageSpeed(double theSpeed) //keeps the throttle between the min and max of the ESC
{
  if(theSpeed <= 40){
    return(40);
  }
  else if (theSpeed >= 110) {
    return(110);
  } 
  else {
    return(theSpeed);
  }
}

Basically, the algorithm is a graph of y=((x+16)^5/20596298).5 which looks like

It seems more stable with this than without any stabilization so I think it's a step in the right direction. It's not nearly stable enough to hover without being tethered to a table that I am testing on. Any suggestions on what to do? Just learn to use the PID library for the Arduino or am I going in the right direction?

ps. I put more effort into this post than most of my life's school work so someone heeeelp me.

 

[GJH105775]

Haha. I am fair with coding (Python and Lua mostly ), well versed in theoretical physics (that is where all the fun is), but not very good for flight physics.

I believe you are moving in the right direction, have you taken a look at open source Arduino based flight controllers to get a basic idea of how their mat works? If you can figure out what they did you can get a good concept design. They spent a few years of tweaking.

Anyways what type of values does your gyro give you. Theoretically you want it to increase a motor's speed if the quad dips in that direction incrementally, other wise you will get terrible oscillations. There also needs to be a range that it responds in, because you can't keep it perfectly level.

so basically instead of the motor speeding up when you are 5 degrees out of level and kicking off when you get over 5 it would speed up when you got 7 degrees out of lever and go until you got to 0 or -1 degrees out of level.

Sorry for this being of little to no help, but that is all I have for right now. Care to explain how the Gyro outputs to the Arduino?

Here is a Arducopter's source, and here is a forum post that is very relevent, I think you may find it useful.

Here is an explanation of PID from a blogger I have followed for a while now.

This I think will be the most useful of all.

 

[Nick]

Thanks for your reply, I'm sorry it took this long for my response.

I do apologize for not explaining what gryo sensor I'm using. I'm currently using the MPU 6050 6-axis gryo/accelerometer that uses the "kalman filter" to combine the gyro and the accelerometer into one stable reading for each axis Pitch Roll and Yaw. I currently am not using Yaw yet. The pitch is either negative ( tilted down ) or positive ( tilted up ) as well as negative ( roll left ) or positive ( roll right ) Roll. This is what the angle_x ( pitch ) and angle_y ( roll ) are in the code above.

I have read the PID articles you read and understand it better to try at it again.

Before I do that I would like to eliminate some other possible factors. I need to know if I'm setting up my motors correctly and if they are all getting the same power.

The frame I'm using has a PCB bottom plate that I soldered the ESCs and the battery connector onto it. Also on the same battery connection, I have another connector that powers the on-board Arduino so I assume the motors are getting the same power. Here is a link to it.

The second thing is if I'm setting up and arming the motors correctly. From what I can tell, the motors are all the same exact model along with the ESCs. I bought the first motor Here, a set of four Here, and finally the last Here. I've burned out a couple so that's why there's six in all. After watching a video about using the servo library on the Arduino to power the motors, I came the to number of 40 to send to the ESCs that arms them using this bit of code.

  motor1.attach(6); // Attaching the motors to their pins on the Arduino
  motor2.attach(9);
  motor3.attach(10);
  motor4.attach(5);

  motor1.write(40); // Arming the motors
  motor2.write(40);
  motor3.write(40);
  motor4.write(40);

They seem to throttle up in uniform but I can't really tell. Is there addition things needed to do like programming the ESCs ( heard of this but didn't seem necessary at the time because they all fired up) or balancing the motors/props ( also heard of but not knowing what to do ). Also, I do not know if the motors take decimals for the throttle such as

motor1.write(45.5);

Here is basically the code I had put into my Arduino when I first tried to use the PID library.

#include <PID_v1.h>

//Define Variables we'll be connecting to
double Setpoint, Input, Output;

//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint,2,5,1, DIRECT);

void setup()
{
//initialize the variables we're linked to
Input = gyro sensor reading;
Setpoint = 0;

//turn the PID on
myPID.SetMode(AUTOMATIC);
}

void loop()
{
Input = gyro sensor reading;
myPID.Compute();
motor1.write(Output);
}

I will continue to try and make this work.

I can post some video of my quadcopter trying to fly ( tethered on a table in my garage ) if that will help spotting my mistakes at all. I probably won't be able to do any more flight tests till Thursday so I will update with my progress. Thanks again for your reply.

 

[Ethylene]

Thanks for your reply, I'm sorry it took this long for my response.

I do apologize for not explaining what gryo sensor I'm using. I'm currently using the MPU 6050 6-axis gryo/accelerometer that uses the "kalman filter" to combine the gyro and the accelerometer into one stable reading for each axis Pitch Roll and Yaw. I currently am not using Yaw yet. The pitch is either negative ( tilted down ) or positive ( tilted up ) as well as negative ( roll left ) or positive ( roll right ) Roll. This is what the angle_x ( pitch ) and angle_y ( roll ) are in the code above.

I have read the PID articles you read and understand it better to try at it again.

Before I do that I would like to eliminate some other possible factors. I need to know if I'm setting up my motors correctly and if they are all getting the same power.

The frame I'm using has a PCB bottom plate that I soldered the ESCs and the battery connector onto it. Also on the same battery connection, I have another connector that powers the on-board Arduino so I assume the motors are getting the same power. Here is a link to it.

The second thing is if I'm setting up and arming the motors correctly. From what I can tell, the motors are all the same exact model along with the ESCs. I bought the first motor Here, a set of four Here, and finally the last Here. I've burned out a couple so that's why there's six in all. After watching a video about using the servo library on the Arduino to power the motors, I came the to number of 40 to send to the ESCs that arms them using this bit of code.

  motor1.attach(6); // Attaching the motors to their pins on the Arduino
  motor2.attach(9);
  motor3.attach(10);
  motor4.attach(5);

  motor1.write(40); // Arming the motors
  motor2.write(40);
  motor3.write(40);
  motor4.write(40);

They seem to throttle up in uniform but I can't really tell. Is there addition things needed to do like programming the ESCs ( heard of this but didn't seem necessary at the time because they all fired up) or balancing the motors/props ( also heard of but not knowing what to do ). Also, I do not know if the motors take decimals for the throttle such as

motor1.write(45.5);

Here is basically the code I had put into my Arduino when I first tried to use the PID library.

#include <PID_v1.h>

//Define Variables we'll be connecting to
double Setpoint, Input, Output;

//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint,2,5,1, DIRECT);

void setup()
{
//initialize the variables we're linked to
Input = gyro sensor reading;
Setpoint = 0;

//turn the PID on
myPID.SetMode(AUTOMATIC);
}

void loop()
{
Input = gyro sensor reading;
myPID.Compute();
motor1.write(Output);
}

I will continue to try and make this work.

I can post some video of my quadcopter trying to fly ( tethered on a table in my garage ) if that will help spotting my mistakes at all. I probably won't be able to do any more flight tests till Thursday so I will update with my progress. Thanks again for your reply.

Hey, hope I'm not late to the party.
I'm trying to do an exact similar thing to what you are doing. I have the same motors, same gyro/accel chip and probably even the same Barometer.
This is my thread.
http://quadcopterforum.com/threads/first-build-am-i-doing-it-right.4598/

I might not be as much help yet, but maybe after I receive my parts. Also, I'm a noob can you explain what the PID library is and how it is helpful? And how did you burn out your motors? Don't want that happening over here

 

[Nick]

I read your post, it looks like you are in the same situation as I was in a month ago. I don't believe I have a barometer on mine.

PID is the most popular way that quads stabilize the motors. It runs your gyro input through some algorithms and output a throttle ( theoretically because I can't get it to work right ) to give and take from motors to keep the quad level.

First motor burned out by having the first motor attached to the frame UNSECURED on the floor, me testing my code, had a typo that sent the motor to max throttle and flipped the quad pinning the prop down so it couldn't spin which burned out the ESC and I assume the motor that was still trying to spin.

The second ESC burned out when I was testing another motor to see which way it was spinning before I heatshinked all of the connections. The bullet connectors that connected the ESC to the motor were touching BARE metal to metal. As soon as I fired up the motor the ESC caught fire and it also damaged the motor. BE CAREFUL WHEN TESTING MOTORS.

For the controller I'm using the Nrf24Lo1-2.4GHz modules and some stock code from the arduino playground to send my throttle to the quadcopter. They were really cheap. I put the receiver and the MPU 6050 on a protoshield I got from radioshack for like $6.

Spoiler: My wireless setup

Arduino on-board the quad

The monster of soldering under the protoshied

Transmitter on a breadboard. Don't make fun I just made it yesterday

Arduino under the breadboard.

Going back to my topic, is there anything that I need to take into account for my unstable quad like balancing the props or propering arming the motors? Thanks

 

[GJH105775]

I'll leave you with Nick, better hands than mine for this problem .

 

[Nick]

Though I could help Ethylene in giving insight to what I have done, I still need help with my problems that this thread is about . I'm going to try to get the PID to work but I'd to also know the answers to the questions I stated above.

 

[GJH105775]

Surprisingly hard to find info.

Also what PID does Ardupilot use?.

 

[Ethylene]

Though I could help Ethylene in giving insight to what I have done, I still need help with my problems that this thread is about . I'm going to try to get the PID to work but I'd to also know the answers to the questions I stated above.

Just curious, how much programming experience do you have really? I want an idea before I get started as to the level of difficulty involved and your experience.

 

[Nick]

It's hard sifting through the ardupilot code. I actually can't find the .ino file to start looking. I've been programming for like 7 years but I've only been using Arduinos for like a year. This is my first actual project with an Arduino. I'll keep looking in the Ardupilot code to see its PID settings.

On a separate note, I think I missed two fundamental things about using brushless motors on an Arduino. I forgot to calibrate the motors for one to set an exact range of throttle. I found this code here to calibrate them ( Only got to calibrate one motor before I left for work I'll do the rest tomorrow ). The second thing that was hinted to me in the code I found to calibrate the motors, the function to use is the

motor.writeMicroseconds( throttle );

that ranges from like 700 to 2000 where as the function I was using

motor.write( throttle );

is less precise and only seems to range from like 40 to 110 for my motor.

 

[GJH105775]

Also be careful if you spin your motors up with no props (especially with custom code) you can burn them out.

 

[Nick]

Well I managed to calibrate all of the motors to the same throttle values so that may have fixed some stability issues. I will know when I test the quad in a couple hours.

 

[GJH105775]

I wish you luck. I'd like to see a Raspberry Pi and Arduino for some really advanced stuff. Or you could just go with a Beagle bone and only have one board. How hard is it to establish a com port between an Arduino and a Pi?

 

[Nick]

I haven't ever used a raspberry pi but i've heard its very compatible with an arduino.

 

[GJH105775]

Did you do the test flight? If so how did it go?

 

[Nick]

I'm still trying to sort out this PID stuff. I'm still not getting the right values. I might just test it using my own algorithm because I'm so pissed off at this PID stuff.

 

[GJH105775]

Have you tried it yet to make sure your hardware does in fact preform ok? I had a few hardware problems that acted like FC problems. Between imperfections in the motors and resistance on the ESC

 

[Nick]

And In saying that, I think I just fixed the PID to work! Time to reassemble and test before it gets to late!

 

[GJH105775]

Really? What was the problem?

 

[Nick]

I mean fixed it as in successfully implementing the PID into my code. I got one test flight before something unrelated when terribly wrong. I was plugging in the Arduino for the second test and sparks flew at the power connector and destroyed the connection to the ground cable. Here is a picture. I think I'm going to upload the one good test to youtube and link it here to show you.

Can this cable just not take the voltage?