Go Down

Topic: MPU6050 low sampling rate (Read 613 times) previous topic - next topic

GeoKrok

Hello.
I am using MPU 6050 with Arduino Uno r3. I also use DS1302 rtc and catalex microsd card adapter. I am facing a problem with low sampling rate and to more specific, in my csv file i take only 23-25 values/sec. Can anyone please help me to explain how I can reach 80-100 values / sec ?

Thanks in advance


Code: [Select]
    #include<Wire.h>
    #include <SD.h>
    #include <SPI.h>
    #include <virtuabotixRTC.h> //Library used
   
//RTC
//Wiring SCLK -> 6, I/O -> 7, CE -> 8
//Or CLK -> 6 , DAT -> 7, Reset -> 8

virtuabotixRTC myRTC(6, 7, 8); //If you change the wiring change the pins here also
//FILE
    File myFile;
    char myLogName[] = "MyLog.csv";

//GYRO   
    const int MPU_addr=0x69;  // I2C address of the MPU-6050
    int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;

   
    void setup(){
  // Set the current date, and time in the following format:
 // seconds, minutes, hours, day of the week, day of the month, month, year
 //myRTC.setDS1302Time(1, 38, 11, 4, 11, 7, 2019); //Here you write your actual time/date as shown above
 //but remember to "comment/remove" this function once you're done
 //The setup is done only one time and the module will continue counting it automatically


     
//SD BEGIN     
      SD.begin(4);
      myFile = SD.open(myLogName, FILE_WRITE);
      myFile.print("Date, Time, AcX, AcY, AcZ, Temp, GyX, GyY, GyZ\n");
      //myFile.println();
//WIRE BEGIN     
      Wire.begin();
      Wire.beginTransmission(MPU_addr);
      Wire.write(0x6B);  // PWR_MGMT_1 register
      Wire.write(0);     // set to zero (wakes up the MPU-6050)
      Wire.endTransmission(true);
      Serial.begin(115200);
      myFile.close();
    }
    void loop(){
//UPDATE TIME
       myRTC.updateTime();     
//GYRO     
      Wire.beginTransmission(MPU_addr);
      Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
      Wire.endTransmission(false);
      Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
      AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)   
      AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
      AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
      Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
      GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
      GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
      GyZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

     
     Serial.print("AcX = "); Serial.print(AcX);
     Serial.print(" | AcY = "); Serial.print(AcY);
     Serial.print(" | AcZ = "); Serial.print(AcZ);
     Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53);  //equation for temperature in degrees C from datasheet
     Serial.print(" | GyX = "); Serial.print(GyX);
     Serial.print(" | GyY = "); Serial.print(GyY);
     Serial.print(" | GyZ = "); Serial.println(GyZ);

//FILE OPEN
      myFile = SD.open(myLogName, FILE_WRITE);
      myFile.print(myRTC.dayofmonth,DEC); //You can switch between day and month if you're using American system
      myFile.print("/");
      myFile.print(myRTC.month,DEC);
      myFile.print("/");
      myFile.print(myRTC.year,DEC);
      myFile.print(" , ");
      myFile.print(myRTC.hours,DEC);
      myFile.print(":");
      myFile.print(myRTC.minutes,DEC);
      myFile.print(":");
      myFile.print(myRTC.seconds,DEC);
     
      myFile.print(" , ");
      myFile.print(AcX);
      myFile.print(" , ");
      myFile.print(AcY);
      myFile.print(" , ");
      myFile.print(AcZ);
      myFile.print(" , ");
      myFile.print(Tmp/340.00+36.53);  //equation for temperature in degrees C from datasheet
      myFile.print(" , ");
      myFile.print(GyX);
      myFile.print(" , ");
      myFile.print(GyY);
      myFile.print(" , ");
      myFile.println(GyZ);
      myFile.print(" , ");


     

