# Raspberry Pi quadcopter thrashes at high speeds

I am attempting to build a Raspberry Pi based quadcopter. So far I have succeeded in interfacing with all the hardware, and I have written a PID controller that is fairly stable at low throttle. The problem is that at higher throttle the quadcopter starts thrashing and jerking. I have not even been able to get it off the ground yet, all my testing has been done on a test bench. I have ruled out bad sensors by testing each sensor individually, and they seem to be giving correct values. Is this a problem with my code, or perhaps a bad motor? Any suggestions are greatly appreciated.

Here is my code so far:

package com.zachary.quadserver;

import java.net.*;
import java.io.*;
import java.util.*;

private static Sensor sensor = new Sensor();

private final static int FREQUENCY = 490;

private static double PX = 0;
private static double PY = 0;

private static double IX = 0;
private static double IY = 0;

private static double DX = 0;
private static double DY = 0;

private static double kP = 1.3;
private static double kI = 2;
private static double kD = 0;

private static long time = System.currentTimeMillis();

private static double last_errorX = 0;
private static double last_errorY = 0;

private static double outputX;
private static double outputY;

private static int val[] = new int[4];

private static int throttle;

static double setpointX = 0;
static double setpointY = 0;

public static void main(String[] args) throws IOException, NullPointerException {

PWMDevice device = new PWMDevice();
device.setPWMFreqency(FREQUENCY);

PWMChannel BR = device.getChannel(12);
PWMChannel TR = device.getChannel(13);
PWMChannel TL = device.getChannel(14);
PWMChannel BL = device.getChannel(15);

DatagramSocket serverSocket = new DatagramSocket(8080);

public void run(){
while(true) {
try {
throttle = (int)(Integer.parseInt((message.split("\\s+")[4]))*12.96)+733;
setpointX = Integer.parseInt((message.split("\\s+")[3]))-50;
setpointY = Integer.parseInt((message.split("\\s+")[3]))-50;

} catch (IOException e) {
e.printStackTrace();
}
}
}
};

while(true)
{
Arrays.fill(val, calculatePulseWidth((double)throttle/1000, FREQUENCY));

double dt = (double)(System.currentTimeMillis()-time)/1000;

double hypotX = Math.sqrt(Math.pow(accelX, 2)+Math.pow(accelZ, 2));
double hypotY = Math.sqrt(Math.pow(accelY, 2)+Math.pow(accelZ, 2));

double accelAngleX = Math.toDegrees(Math.asin(accelY/hypotY));
double accelAngleY = Math.toDegrees(Math.asin(accelX/hypotX));

if(dt > 0.01)
{

PX = errorX;
PY = errorY;

IX += errorX*dt;
IY += errorY*dt;

IX = 0.95*IX+0.05*accelAngleX;
IY = 0.95*IY+0.05*accelAngleY;

DX = (errorX - last_errorX)/dt;
DY = (errorY - last_errorY)/dt;

outputX = kP*PX+kI*IX+kD*DX;
outputY = kP*PY+kI*IY+kD*DY;
time = System.currentTimeMillis();
}

System.out.println(setpointX);

//System.out.println(val[0]+", "+val[1]+", "+val[2]+", "+val[3]);
{
BR.setPWM(0, val[0]);
TR.setPWM(0, val[1]);
TL.setPWM(0, val[2]);
BL.setPWM(0, val[3]);
} else
{
BR.setPWM(0, 1471);
TR.setPWM(0, 1471);
TL.setPWM(0, 1471);
BL.setPWM(0, 1471);
}

}
}

private static void add(double value, int i)
{
value = calculatePulseWidth(value/1000, FREQUENCY);
if(val[i]+value > 1471 && val[i]+value < 4071)
{
val[i] += value;
}else if(val[i]+value < 1471)
{
//System.out.println("low");
val[i] = 1471;
}else if(val[i]+value > 4071)
{
//System.out.println("low");
val[i] = 4071;
}
}

