I have recently built a raspberry pi based quadcopter that communicates with my tablet over wifi. The problem is that it drifts a lot. At first I thought that the problem was vibration, so I mounted the MPU-6050 more securely to the frame. That seemed to help a bit, but it still drifts. I have tried tuning the PID, tuning the complementary filter, and installing a real time OS. Nothing seems to help very much. Below is my code written completely in java. Any suggestions are appreciated.
QuadServer.java:
package com.zachary.quadserver;
import java.net.*;
import java.io.*;
import java.util.*;
import com.pi4j.io.i2c.I2CBus;
import com.pi4j.io.i2c.I2CDevice;
import com.pi4j.io.i2c.I2CFactory;
import se.hirt.pi.adafruit.pwm.PWMDevice;
import se.hirt.pi.adafruit.pwm.PWMDevice.PWMChannel;
public class QuadServer {
private final static int FREQUENCY = 490;
private static final int MIN = 740;
private static final int MAX = 2029;
private static Sensor sensor = new Sensor();
private static double PX = 0;
private static double PY = 0;
private static double PZ = 0;
private static double IX = 0;
private static double IY = 0;
private static double IZ = 0;
private static double DX = 0;
private static double DY = 0;
private static double DZ = 0;
private static double kP = 1.95; //2.0
private static double kI = 10.8; //8.5
private static double kD = 0.15; //0.14
private static long time = System.currentTimeMillis();
private static double last_errorX = 0;
private static double last_errorY = 0;
private static double last_errorZ = 0;
private static double outputX;
private static double outputY;
private static double outputZ;
private static int val[] = new int[4];
private static int throttle;
static double setpointX = 0;
static double setpointY = 0;
static double setpointZ = 0;
static double errorX;
static double errorY;
static double errorZ;
static long receivedTime = System.currentTimeMillis();
private static String data;
static int trimX = -70;
static int trimY = 70;
public static void main(String[] args) throws IOException, NullPointerException {
DatagramSocket serverSocket = new DatagramSocket(40002);
PWMDevice device = new PWMDevice();
device.setPWMFreqency(FREQUENCY);
PWMChannel esc0 = device.getChannel(0);
PWMChannel esc1 = device.getChannel(1);
PWMChannel esc2 = device.getChannel(2);
PWMChannel esc3 = device.getChannel(3);
/*Runtime.getRuntime().addShutdownHook(new Thread(new Runnable() {
public void run() {
System.out.println("terminating");
try {
esc0.setPWM(0, calculatePulseWidth(MIN/1000.0, FREQUENCY));
esc1.setPWM(0, calculatePulseWidth(MIN/1000.0, FREQUENCY));
esc2.setPWM(0, calculatePulseWidth(MIN/1000.0, FREQUENCY));
esc3.setPWM(0, calculatePulseWidth(MIN/1000.0, FREQUENCY));
} catch (IOException e) {
e.printStackTrace();
}
}
}));
System.out.println("running");*/
Thread read = new Thread(){
public void run(){
while(true) {
try {
byte receiveData[] = new byte[1024];
DatagramPacket receivePacket = new DatagramPacket(receiveData, receiveData.length);
serverSocket.receive(receivePacket);
String message = new String(receivePacket.getData());
data = ""+IX;
addData(IY);
addData(sensor.readAccelAngle(0));
addData(sensor.readAccelAngle(1));
byte[] sendData = new byte[1024];
sendData = data.getBytes();
InetAddress IPAddress = InetAddress.getByName("192.168.1.9");
DatagramPacket sendPacket = new DatagramPacket(sendData, sendData.length, IPAddress, 1025);
serverSocket.send(sendPacket);
setpointX = Double.parseDouble(message.split("\\s+")[0])*0.7;
setpointY = Double.parseDouble(message.split("\\s+")[1])*0.7;
throttle = (int)(Integer.parseInt((message.split("\\s+")[3]))*12.67)+MIN;
kP = Math.round((Integer.parseInt(message.split("\\s+")[4])*0.05)*1000.0)/1000.0;
kI = Math.round((Integer.parseInt(message.split("\\s+")[5])*0.2)*1000.0)/1000.0;
kD = Math.round((Integer.parseInt(message.split("\\s+")[6])*0.01)*1000.0)/1000.0;
trimX = (Integer.parseInt(message.split("\\s+")[7])-50)*2;
trimY = (Integer.parseInt(message.split("\\s+")[8])-50)*2;
double accelSmoothing = 0.02;//(Integer.parseInt(message.split("\\s+")[8])*0.05)+1;
double gyroSmoothing = 0.04;//(Integer.parseInt(message.split("\\s+")[7])*0.01);
sensor.setSmoothing(gyroSmoothing, accelSmoothing);
//System.out.println("trimX: "+trimX+" trimY: "+trimY);
System.out.println("kP: "+kP+", kI: "+kI+", kD: "+kD+", trimX: "+trimX+", trimY: "+trimY);
receivedTime = System.currentTimeMillis();
} catch (IOException e) {
e.printStackTrace();
}
}
}
};
read.start();
while(true)
{
Arrays.fill(val, throttle);
errorX = sensor.readGyro(0)-setpointX;
errorY = -sensor.readGyro(1)-setpointY;
errorZ = sensor.readGyro(2)-setpointZ;
double dt = (double)(System.currentTimeMillis()-time)/1000;
double accelAngleX = sensor.readAccelAngle(0);
double accelAngleY = sensor.readAccelAngle(1);
if(dt > 0.005)
{
PX = errorX;
PY = errorY;
PZ = errorZ;
IX += (errorX)*dt;
IY += (errorY)*dt;
//IZ += errorZ*dt;
IX = 0.98*IX+0.02*accelAngleX;
IY = 0.98*IY+0.02*accelAngleY;
DX = (errorX - last_errorX)/dt;
DY = (errorY - last_errorY)/dt;
//DZ = (errorZ - last_errorZ)/dt;
last_errorX = errorX;
last_errorY = errorY;
last_errorZ = errorZ;
outputX = kP*PX+kI*IX+kD*DX;
outputY = kP*PY+kI*IY+kD*DY;
outputZ = kP*PZ+kI*IZ+kD*DZ;
time = System.currentTimeMillis();
}
//System.out.println(IX+", "+IY+", "+throttle);
add(-outputX-outputY-outputZ-trimX+trimY, 0); //clockwise
add(-outputX+outputY+outputZ-trimX-trimY, 1); //counterClockwise
add(outputX+outputY-outputZ+trimX-trimY, 2); //clockwise
add(outputX-outputY+outputZ+trimX+trimY, 3); //counterclockwise
//System.out.println(val[0]+", "+val[1]+", "+val[2]+", "+val[3]);
try {
if(System.currentTimeMillis()-receivedTime < 1000)
{
esc0.setPWM(0, calculatePulseWidth(val[0]/1000.0, FREQUENCY));
esc1.setPWM(0, calculatePulseWidth(val[1]/1000.0, FREQUENCY));
esc2.setPWM(0, calculatePulseWidth(val[2]/1000.0, FREQUENCY));
esc3.setPWM(0, calculatePulseWidth(val[3]/1000.0, FREQUENCY));
} else
{
esc0.setPWM(0, calculatePulseWidth(800/1000.0, FREQUENCY));
esc1.setPWM(0, calculatePulseWidth(800/1000.0, FREQUENCY));
esc2.setPWM(0, calculatePulseWidth(800/1000.0, FREQUENCY));
esc3.setPWM(0, calculatePulseWidth(800/1000.0, FREQUENCY));
}
} catch (IOException e) {
e.printStackTrace();
}
}
}
private static void add(double value, int i)
{
if(val[i]+value > MIN && val[i]+value < MAX)
{
val[i] += value;
}else if(val[i]+value < MIN)
{
//System.out.println("low");
val[i] = MIN;
}else if(val[i]+value > MAX)
{
//System.out.println("low");
val[i] = MAX;
}
}
static void addData(double value)
{
data += " "+value;
}
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 java.net.*;
import java.io.*;
public class Sensor {
static I2CDevice sensor;
static I2CBus bus;
static byte[] accelData, gyroData;
static long accelCalib[] = {0, 0, 0};
static long gyroCalib[] = {0, 0, 0};
static double gyroX;
static double gyroY;
static double gyroZ;
static double smoothedGyroX;
static double smoothedGyroY;
static double smoothedGyroZ;
static double accelX;
static double accelY;
static double accelZ;
static double accelAngleX;
static double accelAngleY;
static double smoothedAccelAngleX;
static double smoothedAccelAngleY;
static double angleX;
static double angleY;
static double angleZ;
static boolean init = true;
static double accelSmoothing = 1;
static double gyroSmoothing = 1;
public Sensor() {
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();
Thread sensors = new Thread(){
public void run(){
try {
readSensors();
} catch (IOException e) {
e.printStackTrace();
}
}
};
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));
accelAngleX = Math.toDegrees(Math.asin(accelY/hypotY));
accelAngleY = Math.toDegrees(Math.asin(accelX/hypotX));
//System.out.println(accelAngleX[0]+" "+accelAngleX[1]+" "+accelAngleX[2]+" "+accelAngleX[3]);
//System.out.println("accelX: " + accelX+" accelY: " + accelY+" accelZ: " + accelZ);
r = sensor.read(0x43, gyroData, 0, 6);
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);
if(init)
{
smoothedAccelAngleX = accelAngleX;
smoothedAccelAngleY = accelAngleY;
smoothedGyroX = gyroX;
smoothedGyroY = gyroY;
smoothedGyroZ = gyroZ;
init = false;
} else {
smoothedAccelAngleX = smoothedAccelAngleX+(accelSmoothing*(accelAngleX-smoothedAccelAngleX));
smoothedAccelAngleY = smoothedAccelAngleY+(accelSmoothing*(accelAngleY-smoothedAccelAngleY));
smoothedGyroX = smoothedGyroX+(gyroSmoothing*(gyroX-smoothedGyroX));
smoothedGyroY = smoothedGyroY+(gyroSmoothing*(gyroY-smoothedGyroY));
smoothedGyroZ = smoothedGyroZ+(gyroSmoothing*(gyroZ-smoothedGyroZ));
/*smoothedAccelAngleX = accelAngleX;
smoothedAccelAngleY = accelAngleY;
smoothedGyroX = gyroX;
smoothedGyroY = gyroY;
smoothedGyroY = gyroY;*/
/*smoothedAccelAngleX += (accelAngleX-smoothedAccelAngleX)/accelSmoothing;
smoothedAccelAngleY += (accelAngleY-smoothedAccelAngleY)/accelSmoothing;
smoothedGyroX += (gyroX-smoothedGyroX)/gyroSmoothing;
smoothedGyroY += (gyroY-smoothedGyroY)/gyroSmoothing;
smoothedGyroZ += (gyroZ-smoothedGyroZ)/gyroSmoothing;*/
}
angleX += smoothedGyroX*(System.currentTimeMillis()-time)/1000;
angleY += smoothedGyroY*(System.currentTimeMillis()-time)/1000;
angleZ += smoothedGyroZ;
angleX = 0.95*angleX + 0.05*smoothedAccelAngleX;
angleY = 0.95*angleY + 0.05*smoothedAccelAngleY;
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 < 100; 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 {
Thread.sleep(1);
} 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]);
System.out.println(accelCalib[0]+", "+accelCalib[1]+", "+accelCalib[2]);
}
public double readAngle(int axis)
{
switch (axis)
{
case 0:
return angleX;
case 1:
return angleY;
case 2:
return angleZ;
}
return 0;
}
public double readGyro(int axis)
{
switch (axis)
{
case 0:
return smoothedGyroX;
case 1:
return smoothedGyroY;
case 2:
return smoothedGyroZ;
}
return 0;
}
public double readAccel(int axis)
{
switch (axis)
{
case 0:
return accelX;
case 1:
return accelY;
case 2:
return accelZ;
}
return 0;
}
public double readAccelAngle(int axis)
{
switch (axis)
{
case 0:
return smoothedAccelAngleX;
case 1:
return smoothedAccelAngleY;
}
return 0;
}
public void setSmoothing(double gyro, double accel)
{
gyroSmoothing = gyro;
accelSmoothing = accel;
}
}