//DATE AND TIME CODE//
//     Serial.print("Current Date / Time: ");
//     Serial.print(myRTC.dayofmonth,DEC); //You can switch between day and month if you're using American system
//     Serial.print("/");
//     Serial.print(myRTC.month,DEC);
//     Serial.print("/");
//     Serial.print(myRTC.year,DEC);
//     Serial.print(" ");
//     Serial.print(myRTC.hours,DEC);
//     Serial.print(":");
//     Serial.print(myRTC.minutes,DEC);
//     Serial.print(":");
//     Serial.println(myRTC.seconds,DEC);
   
      myFile.close(); //close file
     
     
     
      delay(10);
    }

MattLanc

First of all the delay(10) at the end of the program doesn't allow it to run at 100Hz, because you have to add those 10ms to the execution time of your sketch.
The execution time of your sketch is very high because you just call a lot of .print() in the serial monitor and to the SD card, which both of them require some milliseconds for every .print() call.

If you get a 25Hz loop frequency I assume that the execution time of your sketch is around 30ms, as:

1s / 25 = 40ms
40ms - 10ms (the delay) = 30ms

So, if you remove the delay(10) you get approximately 33Hz loop frequency, while if you remove most of the .print() calls you can manage to achieve 50Hz/60Hz, maybe more.

pylon

There are several ways to improve speed.

1. Don't ask the RTC for the time in every round (the internal clock is more than accurate enough)
2. Patch the RTC library to use direct register calls (instead of digitalRead() and digitalWrite())
3. Don't print the values to the serial interface if you write them to SD card too
4. Write to the SD card by 512 byte chunks

But by far the easiest to implement is:

Get rid of this line:

Code: [Select]
     delay(10);

GeoKrok

Thank you for your answers! I did all of them except patching the RTC library (i do not know how) and sampling frequency increased at 32Hz. Any help patching the RTC library will be appreciated!

jremington

#4
Jul 15, 2019, 05:12 pm Last Edit: Jul 15, 2019, 05:13 pm by jremington
It is extremely time consuming to open and close the SD card file every time you write some data points.

Open the output file ONCE in setup() and close it when you are done collecting data.
No PM's please.

GeoKrok

Ok i will change it. As for step 2 ( Patch the RTC library to use direct register calls (instead of digitalRead() and digitalWrite()) ) can anyone help me in this part?
Thank in advance.

jremington

Quote
2. Patch the RTC library to use direct register calls (instead of digitalRead() and digitalWrite())
I don't believe that will make a significant difference in sample rate. Fix the file open/close problem first.
No PM's please.

GeoKrok

#7
Jul 16, 2019, 11:32 am Last Edit: Jul 16, 2019, 05:59 pm by GeoKrok
 I fixed the file open/close problem but did not make any difference. Any help?

pylon

Quote
I did all of them except patching the RTC library (i do not know how) and sampling frequency increased at 32Hz. Any help patching the RTC library will be appreciated!
I cannot believe that you implemented my first point. Post the code your currently using to get the 32Hz rate. If you implemented my first idea, patching the RTC won't give you an increase in the sampling frequency.

Idahowalker

Yea, I got an ideas
Why not measure how fast the sections of code are running?
Ex:
Code: [Select]
void loop(){

unsigned long tStarted = millis();

//UPDATE TIME
       myRTC.updateTime();     
//GYRO     
      Wire.beginTransmission(MPU_addr);
      Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
      Wire.endTransmission(false);
      Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
      AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)   
      AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
      AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
      Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
      GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
      GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
      GyZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
Serial.print( " 1: " );
Serial.println( millis() - sStarted );
     
     Serial.print("AcX = "); Serial.print(AcX);
     Serial.print(" | AcY = "); Serial.print(AcY);
     Serial.print(" | AcZ = "); Serial.print(AcZ);
     Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53);  //equation for temperature in degrees C from datasheet
     Serial.print(" | GyX = "); Serial.print(GyX);
     Serial.print(" | GyY = "); Serial.print(GyY);
     Serial.print(" | GyZ = "); Serial.println(GyZ);
