I want to know how to speed up the i2c sensor

Using Arduino Programming Questions
1

Hello,

I am using Arduino Mega, 5 mpu6050 and mux.
I'm going to use these sensors to find the angular values of each sensor.
And I'm going to print these values at least 500 times per second. But now it's printed much slower than that. (100 times per second)
Is there a way to speed up the I2C sensor? (I changed my I2C clock to 400 kHz).

Thank you!

#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"

#define TCAADDR 0x70 //mux address
#define MPU 0x68

MPU6050 mpu_1;
MPU6050 mpu_2;
MPU6050 mpu_3;
MPU6050 mpu_4;
MPU6050 mpu_5;

#define LED_PIN 13
bool blinkState = false;

long loopTimer, loopTimer2;
double accelPitch;
double accelRoll;
int temperature;
long acc_x, acc_y, acc_z;
double accel_x, accel_y, accel_z;
double gyroRoll, gyroPitch, gyroYaw;
int gyro_x, gyro_y, gyro_z;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
double rotation_x, rotation_y, rotation_z;
double freq, dt;
double tau = 0.98;
double roll1, pitch1 = 0;
double roll2, pitch2 = 0;
double roll3, pitch3 = 0;
double roll4, pitch4 = 0;
double roll5, pitch5 = 0;

// 250 deg/s --> 131.0, 500 deg/s --> 65.5, 1000 deg/s --> 32.8, 2000 deg/s --> 16.4
long scaleFactorGyro = 65.5;

// 2g --> 16384 , 4g --> 8192 , 8g --> 4096, 16g --> 2048
long scaleFactorAccel = 8192;


//MUX init
void tcaselect(uint8_t i)//read mux buses
{
  if (i > 7) return;
  Wire.beginTransmission(TCAADDR);
  Wire.write(1 << i);
  Wire.endTransmission();
}

//mpu init
void mpus_init(int i) {
  tcaselect(i);
  //Activate the MPU-6050
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();

  // Configure the accelerometer
  // Wire.write(0x__);
  // Wire.write; 2g --> 0x00, 4g --> 0x08, 8g --> 0x10, 16g --> 0x18
  Wire.beginTransmission(0x68);
  Wire.write(0x1C);
  Wire.write(0x08);
  Wire.endTransmission();

  // Configure the gyro
  // Wire.write(0x__);
  // 250 deg/s --> 0x00, 500 deg/s --> 0x08, 1000 deg/s --> 0x10, 2000 deg/s --> 0x18
  Wire.beginTransmission(0x68);
  Wire.write(0x1B);
  Wire.write(0x08);
  Wire.endTransmission();
}

void calibrations(int i) {
  tcaselect(i);

  //Calibrating
  Serial.println("calibration... don't move");

  // 1000times for offset
  for (int cal_int = 0; cal_int < 1000; cal_int ++) {
    if (cal_int % 100 == 0) {
      Serial.print(".");
    }
    // raw값 읽어오기
    read_mpu_6050_data();
    gyro_x_cal += gyro_x;
    gyro_y_cal += gyro_y;
    gyro_z_cal += gyro_z;

    delay(3);
  }

  // Average the values
  gyro_x_cal /= 1000;
  gyro_y_cal /= 1000;
  gyro_z_cal /= 1000;

  // Display headers

  Serial.print("Calibration\t");
  Serial.print(gyro_x_cal);
  Serial.print("\t");
  Serial.print(gyro_y_cal);
  Serial.print("\t");
  Serial.println(gyro_z_cal);
  delay(100);

  // Reset the loop timer
  loopTimer = micros();
  loopTimer2 = micros();
}


//read raw data
void read_mpu_6050_data() {
  // Subroutine for reading the raw data
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission();
  Wire.requestFrom(0x68, 14);

  // Read data --> Temperature falls between acc and gyro registers
  acc_x = Wire.read() << 8 | Wire.read();
  acc_y = Wire.read() << 8 | Wire.read();
  acc_z = Wire.read() << 8 | Wire.read();

  temperature = Wire.read() << 8 | Wire.read();

  gyro_x = Wire.read() << 8 | Wire.read();
  gyro_y = Wire.read() << 8 | Wire.read();
  gyro_z = Wire.read() << 8 | Wire.read();
}


