i am using mpu6050 (gyro+ accelometer ) for my quadcopter
i dont know weather to use gyro angular rates as the INPUTS for PID "or" the angle (in degrees) as the INPUTS.
and i even feel that all my motors do not produce the same amount of thrust at same trottle values.
(they are all calibrated.)
please help me
i dont know weather to use gyro angular rates as the INPUTS for PID of the angle (in degrees) as the INPUTS.
You can't. You need to integrate the gyro rates (after subtracting the offset) to get angles, and that will work for only a minute or two.
Hi yashshah99,
For basic quadcopter stabilisation you need to use the MPU6050's 3-axis gyroscope that measures angular rates. The pilot's input also needs to be converted to angular rates. The rates of both are then combined using three PID control loops (one for each axis), the outputs of which are then mixed and added to the throttle before being sent to the motors. This will result in what is know variously as rate, gyro or acro mode and is pretty much the standard mode for most experienced pilots, but can be difficult at first for the beginner. You need to get this mode working first, as it's fundamental to quadcopter stabilisation and all the other modes depend on it.
There's another mode in which the MPU6050's accelerometer is combined with the gyroscope using a complementary or extended kalman filter, (or alternatively let the MPU6050's Digital Motion Processor (DMP) calculate it for you). This provides an estimation of the copter's roll and pitch angle. Either filter effectively removes the gyroscope's tendency to drift over time. Controlling the quadcopter using roll and pitch angle is known variously as auto-level, self-level or stabilise mode. In this mode, when you let go of the sticks the copter will automatically return to level flight, making it easier for the beginner to master. This mode builds on the rate mode described above.
Up to a point it doesn't matter if motors seem to spool up unevenly, the control loops should take care of any variation in motor tolerances.
Suppose we are using the angular rate as inputs. so if at any moment the quad copter is at an angle of say 30degees for a while, then the angular rate becomes zero but the quadcopter is still at a tilted angle, now if the pilot wants 0 angular rate, the quadcopter will nt move at all it will remain tilted.
this will make it very difficult to control. am i right??
this will make it very difficult to control. am i right??
Flying by angular rate is how more experienced pilots fly, it gives a more natural feel to the controls than auto-level, although it's a little more difficult as the computer isn't providing any levelling assistance for you. The size of the quadcopter also matters. It's easier to fly a larger aircraft in rate mode, as everything happens more slowly. It's also possible to flip and roll the quadcopter in rate mode, something that's not possible in auto-level. In any case, most flight controllers allow you to switch between modes during flight, just by flicking a switch on your transmitter.
It's also necessary to tune and test the quadcopter's PID values in rate mode first, before moving on to tuning auto-level. This is because in auto-level the pitch and roll outer angle PID control loops still drive the angular rate inner ones. It's like the layers of an onion angle PIDs on the outside, angular rate at the core.
The pilot input is specifies the aircraft's desired (or future) angular rate. The gyro is providing a snapshot of your aircraft's current angular rate. The PID control loops drive the motors to close the error between where you are and where you'd like to be. In effect the control loops cause the gyroscope to shadow the pilots inputs. However, it also works the other way in that any disturbance not input by the pilot, such as a gust of wind, is sensed by the gyroscope and the control loops work to close the error oncemore. This is also the reason why the quadcopter's inherently unstable design is able to fly.
As you mentioned, in rate mode if pilot inputs roll at 30 degrees/s the motors are driven until the gyro reads 30 degree/s. So the quadcopter will keep rolling until you move the stick back to 0 degrees/s. If the aircraft has moved to say a 30 degrees bank in this time, it will continue to stay there at 30 degrees. If you'd like the quadcopter to be level again, you have to move the stick in the opposite direction to counter this motion. You soon get used to flying it this way.
thanks man for your help. it seems that controlling a rate quadcopter is out of my hands. so i have decided to make a angle based quad. only the yaw function will be rate based. i have written a code. i jst want you to check it and tell me if i am in the write direction.
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Servo.h"
#include "SPI.h"
#include "RF24.h"
#include "PID_v1.h"
#include "MPU6050.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
Servo m1, m2, m3, m4;
RF24 radio (7, 8); //RF24 radio(CE_PIN, CSN_PIN);
const uint64_t pipe = 0xE8E8F0F0E1LL; // Define the transmit pipe
int joystick[4];
int x1, y1, x2, y2;
double Input1;
double Setpoint1;
double Input2;
double Setpoint2;
double Input3;
double Setpoint3;
double Output1;
double Output2;
double Output3;
double mo1, mo2, mo3, mo4;
PID myPID1(&Input1, &Output1, &Setpoint1, 1, 0, 0, DIRECT); //Pitch
PID myPID2(&Input2, &Output2, &Setpoint2, 1, 0, 0, DIRECT); //roll
PID myPID3(&Input3, &Output3, &Setpoint3, 1, 0, 0, DIRECT); //yaw
bool dmpReady = false;
uint8_t mpuIntStatus;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];
// 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
double yawer, yawrate, lastyaw, t, t1, t2;
uint8_t teapotPacket[14] = { '
, 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0x00, 0x00, '\r', '\n' };
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
{
mpuInterrupt = true;
}
void setup()
{
Serial.begin(9600);
radio.begin();
radio.openReadingPipe(1, pipe);
radio.startListening();
;
m1.attach(3);
m2.attach(5);
m3.attach(6);
m4.attach(9);
m1.writeMicroseconds(800);
m2.writeMicroseconds(800);
m3.writeMicroseconds(800);
m4.writeMicroseconds(800);
myPID1.SetMode(AUTOMATIC);
myPID2.SetMode(AUTOMATIC); //turn the PID on
myPID3.SetMode(AUTOMATIC); //turn the PID on
myPID1.SetOutputLimits(-40, 40);
myPID2.SetOutputLimits(-40, 40);
myPID3.SetOutputLimits(-20, 20);
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz). Comment this line if having compilation difficulties with TWBR.
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
mpu.initialize();
devStatus = mpu.dmpInitialize();
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788);
if (devStatus == 0)
{
mpu.setDMPEnabled(true);
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
}
//delay(15000);
}
void loop()
{
delay(50);
if ( radio.available() )
{
bool done = false;
while (!done)
{
done = radio.read( joystick, sizeof(joystick) );
x1 = joystick[0];
y1 = joystick[1];
x2 = joystick[2]; //pitch
y2 = joystick[3]; //roll
}
}
/Serial.println(x1);
Serial.println(y1);
Serial.println(x2);
Serial.println(y2);/
Setpoint3 = y1;
Setpoint2 = y2;
Setpoint1 = -x2;
if (Setpoint1 < 5 && Setpoint1 > -5)
{
Setpoint1 = 0;
}
if (Setpoint2 < 5 && Setpoint2 > -5)
{
Setpoint2 = 0;
}
if (Setpoint3 < 5 && Setpoint3 > -5)
{
Setpoint3 = 0;
}
getangles();
t2 = millis();
t = t2 - t1;
t1 = t2;
yawer = ypr[0] * 180 / M_PI;
yawrate = (yawer - lastyaw) * 1000 / t;
lastyaw = yawer;
Input1 = (ypr[1] * 180 / M_PI);
Input2 = (ypr[2] * 180 / M_PI);
Input3 = yawrate;
if (Input1 > -1 && Input1 < 1)
{
Input1 = 0;
}
if (Input2 > -1 && Input2 < 1)
{
Input2 = 0;
}
/Serial.println(Input1);
Serial.println(Input2);
Serial.println(Input3);/
myPID1.Compute();
myPID2.Compute();
myPID3.Compute();
Serial.println(Output1);
Serial.println(Output2);
mo1 = constrain((x1 + Output2 + Output1 - Output3), 800, 1500);
mo2 = constrain((x1 - Output2 + Output1 + Output3), 800, 1500);
mo3 = constrain((x1 - Output2 - Output1 - Output3), 800, 1500);
mo4 = constrain((x1 + Output2 - Output1 + Output3), 800, 1500);
m1.writeMicroseconds(mo1); //cc
m2.writeMicroseconds(mo2); //c
m3.writeMicroseconds(mo3); //cc
m4.writeMicroseconds(mo4); //c
/*Serial.println("1 "); Serial.println(mo1);
Serial.println("2 "); Serial.println(mo2);
Serial.println("3 "); Serial.println(mo3);
Serial.println("4 "); Serial.println(mo4);
Serial.println("------------------------------------------------------------------------------------------------------------------------");
*/
}
void getangles()
{
if (!dmpReady) return;
while (!mpuInterrupt && fifoCount < packetSize)
{
}
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
if ((mpuIntStatus & 0x10) || fifoCount == 1024)
{
mpu.resetFIFO();
}
else if (mpuIntStatus & 0x02)
{
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
/*Serial.print("ypr\t");
Serial.print(ypr[0] * 180/M_PI);
Serial.print("\t");
Serial.print(ypr[1] * 180/M_PI);
Serial.print("\t");
Serial.println(ypr[2] * 180/M_PI); */
}
}
The point I was making was you have to create an angular rate system first. Only once this is working can you proceed to implementing an angle based approach such as auto-level. Auto-level still depends on the angular rate system.
If you think your piloting skills aren't up to testing it, the best thing to do is to buy a cheap micro quadcopter and learn on that first.
Regarding your code, you'll need to implement an angular rate system. You'll also need to run your electronic speed controllers (ESCs) using analogWrite() that output a PWM frequency of 490Hz, rather than the servo library at 50Hz. At 50Hz your ESCs won't be responsive enough. The PWM signal to the ESCs is usually between 1000 and 2000us. The MPU6050's digital low pass filter should be activated and set at around 20Hz. Finally, it's also very important to implement some means of arming and disarming the motors for safety.
well, what is the low pass filter? how would it help?
The MPU6050 contains a low pass filter, its function is to remove noise due to vibration and unwanted frequencies generated when sampling the gyroscope and/or accelerometer.
what must be the output limits for the PIDs??
what must be the output limits for the PIDs??
For motors I constrain them between the minimum and maximum throttle values.
It might also be necessary to limit the I (integral) term of your PIDs to prevent them from getting too large, (known as integral windup).
i tried making a quadcopter based on angular rate. i cut my arm 
.
i think it was unstable because i used the servo library to control the motors.
MY ESCs DONT WORK ON ANALOG OUTPUTS...