Hello,
I'm working on a quadcopter, which is controlled by means of a visual studio application. I'm making use of those $5 transmitter/receiver to communicate between the PC and the quadcopter. I'm using the MPU-6050 to obtain the Euler angles. Everything worked out well when i coded the wireless transmission, getting euler angles of the DMP of mpu-6050, but when i tried combining the two, FIFO overflow keeps occurring. I understand that I'm not taking data off the fifobuffer, as frequently as it requires, or i'm taking data at a much slower rate.
How do i time my intervals when i take data off the MPU-6050 such that this overflow doesn't occur?
Here's the code,
#include <VirtualWire.h>
#include <ServoTimer2.h>
#include <PID_v1.h>
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu;
ServoTimer2 motor1,motor2,motor3,motor4;
int j=0,pwm,a,b;
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
float euler[3]; // [psi, theta, phi] Euler angle container
Quaternion q; // [w, x, y, z] quaternion container
// PID control variables and initialisation
//0.38 0.08 0.1
float conskp=.38, conski=.08, conskd=.1, aggkp=.38, aggki=.08, aggkd=.1;
/*conskp=.38;
conski=.08;
conskd=.1;
aggkp=.38;
aggki=.08;
aggkd=.1;*/
double setPoint_pitch, input_pitch, output_pitch;
PID pid_pitch(&input_pitch, &output_pitch, &setPoint_pitch, conskp, conski, conskd, DIRECT);
//*******INTERRUPT DETECTION SUB-ROUTINE***********//
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
{
mpuInterrupt = true;
}
void setup()
{
motor1.attach(9);
motor2.attach(6);
motor3.attach(5);
motor4.attach(7);
pinMode(13,OUTPUT);
pinMode(12,OUTPUT);
pinMode(3,OUTPUT);
//****************************PID INITIALISING CODE*******************//
pid_pitch.SetOutputLimits(-20,20);
pid_pitch.SetMode(AUTOMATIC);
setPoint_pitch=0;
//*******************ASK receiver INITIALISING CODE*******************//
vw_set_rx_pin(11);
vw_set_ptt_inverted(true);
vw_setup(2000);
vw_rx_start(); //start the PLL running
//*******************MPU-6050 INITIALISING CODE*******************//
Wire.begin();
Serial.begin(115200);
mpu.initialize();
while (Serial.available() && Serial.read());// empty buffer
devStatus = mpu.dmpInitialize();// load and configure DMP
if (devStatus == 0) {
mpu.setDMPEnabled(true); // turn on the DMP, now that it's ready
// enable Arduino interrupt detection
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
}
else
Serial.print("DMP initialization failed");
}
void loop()
{
digitalWrite(12,HIGH);
if (!dmpReady) return;
while (!mpuInterrupt && fifoCount < packetSize)
{
}
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
}
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetEuler(euler, &q);
Serial.print("euler\t");
Serial.print(euler[0] * 180/M_PI);
Serial.print("\t");
Serial.print(euler[1] * 180/M_PI);
Serial.print("\t");
Serial.println(euler[2] * 180/M_PI);
// transmission();
// if(j==5)
// hover();
}
void transmission()
{
uint8_t buf[VW_MAX_MESSAGE_LEN],i=0;
uint8_t buflen = VW_MAX_MESSAGE_LEN;
char msg[10],starter1[2]="a",starter2[2]="b",starter3[2]="c",starter4[2]="d",starter5[2]="e";
//
if (vw_get_message(buf, &buflen))
{
digitalWrite(13,HIGH);
//pwm=atoi(buf);
for(i=0;i<buflen;i++)
msg[i]=buf[i];
msg[i]='\0';
if(strcmp(msg,starter1)==0)
start1();
else if(strcmp(msg,starter2)==0)
start2();
else if(strcmp(msg,starter3)==0)
start3();
else if(strcmp(msg,starter4)==0)
start4();
else if(strcmp(msg,starter5)==0)
start5();
digitalWrite(13,LOW);
}
switch(j)
{
case 1:
digitalWrite(2,HIGH);
pwm=atoi(msg);
Serial.println(pwm);
motor1.write(pwm);
digitalWrite(2,LOW);
break;
case 2:
digitalWrite(3,HIGH);
pwm=atoi(msg);
Serial.println(pwm);
motor2.write(pwm);
digitalWrite(3,LOW);
break;
case 3:
digitalWrite(3,HIGH);
pwm=atoi(msg);
Serial.println(pwm);
motor3.write(pwm);
digitalWrite(3,LOW);
break;
case 4:
digitalWrite(3,HIGH);
pwm=atoi(msg);
Serial.println(pwm);
motor4.write(pwm);
digitalWrite(3,LOW);
break;
case 5:
digitalWrite(3,HIGH);
pwm=atoi(msg);
a=pwm;
b=pwm;
hover();
digitalWrite(3,LOW);
}
}
/*void angle()
{
}*/
void start1()
{
j=1;
delay(20);
}
void start2()
{
j=2;
delay(20);
}
void start3()
{
j=3;
delay(20);
}
void start4()
{
j=4;
delay(20);
}
void start5()
{
j=5;
delay(20);
}
void hover()
{
float pitch_gap= abs(setPoint_pitch-euler[1]);
if(pitch_gap<3)
pid_pitch.SetTunings(conskp,conski,conskd);
else
pid_pitch.SetTunings(aggkp,aggki,aggkd);
pid_pitch.Compute();
if(output_pitch>0)
{
a++;
motor4.write(a);
}
else
{
b++;
motor2.write(b);
}
Serial.print(a);
Serial.print("\t");
Serial.println(b);
}