private static int calculatePulseWidth(double millis, int frequency) {
return (int) (Math.round(4096 * millis * frequency/1000));
}
}


Sensor.java:

package com.zachary.quadserver;

import com.pi4j.io.gpio.GpioController;
import com.pi4j.io.gpio.GpioFactory;
import com.pi4j.io.gpio.GpioPinDigitalOutput;
import com.pi4j.io.gpio.PinState;
import com.pi4j.io.gpio.RaspiPin;
import com.pi4j.io.i2c.*;
import com.pi4j.io.gpio.GpioController;
import com.pi4j.io.gpio.GpioFactory;
import com.pi4j.io.gpio.GpioPinDigitalOutput;
import com.pi4j.io.gpio.PinState;
import com.pi4j.io.gpio.RaspiPin;
import com.pi4j.io.i2c.*;

import java.net.*;
import java.io.*;

public class Sensor {
static I2CDevice sensor;
static I2CBus bus;
static byte[] accelData, gyroData;
static long accelCalib[] = new long[3];
static long gyroCalib[] = new long[3];

static double gyroX = 0;
static double gyroY = 0;
static double gyroZ = 0;

static double accelX;
static double accelY;
static double accelZ;

static double angleX;
static double angleY;
static double angleZ;

public Sensor() {
//System.out.println("Hello, Raspberry Pi!");
try {
bus = I2CFactory.getInstance(I2CBus.BUS_1);

sensor = bus.getDevice(0x68);

sensor.write(0x6B, (byte) 0x0);
sensor.write(0x6C, (byte) 0x0);
System.out.println("Calibrating...");

calibrate();

public void run(){
try {
} catch (IOException e) {
System.out.println(e.getMessage());
}
}
};
sensors.start();
} catch (IOException e) {
System.out.println(e.getMessage());
}
}

private static void readSensors() throws IOException {
long time = System.currentTimeMillis();
long sendTime = System.currentTimeMillis();

while (true) {
accelData = new byte[6];
gyroData = new byte[6];
int r = sensor.read(0x3B, accelData, 0, 6);
accelX = (((accelData[0] << 8)+accelData[1]-accelCalib[0])/16384.0)*9.8;
accelY = (((accelData[2] << 8)+accelData[3]-accelCalib[1])/16384.0)*9.8;
accelZ = ((((accelData[4] << 8)+accelData[5]-accelCalib[2])/16384.0)*9.8)+9.8;
accelZ = 9.8-Math.abs(accelZ-9.8);

double hypotX = Math.sqrt(Math.pow(accelX, 2)+Math.pow(accelZ, 2));
double hypotY = Math.sqrt(Math.pow(accelY, 2)+Math.pow(accelZ, 2));

double accelAngleX = Math.toDegrees(Math.asin(accelY/hypotY));
double accelAngleY = Math.toDegrees(Math.asin(accelX/hypotX));

//System.out.println((int)gyroX+", "+(int)gyroY);

//System.out.println("accelX: " + accelX+" accelY: " + accelY+" accelZ: " + accelZ);

r = sensor.read(0x43, gyroData, 0, 6);
if(System.currentTimeMillis()-time >= 5)
{
gyroX = (((gyroData[0] << 8)+gyroData[1]-gyroCalib[0])/131.0);
gyroY = (((gyroData[2] << 8)+gyroData[3]-gyroCalib[1])/131.0);
gyroZ = (((gyroData[4] << 8)+gyroData[5]-gyroCalib[2])/131.0);

angleX += gyroX*(System.currentTimeMillis()-time)/1000;
angleY += gyroY*(System.currentTimeMillis()-time)/1000;
angleZ += gyroZ;

angleX = 0.95*angleX + 0.05*accelAngleX;
angleY = 0.95*angleY + 0.05*accelAngleY;

time = System.currentTimeMillis();
}
//System.out.println((int)angleX+", "+(int)angleY);
//System.out.println((int)accelAngleX+", "+(int)accelAngleY);
}
}

