Increase freq, with MPU6050

Hi,

I’m doing a quadcopter with MPU6050 but my code seems a bit slow, a loop takes 100ms! Is it enough for a quadcopter? How could I increase it?

#ifndef QUADARDU
#define QUADARDU

#include <Servo.h>
#include <Wire.h>
#include <I2Cdev.h>
#include <helper_3dmath.h>
#include <MPU6050_6Axis_MotionApps20.h>
#include <PID_v1.h>
#include <PinChangeInt.h>
#include <PinChangeIntConfig.h>



/*  Arduino Pin configuration
 *  
 */

#define ESC_A 26
#define ESC_B 22
#define ESC_C 28
#define ESC_D 24

#define RC_1 46
#define RC_2 44 
#define RC_3 42
#define RC_4 48
#define RC_5 8
#define RC_PWR A0


/* ESC configuration
 *
 */

#define ESC_MIN 20
#define ESC_MAX 120
#define ESC_TAKEOFF_OFFSET 0
#define ESC_ARM_DELAY 5000

/* RC configuration
 * 
 */

#define RC_HIGH_CH1 1000
#define RC_LOW_CH1 2000
#define RC_HIGH_CH2 1000
#define RC_LOW_CH2 2000
#define RC_HIGH_CH3 1000
#define RC_LOW_CH3 2000
#define RC_HIGH_CH4 1000
#define RC_LOW_CH4 2000
#define RC_HIGH_CH5 1000
#define RC_LOW_CH5 2000
#define RC_ROUNDING_BASE 50

/*  PID configuration
 *  
 */

#define PITCH_P_VAL 4
#define PITCH_I_VAL 0.01
#define PITCH_D_VAL 20

#define ROLL_P_VAL 4
#define ROLL_I_VAL 0.01
#define ROLL_D_VAL 20

#define YAW_P_VAL 0
#define YAW_I_VAL 0
#define YAW_D_VAL 0


/* Flight parameters
 *
 */

#define PITCH_MIN -30
#define PITCH_MAX 30
#define ROLL_MIN -30
#define ROLL_MAX 30
#define YAW_MIN -180
#define YAW_MAX 180
#define PID_PITCH_INFLUENCE 20
#define PID_ROLL_INFLUENCE 20
#define PID_YAW_INFLUENCE 20


/*  MPU variables
 *
 */

MPU6050 mpu;                           // mpu interface object


uint8_t mpuIntStatus;                  // mpu statusbyte
uint8_t devStatus;                     // device status    
uint16_t packetSize;                   // estimated packet size  
uint16_t fifoCount;                    // fifo buffer size   
uint8_t fifoBuffer[64];                // fifo buffer 

Quaternion q;                          // quaternion for mpu output
VectorFloat gravity;                   // gravity vector for ypr
float ypr[3] = {0.0f,0.0f,0.0f};       // yaw pitch roll values
float yprLast[3] = {0.0f, 0.0f, 0.0f};


volatile bool mpuInterrupt = false;    //interrupt flag

long temps;
 
boolean interruptLock = false;

float ch1, ch2, ch3, ch4, ch5;         // RC channel inputs


int heightOrder;
int pitchOrder;
int rollOrder;
int yawOrder;

float heightSensor;
float pitchSensor;
float rollSensor;
float yawSensor;

float heightSensorLast;
float pitchSensorLast;
float rollSensorLast;
float yawSensorLast;


int velocity;                          // global velocity

float bal_ac, bal_bd;                 // motor balances can vary between -100 & 100
float bal_axes;                       // throttle balance between axes -100:ac , +100:bd

int va, vb, vc, vd;                    //velocities
int v_ac, v_bd;                        // velocity of axes

Servo a,b,c,d;


PID pitchReg(&pitchSensor, &bal_bd, &ch2, PITCH_P_VAL, PITCH_I_VAL, PITCH_D_VAL, DIRECT);
PID rollReg(&rollSensor, &bal_ac, &ch1, ROLL_P_VAL, ROLL_I_VAL, ROLL_D_VAL, REVERSE);
PID yawReg(&yawSensor, &bal_axes, &ch4, YAW_P_VAL, YAW_I_VAL, YAW_D_VAL, DIRECT);
 