Serial.print( " 2: " );
Serial.println( millis() - sStarted );
//FILE OPEN
      myFile = SD.open(myLogName, FILE_WRITE);
      myFile.print(myRTC.dayofmonth,DEC); //You can switch between day and month if you're using American system
      myFile.print("/");
      myFile.print(myRTC.month,DEC);
      myFile.print("/");
      myFile.print(myRTC.year,DEC);
      myFile.print(" , ");
      myFile.print(myRTC.hours,DEC);
      myFile.print(":");
      myFile.print(myRTC.minutes,DEC);
      myFile.print(":");
      myFile.print(myRTC.seconds,DEC);
     
      myFile.print(" , ");
      myFile.print(AcX);
      myFile.print(" , ");
      myFile.print(AcY);
      myFile.print(" , ");
      myFile.print(AcZ);
      myFile.print(" , ");
      myFile.print(Tmp/340.00+36.53);  //equation for temperature in degrees C from datasheet
      myFile.print(" , ");
      myFile.print(GyX);
      myFile.print(" , ");
      myFile.print(GyY);
      myFile.print(" , ");
      myFile.println(GyZ);
      myFile.print(" , ");
Serial.print( " 3: " );
Serial.println( millis() - sStarted );
     

//DATE AND TIME CODE//
//     Serial.print("Current Date / Time: ");
//     Serial.print(myRTC.dayofmonth,DEC); //You can switch between day and month if you're using American system
//     Serial.print("/");
//     Serial.print(myRTC.month,DEC);
//     Serial.print("/");
//     Serial.print(myRTC.year,DEC);
//     Serial.print(" ");
//     Serial.print(myRTC.hours,DEC);
//     Serial.print(":");
//     Serial.print(myRTC.minutes,DEC);
//     Serial.print(":");
//     Serial.println(myRTC.seconds,DEC);
   
      myFile.close(); //close file
Serial.print( " 4: " );
      Serial.println( millis() - sStarted );
     
     
      delay(10);
Serial.print( " 5: " );
Serial.println( millis() - sStarted );
    }


Which may give you a rough idea of the hold up and then you can work from there.

GeoKrok

Code: [Select]
    #include<Wire.h>
   #include <SD.h>
   #include <SPI.h>
   #include <virtuabotixRTC.h> //Library used
   
//RTC
//Wiring SCLK -> 6, I/O -> 7, CE -> 8
//Or CLK -> 6 , DAT -> 7, Reset -> 8

virtuabotixRTC myRTC(6, 7, 8); //If you change the wiring change the pins here also
//FILE
   File myFile;
   char myLogName[] = "MyLog.csv";

//GYRO   
   const int MPU_addr=0x69;  // I2C address of the MPU-6050
   int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;

   
   void setup(){
 // Set the current date, and time in the following format:
// seconds, minutes, hours, day of the week, day of the month, month, year
//myRTC.setDS1302Time(0000, 23, 12, 1, 15, 7, 2019); //Here you write your actual time/date as shown above
//but remember to "comment/remove" this function once you're done
//The setup is done only one time and the module will continue counting it automatically


     
//SD BEGIN     
     SD.begin(4);
//      myFile = SD.open(myLogName, FILE_WRITE);
//      myFile.print("Date, Time, AcX, AcY, AcZ, Temp, GyX, GyY, GyZ");
//      myFile.println();
//WIRE BEGIN     
     Wire.begin();
     Wire.beginTransmission(MPU_addr);
     Wire.write(0x6B);  // PWR_MGMT_1 register
     Wire.write(0);     // set to zero (wakes up the MPU-6050)
     Wire.endTransmission(true);
    // Serial.begin(115200);
     myFile.close();
   }
   void loop(){
//UPDATE TIME
      myRTC.updateTime();     
//GYRO     
     Wire.beginTransmission(MPU_addr);
     Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
     Wire.endTransmission(false);
     Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
     AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)   
     AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
     AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
     //Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
     GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
     GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
     GyZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

   
    //Serial.print(" | AcX = "); Serial.print(AcX);
   // Serial.print(" | AcY = "); Serial.print(AcY);
   // Serial.print(" | AcZ = "); Serial.println(AcZ);
    //Serial.print(" | Tmp = "); Serial.println(Tmp/340.00+36.53);
   // Serial.print(" | GyX = "); Serial.print(GyX);
   // Serial.print(" | GyY = "); Serial.print(GyY);
   // Serial.print(" | GyZ = "); Serial.println(GyZ);
//FILE OPEN
     myFile = SD.open(myLogName, FILE_WRITE);