public static void calibrate() throws IOException {
int i;
for(i = 0; i < 3000; i++)
{
accelData = new byte[6];
gyroData = new byte[6];
int r = sensor.read(0x3B, accelData, 0, 6);
accelCalib[0] += (accelData[0] << 8)+accelData[1];
accelCalib[1] += (accelData[2] << 8)+accelData[3];
accelCalib[2] += (accelData[4] << 8)+accelData[5];

r = sensor.read(0x43, gyroData, 0, 6);
gyroCalib[0] += (gyroData[0] << 8)+gyroData[1];
gyroCalib[1] += (gyroData[2] << 8)+gyroData[3];
gyroCalib[2] += (gyroData[4] << 8)+gyroData[5];
try {
} catch (Exception e){
e.printStackTrace();
}
}
gyroCalib[0] /= i;
gyroCalib[1] /= i;
gyroCalib[2] /= i;

accelCalib[0] /= i;
accelCalib[1] /= i;
accelCalib[2] /= i;
System.out.println(gyroCalib[0]+", "+gyroCalib[1]+", "+gyroCalib[2]);
}

{
switch (axis)
{
case 0:
return angleX;
case 1:
return angleY;
case 2:
return angleZ;
}

return 0;
}

{
switch (axis)
{
case 0:
return gyroX;
case 1:
return gyroY;
case 2:
return gyroZ;
}

return 0;
}

{
switch (axis)
{
case 0:
return accelX;
case 1:
return accelY;
case 2:
return accelZ;
}

return 0;
}
}


Edit:

I have re-written my code in C++ to see if it will run faster but it's still running at about the same speed(about 15 ms per cycle or about 66 Hz).

This is my new code in C++:

#include <wiringPi.h>
#include <wiringPiI2C.h>

#include <sys/socket.h>
#include <netinet/in.h>

#include <string.h>
#include <string>
#include <iostream>
#include <unistd.h>

#include <time.h>

#include <cmath>

#define axisX 0
#define axisY 1
#define axisZ 2

#define kP 20
#define kI 0
#define kD 0

#define FREQUENCY 490

#define MODE1 0x00
#define MODE2 0x01
#define PRESCALE 0xFE
#define LED0_ON_L 0x06
#define LED0_ON_H 0x07
#define LED0_OFF_L 0x08
#define LED0_OFF_H 0x09
#define ALL_LED_ON_L 0xFA
#define ALL_LED_ON_H 0xFB
#define ALL_LED_OFF_L 0xFC
#define ALL_LED_OFF_H 0xFD

// Bits
#define RESTART 0x80
#define SLEEP 0x10
#define ALLCALL 0x01
#define INVRT 0x10
#define OUTDRV 0x04

#define BILLION 1000000000L

using namespace std;

double accelCalX = 0;
double accelCalY = 0;
double accelCalZ = 0;

double gyroCalX = 0;
double gyroCalY = 0;
double gyroCalZ = 0;

double PX;
double PY;

double IX = 0;
double IY = 0;

double DX;
double DY;

double lastErrorX;
double lastErrorY;

int throttle = 1471;

int sensor = wiringPiI2CSetup(0x68);
int pwm = wiringPiI2CSetup(0x40);

array<int,4> motorVal;

struct timespec now, then;

int toSigned(int unsignedVal)
{
int signedVal = unsignedVal;
if(unsignedVal > 32768)
{
signedVal = -(32768-(unsignedVal-32768));
}
return signedVal;
}

double getAccel(int axis)
{

X -= accelCalX;
Y -= accelCalY;
Z -= accelCalZ;
Z = 9.8-abs(Z-9.8);

switch(axis)
{
case axisX:
return X;
case axisY:
return Y;
case axisZ:
return Z;
}
}