float ch1Last, ch2Last, ch4Last, velocityLast;

void setup(){
  
  //receptor init
  pinMode(47, OUTPUT);
  digitalWrite(47,HIGH);
  pinMode(42, INPUT);
  pinMode(44, INPUT);
  pinMode(46, INPUT);
  pinMode(48, INPUT);
  pinMode(52, INPUT);
  delay(2000);
  
  initMPU();
  initESCs();
  initBalancing();
  initRegulators();
  
  Serial.begin(115200);  
  Serial.println("Initialisation effectué");
  temps = millis();
  
}

/* loop function
 *
 */

void loop(){
  
  //heightOrder = map(pulseIn(42, HIGH, 25000),1000,2000,0,180);
  if (digitalRead(52) == HIGH) {
  if (heightOrder < 60) heightOrder = heightOrder + 1;
  if (heightOrder > 60) heightOrder = 60;
  Serial.println("En marche");
  }
  if (digitalRead(52) == LOW) {
  heightOrder = 20;
  Serial.println("A l'arret");
  }  
  pitchOrder = map(pulseIn(44, HIGH, 25000),1000,2000,-90,90);
  rollOrder = map(pulseIn(46, HIGH, 25000),1000,2000,-90,90);
  yawOrder = map(pulseIn(40, HIGH, 25000),1000,2000,-90,90);
  
  while(!mpuInterrupt && fifoCount < packetSize){
     
    /* Do nothing while MPU is not working
     * This should be a VERY short period
     */
      
  }
  
  getYPR();
//  Serial.print("pitchSensor = "); // Afficher les lectures Arduino
//  Serial.println(pitchSensor);
//  Serial.print("rollSensor = "); // Afficher les lectures Arduino
//  Serial.println(rollSensor);
//  Serial.print("yawSensor = "); // Afficher les lectures Arduino
//  Serial.println(yawSensor);
//  Serial.println(" ");
  computePID();
  calculateVelocities();
  updateMotors();
  Serial.println(millis()-temps);
  temps = millis();
  
}

/*  computePID function
 *
 *  formats data for use with PIDLib
 *  and computes PID output
 */

void computePID(){

  acquireLock();

  ch1 = rollOrder;
  ch2 = pitchOrder;
  ch4 = yawOrder;
  
  if((ch2 < PITCH_MIN) || (ch2 > PITCH_MAX)) ch2 = ch2Last;
  if((ch1 < ROLL_MIN) || (ch1 > ROLL_MAX)) ch1 = ch1Last;
  if((ch4 < YAW_MIN) || (ch4 > YAW_MAX)) ch4 = ch4Last;
  
  ch1Last = ch1;
  ch2Last = ch2;
  ch4Last = ch4;

  pitchReg.Compute();
  rollReg.Compute();
  yawReg.Compute();
  
  releaseLock();

}

/*  getYPR function
 *
 *  gets data from MPU and
 *  computes pitch, roll, yaw on the MPU's DMP
 */

void getYPR(){
  
    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);
      yawSensor = (ypr[0] * 180/M_PI);
      pitchSensor = (ypr[2] * 180/M_PI);
      rollSensor = (ypr[1] * 180/M_PI);
      
//      if(abs(pitchSensor-pitchSensorLast)>30) pitchSensor[0] = yprLast[0];
//      if(abs(ypr[1]-yprLast[1])>30) ypr[1] = yprLast[1];
//      if(abs(ypr[2]-yprLast[2])>30) ypr[2] = yprLast[2];
//      yprLast[0] = ypr[0];
//      yprLast[1] = ypr[1];
//      yprLast[2] = ypr[2];
    
    }

}

/*  calculateVelocities function
 *  
 *  calculates the velocities of every motor
 *  using the PID output
 */