//      myFile.print(myRTC.dayofmonth,DEC); //You can switch between day and month if you're using American system
//      myFile.print("/");
//      myFile.print(myRTC.month,DEC);
//      myFile.print("/");
//      myFile.print(myRTC.year,DEC);
//      myFile.print(" , ");
     myFile.print(myRTC.hours,DEC);
     myFile.print(":");
     myFile.print(myRTC.minutes,DEC);
     myFile.print(":");
     myFile.print(myRTC.seconds,DEC);
     
     myFile.print(" , ");
     myFile.print(AcX);
     myFile.print(" , ");
     myFile.print(AcY);
     myFile.print(" , ");
     myFile.print(AcZ);
     myFile.print(" , ");
    // myFile.print(Tmp/340.00+36.53);  //equation for temperature in degrees C from datasheet
    // myFile.print(" , ");
     myFile.print(GyX);
     myFile.print(" , ");
     myFile.print(GyY);
     myFile.print(" , ");
     myFile.println(GyZ);
     myFile.print(" , ");


//DATE AND TIME CODE//
//     Serial.print("Current Date / Time: ");
//     Serial.print(myRTC.dayofmonth,DEC); //You can switch between day and month if you're using American system
//     Serial.print("/");
//     Serial.print(myRTC.month,DEC);
//     Serial.print("/");
//     Serial.print(myRTC.year,DEC);
//     Serial.print(" ");
//     Serial.print(myRTC.hours,DEC);
//     Serial.print(":");
//     Serial.print(myRTC.minutes,DEC);
//     Serial.print(":");
//     Serial.println(myRTC.seconds,DEC);

     myFile.close(); //close file
     
     
     
     delay(10);
   }

GeoKrok

Currently the sampling frequency reaches 35-38 samples/sec. Any ideas how to reach more?

Idahowalker

#12
Jul 18, 2019, 01:29 pm Last Edit: Jul 18, 2019, 01:57 pm by Idahowalker
A)Set the MPU6050 to use interrupts and have the MPU6050 tell you when it has data to read and then you read that data when its ready.

B) Use a Arduino Due or STM32 Bluepill. Set the MPU6050 to use interrupts and have the MPU6050 tell you when it has data to read and then you read and process that data.
C) Use a ESP32 with freeRTOS, assign the task to its own core, set the task to read the MPU using interrupts and have the MPU6050 tell you when it has data to read and then you read that data when its ready.
D) get a MPU that uses the SPI buss and use a Due or a STM32 Bluepill or an ESP32.

E) get the MPU60X0 register datasheet, read it learn it and you may get a few ideas from the data provided.

SPI buss faster then I2C.

If you require more power get more power. A Uno is an 8bit uController. You will, eventually, be doing floating point calculations with the MPU data, is my guess. The Uno is not very efficient with floats. I mention the Due, STM32 and ESP32 because that's what I have used and does not mean that those are the only solutions for more power; do your research.

Note: there is a reoccurring theme.

Another thing look over this guys code: https://os.mbed.com/users/onehorse/

Bonus, my code for using the MPU9250 with a STM32 Bluepill and the BolderFlightSystems library
Code: [Select]
#include <STM32FreeRTOS.h> // works. stm32 boards Generic STM32F103 series (selected from submenu)
#include <MPU9250.h>
/* https://github.com/bolderflight/MPU9250 */
MPU9250 IMU( SPI, PB0 );
//////////
/*Pin connections
  STM32        MPU9250
  3.3          Vcc
  GND          GND
  PA5          SCK(SCL)
  PA7          MOSI(SDA)
  PA6          MISO(ADO)
  PB0          NCS
  GND          FSYNC*/