double getGyro(int axis)
{

X -= gyroCalX;
Y -= gyroCalY;
Z -= gyroCalZ;

switch(axis)
{
case axisX:
return X;
case axisY:
return Y;
case axisZ:
return Z;
}
}

void calibrate()
{
int i;
for(i = 0; i < 1500; i++)
{

usleep(1000);
}

accelCalX /= i;
accelCalY /= i;
accelCalZ /= i;
accelCalZ -= 9.8;

gyroCalX /= i;
gyroCalY /= i;
gyroCalZ /= i;

cout << accelCalX << " " << accelCalY << " " << accelCalZ << "\n";
}

int calculatePulseWidth(double millis, int frequency) {
return (int)(floor(4096 * millis * frequency/1000));
}

{
value = calculatePulseWidth(value/1000, FREQUENCY);
if(motorVal[i]+value > 1471 && motorVal[i]+value < 4071)
{
motorVal[i] += value;
}else if(motorVal[i]+value < 1471)
{
//System.out.println("low");
motorVal[i] = 1471;
}else if(motorVal[i]+value > 4071)
{
//System.out.println("low");
motorVal[i] = 4071;
}
}

void getThrottle()
{
int sockfd,n;
socklen_t len;
char mesg[1000];

sockfd=socket(AF_INET,SOCK_DGRAM,0);

while(true)
{
mesg[n] = 0;
string message(mesg);
string values[5];

int valIndex = 0;
int lastIndex = 0;
for(int i = 0; i < message.length(); i++)
{
if(message[i] == ' ')
{
values[valIndex] = message.substr(lastIndex+1, i);
lastIndex = i;
valIndex++;
}
}
values[valIndex] = message.substr(lastIndex+1, message.length());

throttle = calculatePulseWidth(((stoi(values[4])*12.96)+733)/1000, FREQUENCY);
}
}

void setAllPWM(int on, int off) {
wiringPiI2CWriteReg8(pwm, ALL_LED_ON_L, (on & 0xFF));
wiringPiI2CWriteReg8(pwm, ALL_LED_ON_H, (on >> 8));
wiringPiI2CWriteReg8(pwm, ALL_LED_OFF_L, (off & 0xFF));
wiringPiI2CWriteReg8(pwm, ALL_LED_OFF_H, (off >> 8));
}

void setPWM(int on, int off, int channel) {
wiringPiI2CWriteReg8(pwm, LED0_ON_L + 4 * channel, (on & 0xFF));
wiringPiI2CWriteReg8(pwm, LED0_ON_H + 4 * channel, (on >> 8));
wiringPiI2CWriteReg8(pwm, LED0_OFF_L + 4 * channel, (off & 0xFF));
wiringPiI2CWriteReg8(pwm, LED0_OFF_H + 4 * channel, (off >> 8));
}

void setPWMFrequency(double frequency) {
double prescaleval = 25000000.0;
prescaleval /= 4096.0;
prescaleval /= frequency;
prescaleval -= 1.0;
double prescale = floor(prescaleval + 0.5);
int newmode = (oldmode & 0x7F) | 0x10;
wiringPiI2CWriteReg8(pwm, MODE1, newmode);
wiringPiI2CWriteReg8(pwm, PRESCALE, (floor(prescale)));
wiringPiI2CWriteReg8(pwm, MODE1, oldmode);
usleep(50000);
wiringPiI2CWriteReg8(pwm, MODE1, (oldmode | 0x80));
}

void initSensor()
{
wiringPiI2CWriteReg8(sensor, 0x6B, 0x0);
wiringPiI2CWriteReg8(sensor, 0x6C, 0x0);
}

void initPWM()
{
setAllPWM(0, 0);
wiringPiI2CWriteReg8(pwm, MODE2, OUTDRV);
wiringPiI2CWriteReg8(pwm, MODE1, ALLCALL);
usleep(50000);
mode1 = mode1 & ~SLEEP;
wiringPiI2CWriteReg8(pwm, MODE1, mode1);
usleep(50000);
setPWMFrequency(FREQUENCY);
}