void calculateVelocities(){

  acquireLock();

  //ch3 = floor(ch3/RC_ROUNDING_BASE)*RC_ROUNDING_BASE;
  //velocity = map(ch3, RC_LOW_CH3, RC_HIGH_CH3, ESC_MIN, ESC_MAX);
  velocity = heightOrder;
  
  
  releaseLock();

  if((velocity < ESC_MIN) || (velocity > ESC_MAX)) velocity = velocityLast;
  
  velocityLast = velocity;
  
  v_ac = (abs(-100+bal_axes)/100)*velocity;
  v_bd = ((100+bal_axes)/100)*velocity;
  
  va = ((100+bal_ac)/100)*v_ac;
  vb = ((100+bal_bd)/100)*v_bd;
  
  vc = (abs((-100+bal_ac)/100))*v_ac;
  vd = (abs((-100+bal_bd)/100))*v_bd;
  
  //Serial.println(bal_bd);
  
  if(velocity < ESC_TAKEOFF_OFFSET){
  
    va = ESC_MIN;
    vb = ESC_MIN;
    vc = ESC_MIN;
    vd = ESC_MIN;
  
  }
  
}

void updateMotors(){

  a.write(va);
  c.write(vc);
  b.write(vb);
  d.write(vd);
//  Serial.println("Vitesse a");
//  Serial.println(va);
//  Serial.println("Vitesse b");
//  Serial.println(vb);
//  Serial.println("Vitesse c");
//  Serial.println(vc);
//  Serial.println("Vitesse d");
//  Serial.println(vd);
  

}


void dmpDataReady() {
    mpuInterrupt = true;
}


void initMPU(){
  
  Wire.begin();
  mpu.initialize();
  devStatus = mpu.dmpInitialize();
  if(devStatus == 0){
  
    mpu.setDMPEnabled(true);
    attachInterrupt(2, dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }  
}


void initESCs(){
  a.write(ESC_MAX);
  b.write(ESC_MAX);
  c.write(ESC_MAX);
  d.write(ESC_MAX);
  a.attach(ESC_A);
  b.attach(ESC_B);
  c.attach(ESC_C);
  d.attach(ESC_D);
  delay(5000);
  a.write(ESC_MIN);
  b.write(ESC_MIN);
  c.write(ESC_MIN);
  d.write(ESC_MIN);
  delay(5000);
}

void initBalancing(){

  bal_axes = 0;
  bal_ac = 0;
  bal_bd = 0;

}

void initRegulators(){

  pitchReg.SetMode(AUTOMATIC);
  pitchReg.SetOutputLimits(-PID_PITCH_INFLUENCE, PID_PITCH_INFLUENCE);
  
  rollReg.SetMode(AUTOMATIC);
  rollReg.SetOutputLimits(-PID_ROLL_INFLUENCE, PID_ROLL_INFLUENCE);
  
  yawReg.SetMode(AUTOMATIC);
  yawReg.SetOutputLimits(-PID_YAW_INFLUENCE, PID_YAW_INFLUENCE);

}


void acquireLock(){
  interruptLock = true; 
}

void releaseLock(){
  interruptLock = false;
}

#endif

Thank you

Replace digitalRead( ) with direct port manipulation.

For example:

if (digitalRead(52) == HIGH) {
// replacew with
if ((PIND & 0b00000100) != 0){ // test if PORTD has a bit high, in this case only bit 2 is being looked at

Need to look up the ports & bits you are using, I chose D and bit 2 as an example only, D52 is another port and another bit.
Code becomes less portable across processors, but faster for the processor you are using.

I'm doing a quadcopter with MPU6050 but my code seems a bit slow, a loop takes 100ms! Is it enough for a quadcopter? How could I increase it?

You increase it by adding some delays in the loop. But I suspect you want to decrease the loop time.

Basically get rid of all the floating point stuff and work with integers. This is easier than you might think, you just scale things. For example have the integer value 1000 times what you want and then anything less than 1000 is your fractional part.

Thank you, (yes I wanted to say increase the freq :wink: )

I will try your two techniques.

First, you need to define where the problem is.

a loop takes 100ms!

Which loop? Are you referring to loop()? If so, then time consuming things like:

  pitchOrder = map(pulseIn(44, HIGH, 25000),1000,2000,-90,90);
  rollOrder = map(pulseIn(46, HIGH, 25000),1000,2000,-90,90);
  yawOrder = map(pulseIn(40, HIGH, 25000),1000,2000,-90,90);

are not a good idea.

  while(!mpuInterrupt && fifoCount < packetSize){
     
    /* Do nothing while MPU is not working
     * This should be a VERY short period
     */
      
  }

All variables that are used in interrupt service routines and other parts of the code need to be volatile. Only mpuInterrupt is. Why?