//output
int count;
void output_angle() {
  count++;

  //mpu1
  tcaselect(0);
  freq = 1 / ((micros() - loopTimer2) * 1e-6);
  loopTimer2 = micros();
  dt = 1 / freq;

  // Read the raw acc data from MPU-6050
  read_mpu_6050_data();

  gyro_x -= gyro_x_cal;
  gyro_y -= gyro_y_cal;
  gyro_z -= gyro_z_cal;

  // Convert to instantaneous degrees per second
  rotation_x = (double)gyro_x / (double)scaleFactorGyro;
  rotation_y = (double)gyro_y / (double)scaleFactorGyro;
  rotation_z = (double)gyro_z / (double)scaleFactorGyro;

  // Convert to g force
  accel_x = (double)acc_x / (double)scaleFactorAccel;
  accel_y = (double)acc_y / (double)scaleFactorAccel;
  accel_z = (double)acc_z / (double)scaleFactorAccel;

  accelPitch = atan2(accel_y, accel_z) * RAD_TO_DEG;
  accelRoll = atan2(accel_x, accel_z) * RAD_TO_DEG;

  pitch1 = (tau) * (pitch1 + rotation_x * dt) + (1 - tau) * (accelPitch);
  roll1 = (tau) * (roll1 - rotation_y * dt) + (1 - tau) * (accelRoll);

  gyroPitch += rotation_x * dt;
  gyroRoll -= rotation_y * dt;
  gyroYaw += rotation_z * dt;


  //==============================
  //mpu2
  tcaselect(1);
  freq = 1 / ((micros() - loopTimer2) * 1e-6);
  loopTimer2 = micros();
  dt = 1 / freq;

  // Read the raw acc data from MPU-6050
  read_mpu_6050_data();

  gyro_x -= gyro_x_cal;
  gyro_y -= gyro_y_cal;
  gyro_z -= gyro_z_cal;

  // Convert to instantaneous degrees per second
  rotation_x = (double)gyro_x / (double)scaleFactorGyro;
  rotation_y = (double)gyro_y / (double)scaleFactorGyro;
  rotation_z = (double)gyro_z / (double)scaleFactorGyro;

  // Convert to g force
  accel_x = (double)acc_x / (double)scaleFactorAccel;
  accel_y = (double)acc_y / (double)scaleFactorAccel;
  accel_z = (double)acc_z / (double)scaleFactorAccel;

  accelPitch = atan2(accel_y, accel_z) * RAD_TO_DEG;
  accelRoll = atan2(accel_x, accel_z) * RAD_TO_DEG;

  pitch2 = (tau) * (pitch2 + rotation_x * dt) + (1 - tau) * (accelPitch);
  roll2 = (tau) * (roll2 - rotation_y * dt) + (1 - tau) * (accelRoll);

  gyroPitch += rotation_x * dt;
  gyroRoll -= rotation_y * dt;
  gyroYaw += rotation_z * dt;


  //==============================
  //mpu3
  tcaselect(2);
  freq = 1 / ((micros() - loopTimer2) * 1e-6);
  loopTimer2 = micros();
  dt = 1 / freq;

  // Read the raw acc data from MPU-6050
  read_mpu_6050_data();

  gyro_x -= gyro_x_cal;
  gyro_y -= gyro_y_cal;
  gyro_z -= gyro_z_cal;

  // Convert to instantaneous degrees per second
  rotation_x = (double)gyro_x / (double)scaleFactorGyro;
  rotation_y = (double)gyro_y / (double)scaleFactorGyro;
  rotation_z = (double)gyro_z / (double)scaleFactorGyro;

  // Convert to g force
  accel_x = (double)acc_x / (double)scaleFactorAccel;
  accel_y = (double)acc_y / (double)scaleFactorAccel;
  accel_z = (double)acc_z / (double)scaleFactorAccel;

  accelPitch = atan2(accel_y, accel_z) * RAD_TO_DEG;
  accelRoll = atan2(accel_x, accel_z) * RAD_TO_DEG;

  pitch3 = (tau) * (pitch3 + rotation_x * dt) + (1 - tau) * (accelPitch);
  roll3 = (tau) * (roll3 - rotation_y * dt) + (1 - tau) * (accelRoll);

  gyroPitch += rotation_x * dt;
  gyroRoll -= rotation_y * dt;
  gyroYaw += rotation_z * dt;


  //==============================
  //mpu4
  tcaselect(3);
  freq = 1 / ((micros() - loopTimer2) * 1e-6);
  loopTimer2 = micros();
  dt = 1 / freq;

  // Read the raw acc data from MPU-6050
  read_mpu_6050_data();

  // 오프셋 보정 값 빼기
  gyro_x -= gyro_x_cal;
  gyro_y -= gyro_y_cal;
  gyro_z -= gyro_z_cal;

  // Convert to instantaneous degrees per second
  rotation_x = (double)gyro_x / (double)scaleFactorGyro;
  rotation_y = (double)gyro_y / (double)scaleFactorGyro;
  rotation_z = (double)gyro_z / (double)scaleFactorGyro;

  // Convert to g force
  accel_x = (double)acc_x / (double)scaleFactorAccel;
  accel_y = (double)acc_y / (double)scaleFactorAccel;
  accel_z = (double)acc_z / (double)scaleFactorAccel;

  // 상보필터 계산
  accelPitch = atan2(accel_y, accel_z) * RAD_TO_DEG;
  accelRoll = atan2(accel_x, accel_z) * RAD_TO_DEG;

  pitch4 = (tau) * (pitch4 + rotation_x * dt) + (1 - tau) * (accelPitch);
  roll4 = (tau) * (roll4 - rotation_y * dt) + (1 - tau) * (accelRoll);

  gyroPitch += rotation_x * dt;
  gyroRoll -= rotation_y * dt;
  gyroYaw += rotation_z * dt;


  //==============================
  //mpu5
  tcaselect(4);
  freq = 1 / ((micros() - loopTimer2) * 1e-6);
  loopTimer2 = micros();
  dt = 1 / freq;

  // Read the raw acc data from MPU-6050
  read_mpu_6050_data();

  gyro_x -= gyro_x_cal;
  gyro_y -= gyro_y_cal;
  gyro_z -= gyro_z_cal;

  // Convert to instantaneous degrees per second
  rotation_x = (double)gyro_x / (double)scaleFactorGyro;
  rotation_y = (double)gyro_y / (double)scaleFactorGyro;
  rotation_z = (double)gyro_z / (double)scaleFactorGyro;

  // Convert to g force
  accel_x = (double)acc_x / (double)scaleFactorAccel;
  accel_y = (double)acc_y / (double)scaleFactorAccel;
  accel_z = (double)acc_z / (double)scaleFactorAccel;

  // comp filter
  accelPitch = atan2(accel_y, accel_z) * RAD_TO_DEG;
  accelRoll = atan2(accel_x, accel_z) * RAD_TO_DEG;

  pitch5 = (tau) * (pitch5 + rotation_x * dt) + (1 - tau) * (accelPitch);
  roll5 = (tau) * (roll5 - rotation_y * dt) + (1 - tau) * (accelRoll);

  gyroPitch += rotation_x * dt;
  gyroRoll -= rotation_y * dt;
  gyroYaw += rotation_z * dt;


  //==============================
  // Data out serial monitor
  Serial.print(freq, 0);
  Serial.println("Hz");
  //  Serial.print("MPU1: ");
  //  Serial.print(roll1, 1);
  //  Serial.print("\t");
  //  Serial.println(pitch1, 1);
  //
  //  Serial.print("MPU2: ");
  //  Serial.print(roll2, 1);
  //  Serial.print("\t");
  //  Serial.println(pitch2, 1);
  //
  //  Serial.print("MPU3: ");
  //  Serial.print(roll3, 1);
  //  Serial.print("\t");
  //  Serial.println(pitch3, 1);
  //
  //  Serial.print("MPU4: ");
  //  Serial.print(roll4, 1);
  //  Serial.print("\t");
  //  Serial.println(pitch4, 1);
  //
  //  Serial.print("MPU5: ");
  //  Serial.print(roll5, 1);
  //  Serial.print("\t");
  //  Serial.println(pitch5, 1);

  Serial.println(count);
  Serial.println();

  // blink LED to indicate activity
  blinkState = !blinkState;
  digitalWrite(LED_PIN, blinkState);

  // Wait until the loopTimer reaches 4000us (250Hz) before next loop
  while (micros() - loopTimer <= 4000);
  loopTimer = micros();
}