double millis(timespec time)
{
return (time.tv_sec*1000)+(time.tv_nsec/1.0e6);
}

double intpow( double base, int exponent )
{
int i;
double out = base;
for( i=1 ; i < exponent ; i++ )
{
out *= base;
}
return out;
}

int main (void)
{

initSensor();
initPWM();

cout << "Calibrating..." << "\n";
calibrate();

clock_gettime(CLOCK_MONOTONIC, &then);

while(true)
{
motorVal.fill(throttle);

clock_gettime(CLOCK_MONOTONIC, &now);

double dt = (millis(now)-millis(then))/1000;

then = now;

double accelX = getAccel(0);
double accelY = getAccel(1);
double accelZ = getAccel(2);

double hypotX = sqrt(intpow(accelX, 2)+intpow(accelZ, 2));
double hypotY = sqrt(intpow(accelY, 2)+intpow(accelZ, 2));

double accelAngleX = (180/3.14)*(asin(accelY/hypotY));
double accelAngleY = (180/3.14)*(asin(accelX/hypotX));

double errorX = -getGyro(0);
double errorY = getGyro(1);

PX = errorX;
PY = errorY;

IX += errorX*dt;
IY += errorY*dt;

IX = 0.95*IX+0.05*accelAngleX;
IY = 0.95*IY+0.05*accelAngleY;

DX = (errorX-lastErrorX)*dt;
DY = (errorY-lastErrorY)*dt;

lastErrorX = errorX;
lastErrorY = errorY;

double outputX = kP*PX+kI*IX+kD*DX;
double outputY = kP*PY+kI*IY+kD*DY;

setPWM(0, motorVal[0], 12);
setPWM(0, motorVal[1], 13);
setPWM(0, motorVal[2], 14);
setPWM(0, motorVal[3], 15);
}
}


In addition two of the motors seem like they are lagging when I turn the quadcopter fast in one direction. Also for some strange reason the quadcopter seems less responsive to P gain; I have it at 20 in the C++ version and it is working about the same as when I had it at 1.5 in the java version.

Edit:

After doing some more testing I have determined that reading from the MPU6050 and writing to the PCA9685 board that I am using to control the ESCs is the source of the delay. Does anybody know how to speed this up?

Edit:

I managed to speed up my code to about 200 Hz by changing the i2c baud rate, but the quadcopter is still thrashing. I have spent hours trying to tune the pid controller, but it doesn't seem to help at all.