//////////
void setup() {
  Serial.begin(115200);
  pinMode(PC13, OUTPUT);
  IMU.begin();
  // set the accelerometer full scale range to the given value
  // IMU.setAccelRange(MPU9250::ACCEL_RANGE_8G);
  // set the gyroscope full scale range to the given value.
  // IMU.setGyroRange(MPU9250::GYRO_RANGE_2000DPS);
  // setting DLPF bandwidth.   Digital Low Pass Filter (DLPF) bandwidth. fast bandwidth faster gryo reacts
  //// DLPF_BANDWIDTH_184HZ DLPF_BANDWIDTH_92HZ DLPF_BANDWIDTH_41HZ DLPF_BANDWIDTH_20HZ DLPF_BANDWIDTH_10HZ DLPF_BANDWIDTH_5HZ
  // IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_92HZ); // defaults to DLPF_BANDWIDTH_184HZ
  //// setting SRD to 1 for a 500 Hz update rate, 1000/(1+1)
  IMU.setSrd(1);
  // Task function, name of task, Stack size of task, parameter of the task, priority of the task, Task handle to keep track of created task
  xTaskCreate(v_getIMU, "getIMU", 2100, NULL, tskIDLE_PRIORITY + 2, NULL);
  xTaskCreate(fBlinkBuiltIn, "fBlinkBuiltIn", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY + 1, NULL);
  // start task
  vTaskStartScheduler();
}
//
void loop() {}
//
// mpu should be level and held still for this process
void calibrate_sensors(float RADIANS_TO_DEGREES, float& xOffset, float& yOffest )
{
  int num_readings = 10;
  // Discard the first reading
  IMU.readSensor();
  vTaskDelay( pdMS_TO_TICKS( 1 ) );
  //
  IMU.calibrateAccel();
  //
  IMU.setAccelCalX( IMU.getAccelBiasX_mss(), IMU.getAccelScaleFactorX() );
  IMU.setAccelCalY( IMU.getAccelBiasY_mss(), IMU.getAccelScaleFactorY() );
  IMU.setAccelCalZ( IMU.getAccelBiasZ_mss(), IMU.getAccelScaleFactorZ() );
  //
  IMU.calibrateGyro();
  //
  IMU.setGyroBiasX_rads( IMU.getGyroBiasX_rads() );
  IMU.setGyroBiasY_rads( IMU.getGyroBiasY_rads() );
  IMU.setGyroBiasZ_rads( IMU.getGyroBiasZ_rads() );
  // Read and average the raw values
  for (int i = 0; i < num_readings; i++)
{
  if ( IMU.readSensor() > -1 )
    {
      // get the data
      // calculate angle based on acceleration, store
      yOffest += atan(-1 * IMU.getAccelX_mss() / sqrt(pow(IMU.getAccelY_mss(), 2) + pow(IMU.getAccelZ_mss(), 2))) * RADIANS_TO_DEGREES;
      xOffset += atan(IMU.getAccelY_mss() / sqrt(pow(IMU.getAccelX_mss(), 2) + pow(IMU.getAccelZ_mss(), 2))) * RADIANS_TO_DEGREES;
      //
    }
    else // if readsensor fail decrement i if i is not zero
    {
      if ( i != 0 )
      {
        i -= 1;
      }
    }
    vTaskDelay( pdMS_TO_TICKS(2) );
  }
  yOffest /= num_readings; // average the readings
  xOffset /= num_readings;
} // void calibrate_sensors()
//
//
static void v_getIMU( void *pvParameters )
{
  const float RADIANS_TO_DEGREES = 57.2958; //180/3.14159
  const float K = 0.97; //this is the gyro:accelerometer ratio
  const float K1 = 1 - K;
  const TickType_t xDelay = pdMS_TO_TICKS(1);
  float accel_angle_y;
  float accel_angle_x;
  float angle_z = 0.0;
  float last_x_angle;
  float last_y_angle;
  float dt;
  unsigned long t_now = micros(); // stem32 millis record 0, use micros instead
  unsigned long last_read_time = micros();
  //  unsigned long t_start = micros();
  float angle_x;
  float angle_y;
  float base_accel_angle_x = 0;
  float base_accel_angle_y = 0;
  float TourqeAngle_uSec = 22.22; // 22.22uS per degree, based upon set servo limits of 90 degrees (+/-45 degrees)
  int XservoInitial = 1500; // may need to adjust to servo mounted level.
  int YservoInitial = 1500;
  String sBuffer;
  sBuffer.reserve( 25 );
  calibrate_sensors(RADIANS_TO_DEGREES, base_accel_angle_x, base_accel_angle_y );
  //
  for (;;)
  {
    // read the sensor
    if ( IMU.readSensor() > -1 )
    {
      //t_start = micros(); // remove in final version
      t_now = micros();
      // Get time of last data read
      dt = (t_now - last_read_time) / 1000000.0f;
      // calculate accelerometer angles
      accel_angle_y = atan(-1 * IMU.getAccelX_mss() / sqrt(pow(IMU.getAccelY_mss(), 2) + pow(IMU.getAccelZ_mss(), 2))) * RADIANS_TO_DEGREES;
      accel_angle_x = atan(IMU.getAccelY_mss() / sqrt(pow(IMU.getAccelX_mss(), 2) + pow(IMU.getAccelZ_mss(), 2))) * RADIANS_TO_DEGREES;
      accel_angle_y -= base_accel_angle_y; //subtract angle offset from angle reading, accelerometer
      accel_angle_x -= base_accel_angle_x;
      // Compute the (filtered) gyro angles
      angle_x = IMU.getGyroX_rads() * dt + last_x_angle; // factor in offset here if needed
      angle_y = IMU.getGyroY_rads() * dt + last_y_angle;
      // Apply the complementary filter
      angle_x = K * angle_x +  (K1 * accel_angle_x);
      angle_y = K * angle_y + (K1 * accel_angle_y);
      if ( (abs(angle_x) > 0.1) || (abs(angle_y) > 0.1) )
      {
        float xMagnitude = angle_x * TourqeAngle_uSec;
        float yMagnitude = angle_y * TourqeAngle_uSec;
        float xAdjustment = XservoInitial - xMagnitude; // may need to adjust sign depending on servo/mpu orientation for proper direction
        float yAdjustment = YservoInitial - yMagnitude; //  may need to adjust sign depending on servo/mpu orientation for proper direction
        //set high low limit
        // send xAdjustment and yAdjustment torques to servos
        // print the data for testing
        sBuffer.concat( String(angle_x) );
        sBuffer.concat( "," );
        sBuffer.concat( String(xAdjustment) );
        sBuffer.concat( "," );
        sBuffer.concat( String(angle_y) );
        sBuffer.concat( "," );
        sBuffer.concat( String(yAdjustment) );
        //      sBuffer.concat( String(sBuffer.length()) );
        sBuffer.concat( "," );
        //        Serial.print( sBuffer );
        //        Serial.print(uxTaskGetStackHighWaterMark( NULL )); // stack size used
        //        Serial.println();
        //        Serial.flush();
        sBuffer = "";
      }
      //            Serial.print( micros() - t_now ); // time of execution
      //            Serial.println();
    } // if ( IMU.readSensor() > -1 )
    // Update the saved data with the latest values
    last_read_time = t_now;
    last_x_angle = angle_x;
    last_y_angle = angle_y;
    //
    vTaskDelay( xDelay );
  }
  vTaskDelete( NULL );
}
//
void fBlinkBuiltIn( void* pvParameters )
{
  // toggle built in LED off/on
  for (;;)
  {
    digitalWrite(PC13, LOW);
    vTaskDelay( pdMS_TO_TICKS( 10 ) );
    digitalWrite(PC13, HIGH);
    vTaskDelay( pdMS_TO_TICKS( 1000 ) );
  }
  vTaskDelete( NULL );
}
//

Note, the line of code: const TickType_t xDelay = pdMS_TO_TICKS(1);. Yup that means do the thing once a millisecond. 1mS=1000 times a second.

Note, the use or the SPI buss.

GeoKrok

Idahowalker thank you very much for your extended answer. Since i am a beginner, do not know how to implement most of these steps but definitelly i will try. At this stage i wanted to use my current hardware (MPU6050, RTC DS1302 and Catalex microSD card adapter) and buy in the future something else.. So any further help on this hardware will be much appreciated! Thanks

pylon

Quote
Currently the sampling frequency reaches 35-38 samples/sec. Any ideas how to reach more?
You're still reading the RTC out in every run of the loop(). So you're reading exactly the same value more than 30 times a second. Don't you think this isn't necessary? Reading the value once an hour is more than enough.

Go Up