//=====================================================================

void setup() {
  Wire.begin();           //init I2C  and serial port
  Wire.setClock(400000); // 400kHz I2C clock
  Serial.begin(115200); 
  while (!Serial); 

  //mpus init
  for (int i = 0; i < 5; i++) {
    mpus_init(i);
  }

  //error check
  if (mpu_1.testConnection()) {
    Serial.println("Sensor1 OK");
  }
  else {
    Serial.println("Error sensor1");
  }

  if (mpu_2.testConnection()) {
    Serial.println("Sensor2 OK");
  }
  else {
    Serial.println("Error sensor2");
  }

  if (mpu_3.testConnection()) {
    Serial.println("Sensor3 OK");
  }
  else {
    Serial.println("Error sensor3");
  }

  if (mpu_4.testConnection()) {
    Serial.println("Sensor4 OK");
  }
  else {
    Serial.println("Error sensor4");
  }

  if (mpu_5.testConnection()) {
    Serial.println("Sensor5 OK");
  }
  else {
    Serial.println("Error sensor5");
  }

  //calibration
  for (int i = 0; i < 5; i++) {
    calibrations(i);
  }
}

//=====================================================================


void loop() {
  output_angle();
}
2

I would suggest that the reading of the I2C sensor it what it is and probably can't be sped up much, unless the library is badly written and could be optimized.