• My intention in suggesting that you post here assumed you'd also take the suggestion of analyzing data and narrowing down your problem seriously first. Reiterating from the original: There is no way to know given the information provided. Tuning PID controllers is an art, your problem could be there. Incorrect controller implementation could also be an issue. Bad sensors, motor problems, or any number of other things subtle or not-so-subtle could also be the cause. You need to perform some more tests, gather and analyze data, and narrow down your problem. – Jason C Mar 4 '15 at 21:48
• For a good example of formulating a question after research see robotics.stackexchange.com/questions/2167/… – Jason C Mar 4 '15 at 21:51
• have you considered how quick your code is? Java is not the fastest platform so the code may not run fast enough to capture the error/perturbation induced by the speed given by the higher throttle.... a typical commercial quadcopter corrects itself 400 times/sec or so... – dm76 Mar 4 '15 at 23:20
• I would guess either the PID tuning makes the quadcopter overreact at high throttle settings, or as @dm76 pointed out, your control system does not refresh fast enough. Time each iteration of your control system and make sure it is above 100 (that's what my controls professor suggested when I was working on a quad). I would read your gyro as fast as possible and average the readings rather than wait 5 ms for each one (at least). – Mewa Mar 6 '15 at 18:22
• what do you mean by above 100? 100 what? – Zachary Schroeder Mar 7 '15 at 5:36

So, I figured out that the problem was vibration. I first tried using a running average to smooth the data that I was getting from my MPU, but the delay that it created made it oscillate a lot, so I tried a low pass filter instead, which worked GREAT! Thank you to everyone for your help!

Rasbian on Raspberry pi is not real time OS, when u try to read some value due to some processing factors there is a chance that values are read late and thus the overall real-time control does not occur which is very crucial for a quadcopter project. So it would be better to connect the mpu6050 and pwm out to a microcontroller rather than using Pi. You can use real time OS on Raspberry pi like FreeRTOS and ChibiOS/RT. Or instead attach a real-time hardware to serial port to make it real-time.

But for your case the trashing maybe caused due to incorrect PID values. Maybe you for got to put a -ve sign somewhere in the code. Please check this article clearly to know the correct -ve sign placements in code : How to build your own Quadcopter's Flight Controller.

But from my experience I would suggest you to trash Raspberry Pi for Quadcopter project since it might get heavy once u use a separate real-time unit, microcontroller and GPIO extensions for using more PWM pins. So instead of adding all those hardwares I would prefer you to use BeagleBone Back since it has an inbuilt Programmable real-time units that is you can use the 1GHz BBB processor for control logic and also its 2 independent PRU's to read PPM values from RC receiver and send PWM values to ESC's and it also has enough GPIO pins to read PPM and send PWM outputs to upto 12 pins. Check out erle robot Project which is an Open Hardware/Open Source quadcopter project using BBB running on ROS.

Actually, Rasbian can be turned into a real time OS. Before switching to ChibiOS and the like, you could give a try to Linux RT patch. There is a simple tutorial how to set up a RT kernel on RPi.

If the issue you're observing is caused by the scheduler jitter, the problem might go away.

I've managed to run a copter on 100Hz without a significant jitter.

You are using a PID incorrectly. You are reading sensor values that in terms of acceleration, but you are using that to correct an error in velocity (without conversion). Your PID should be reading and controlling the same measurement -- velocity vs velocity, acceleration vs acceleration, or position vs position.

The behavior you are seeing can be explained simply by $$a = \frac{dv}{dt}$$

At low velocities, $dv$ can only be as high as your maximum velocity, so your system appears to work properly within that bound. But as you increase the maximum velocity, your acceleration can spike much higher.

This is simply a logic error in your code played out in physical space.

• This may be a stupid question, so forgive me, but it seems to me that I am using the angular velocity from the gyro to control the correction of the motors. So I'm not using acceleration to control velocity, I'm using angular velocity to control angular velocity. I'm sorry if I have misunderstood your answer, and please correct me if I am wrong. – Zachary Schroeder Mar 13 '15 at 14:56
• On which axis is/was it thrashing? I assumed the Z axis. – Ian Mar 13 '15 at 21:11

I hope this won't sound too harsh but from my experience building a quadcopter and asking questions here this is exactly the kind of question which won't get you an answer that you'll find helpful. From what you describe there are far too many potential points of failures in your system for the community to contribute effectively. The people who have already commented with PID tuning tips or questions about the speed of your loop are making the huge assumption that everything else you have not talked about works.

I would suggest you test the different software modules independently and try to narrow down the problem. I appreciate you said you tested your sensors independently and they worked fine. Is that all you tested? How well does your attitude estimation algorithm work? What's its precision and its response time like? Does it still work when the motors spin (is it vibrations tolerant)? Can you control your motors directly (not through the PID)? What's their response time? Any glitches? What's the structure of your PID controllers: angle stabilisation, rate stabilisation, cascaded rate then angle stabilisation? Chances are you'll find the solution to your problem while answering one of these questions. And even if you don't you would then be able to ask a much more precise and answerable question.

Best of luck with your project in any case: quadcopters are hard!