Hi everyone,
I am trying to get a self-buil drone working with an Arduino Nano. I use PWM to speak to 4 Motors. After unsucessfully trying to get PID constants right, I started plotting some data of my angles, obtained by the MPU. The Pitch angle seems fine but the roll angle is ALWAYS breaking out into negative territory, as seen in the picture
(angles in 10*degrees, blue = roll, red = pitch; angle speeds in degrees per secong, green = roll, yellow = pitch)
I am using a complementary filter and median filters on raw data. I really don't understand how only one axis can be so heavily effected by this.
My complete Code:
#include <Wire.h>
#define calibrateAtStart false
#define sampleSize 9
#define MAX_INT 32768
#define MOTOR1 3
#define MOTOR2 9
#define MOTOR3 10
#define MOTOR4 11
//PID can't exceed MAX_INT! Roll and pitch PID also can't exceed pwm (255) together. Maximum at 255/4~60
//kp*450 + ki*1 000 000 + kd*5000 < 6 000 < 32 768
#define kp 3
#define ki 0.002
#define kd 7
//!!!!!!!!Set to 0 before plugging in! Drone might take off uncontrolably!!!!!!!!!!!
int testTimer = 0;
long prevTime;
int elapsedTime; //in milliseconds
int omega[3]; //rotation speeds
int phi[3]; //absolute angles
float I[3]; //Integral has to be global to be increased each step
int thrust;
//Offset Values
int8_t errX, errY, errZ, errOmegaX, errOmegaY, errOmegaZ;
//filters
int8_t oldestValue = 0;
int omegaBuffer[3][sampleSize];
int accBuffer[3][sampleSize];
void setup() {
thrust = 65;
Serial.begin(9600);
Wire.begin(); //begin the wire comunication
Wire.beginTransmission(0x68); //begin, Send the slave adress (in this case 68)
Wire.write(0x6B); //make the reset (place a 0 into the 6B register)
Wire.write(0x00);
Wire.endTransmission(true); //end the transmission
//Gyro config
Wire.beginTransmission(0x68); //begin, Send the slave adress (in this case 68)
Wire.write(0x1B); //We want to write to the GYRO_CONFIG register (1B hex)
Wire.write(0x10); //Set the register bits as 00010000 (1000dps full scale)
Wire.endTransmission(true); //End the transmission with the gyro
//Acc config
Wire.beginTransmission(0x68); //Start communication with the address found during search.
Wire.write(0x1C); //We want to write to the ACCEL_CONFIG register
Wire.write(0x10); //Set the register bits as 00010000 (+/- 8g full scale range)
Wire.endTransmission(true);
//----------prefill median filters----------//
for(int i=0; i<sampleSize; i++) {
Wire.beginTransmission(0x68); //begin, Send the slave adress (in this case 68)
Wire.write(0x43); //First adress of the Gyro data
Wire.endTransmission(false);
Wire.requestFrom(0x68,6,true); //We ask for just 4 registers
omegaBuffer[0][i] = (Wire.read()<<8|Wire.read()) - errOmegaX;
omegaBuffer[1][i] = (Wire.read()<<8|Wire.read()) - errOmegaY;
//fOmega[2][i] = (Wire.read()<<8|Wire.read()) - errOmegaZ;
Wire.beginTransmission(0x68);
Wire.write(0x3B); //Ask for the 0x3B register- correspond to AcX
Wire.endTransmission(false);
Wire.requestFrom(0x68,6,true);
accBuffer[0][i] = (Wire.read()<<8|Wire.read()) - errX;
accBuffer[1][i] = (Wire.read()<<8|Wire.read()) - errY;
accBuffer[2][i] = (Wire.read()<<8|Wire.read()) - errZ;
}
}
void loop() {
elapsedTime = millis() - prevTime;
prevTime = millis();
processMPUData(true);
executeFCS(false);
}
int getMedian(int buffer[], int newValue) {
buffer[oldestValue] = newValue;
int sortedArray[sampleSize];
memcpy(sortedArray, buffer, sizeof(sortedArray));
quickSort(sortedArray, 0, sampleSize-1);
return sortedArray[(sampleSize-1)/2];
}
void quickSort(int arr[], int left, int right) {
int i = left, j = right;
int tmp;
int pivot = arr[(left + right) / 2];
//partition
while (i <= j) {
while (arr[i] < pivot)
i++;
while (arr[j] > pivot)
j--;
if (i <= j) {
tmp = arr[i];
arr[i] = arr[j];
arr[j] = tmp;
i++;
j--;
}
};
//recursion
if (left < j)
quickSort(arr, left, j);
if (i < right)
quickSort(arr, i, right);
}
void processMPUData(boolean print) {
//----------Gyro----------//
Wire.beginTransmission(0x68); //begin, Send the slave adress (in this case 68)
Wire.write(0x43); //First adress of the Gyro data
Wire.endTransmission(false);
Wire.requestFrom(0x68,4,true); //We ask for just 4 registers
//In 10*degrees per seconds (dps) Max: 10 000
while(Wire.available() < 4);
omega[0] = (Wire.read()<<8|Wire.read()) - errOmegaX;
omega[1] = (Wire.read()<<8|Wire.read()) - errOmegaY;
//apply median filter and scaling
omega[0] = getMedian(omegaBuffer[0], omega[0])/3.28;
omega[1] = getMedian(omegaBuffer[1], omega[1])/3.28;
//----------Accelerometer---------//
Wire.beginTransmission(0x68); //begin, Send the slave adress (in this case 68)
Wire.write(0x3B); //Ask for the 0x3B register- correspond to AcX
Wire.endTransmission(false); //keep the transmission and next
Wire.requestFrom(0x68,6,true); //We ask for next 6 registers starting with the 3B
//For a range of 8g divide by 4096
while(Wire.available() < 6);
int rawX = (Wire.read()<<8|Wire.read()) - errX;
int rawY = (Wire.read()<<8|Wire.read()) - errY;
int rawZ = (Wire.read()<<8|Wire.read()) - errZ;
//apply median filter
rawX = getMedian(accBuffer[0], rawX);
rawY = getMedian(accBuffer[1], rawY);
rawZ = getMedian(accBuffer[2], rawZ);
//Apply Eulers Formula (180/pi*100)
//In 10*degrees Max: 450
int rawRoll = atan( rawY/sqrt((long)rawX*rawX + (long)rawZ*rawZ))*1800/M_PI;
int rawPitch = atan(-rawX/sqrt((long)rawY*rawY + (long)rawZ*rawZ))*1800/M_PI;
//----------Complementary Filter----------//
phi[0] = 0.98*(phi[0] + omega[0]*elapsedTime/1000) + 0.02*rawRoll;
phi[1] = 0.98*(phi[1] + omega[1]*elapsedTime/1000) + 0.02*rawPitch;
//update oldest value index//
oldestValue++;
if (oldestValue == sampleSize) {
oldestValue = 0;
}
//----------Print----------//
if (print) {
Serial.print(phi[0]); //blue = X (degrees*10)
Serial.print(",");
Serial.print(phi[1]); //red = Y
Serial.print(",");
Serial.print(omega[0]/10); //green = X (dps)
Serial.print(",");
Serial.println(omega[1]/10); //yellow = Y
}
}
void executeFCS(boolean print) {
...
}