ROBOTIS-GIT/OpenCR

OpenCR IMU Error

Huafeng-XU opened this issue · 4 comments

Hello, I use the OpenCR IMU Example IMU_Read_RollPitchYaw in Arduino IDE. But When i add the delay(2000) function in the code, the IMU data become error and the ACC Cali doesn't work. Can anyone tell me how the address this problem and tell me why? Thank you.

83 0.95 -4.97 -11.87
83 -3.75 17.50 -12.39
ACC Cali Start
ACC Cali End 83 -5.48 17.15 -12.63
84 1.61 -4.71 -11.84
83 -5.71 17.06 -12.65
83 1.26 -4.84 -11.89
84 -4.71 17.32 -12.55

The code is below:


#include <IMU.h>

cIMU    IMU;

uint8_t   err_code;
uint8_t   led_tog = 0;
uint8_t   led_pin = 13;

void setup()
{
  Serial.begin(115200);

  IMU.begin();

  pinMode( led_pin, OUTPUT );
}


void loop()
{
  static uint32_t tTime[3];
  static uint32_t imu_time = 0;


  if( (millis()-tTime[0]) >= 500 )
  {
    tTime[0] = millis();

    digitalWrite( led_pin, led_tog );
    led_tog ^= 1;
  }

  tTime[2] = micros();
  if( IMU.update() > 0 ) imu_time = micros()-tTime[2];



  if( (millis()-tTime[1]) >= 50 )
  {
    tTime[1] = millis();

    Serial.print(imu_time);
    Serial.print(" ");
    Serial.print(IMU.rpy[0]);
    Serial.print(" ");
    Serial.print(IMU.rpy[1]);
    Serial.print(" ");
    Serial.println(IMU.rpy[2]);
  }


  if( Serial.available() )
  {
    char Ch = Serial.read();

    if( Ch == '1' )
    {
      Serial.println("ACC Cali Start");

      IMU.SEN.acc_cali_start();
      while( IMU.SEN.acc_cali_get_done() == false )
      {
        IMU.update();
      }

      Serial.print("ACC Cali End ");
    }
  }
  delay(2000);   // The code i add, and the data of the IMU become error.
}

@theseankelly @ROBOTIS-Will Son @robotpilot

Hi @Huafeng-XU
I'll review this and get back to you shortly.
Thanks.

Thank you @ROBOTIS-Will

I want to get a OpenCR IMU data from a function whenever i call that function. I will prefer the IMU data function will not affect by the delay() function. Thank you so much again.

@Huafeng-XU
When using a blocking function such as delay(), it affects the processing time used for the Madgwick filter in the computeIMU function.
If you need to read the IMU sensor value, it is recommended to use an interrupt with a callback so that IMU update is not affected by blocking.
Thank you.

Ok, I got it. Thank you for let me know the details.