I think you should look at the bigger picture and not ask yourself, how can I speed up the I2C sensor, but instead, how can I speed up the processing of values read from the sensor.

Since there is a whole lot of code in there, try determining the millis used by each part and work from there. I would proceed by inserting prints into your output_angle function. Something like Serial.print('A');Serial.println(millis());. A few lines later, Serial.print('B';Serial.println(millis()); etc. Looking at those values, you can start narrowing down what code is taking the most time and concentrate on making that code faster.

3 
  // Wait until the loopTimer reaches 4000us (250Hz) before next loop
  while (micros() - loopTimer <= 4000);
  loopTimer = micros();
}

How do you want to print all the data 500 times per second if you yourself are slowing down the speed of the loop to 250 Hz??

best regards Stefan

4

Nice catch Stefan, you have more patience than I do to analyze all that code.

If that code were correct, it would reduce the frequency to 0 and not 250 because loopTimer = micros() each time through the loop so micros() - loopTimer will always be a very very small number.

The only reason the program doesn't hang is that the while loop is improperly coded. That semicolon on the while line ENDS the loop, rendering the while loop useless!

5

Nope.

While the condition is met, do nothing

is perfectly valid for a wait.

6

Increase the serial baud rate, at 500Hz you will only have time to print 23 characters per loop (including the carriage return and newline).

Floating point math is fairly slow, and on a Mega there is no double, double is implemented as float.

You will likely be limited by the speed of the sensor and the I2C bus.

Suggestion, use an array for the sensors, that will greatly reduce the amount of repetitive code.

7

Use SPI.

8

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.