Go Down

Topic: Rotation along one axis. (Read 302 times) previous topic - next topic

TJHUNTER

Apr 19, 2019, 06:41 pm Last Edit: Apr 19, 2019, 07:09 pm by TJHUNTER
Hi,

I'm trying to read the roll (see picture) of a "stick" in one of my projects.
For this I'm using a GY-521 as I don't have a fixed reference point which rules pot-meters and visual sensors out.
My only problem is that I can't get an accurate roll when the sensor is at a pitch of 60° or higher.
Could anyone help me with this?
If possible I'd prefer using the I2C commands, instead of the functions of a large library.

Thank you in advance



EDIT: added code: https://pastebin.com/SAEsMA55 (as a pastebin as even without banks this got over 10.8K chars for the message)

jremington

#1
Apr 19, 2019, 06:44 pm Last Edit: Apr 19, 2019, 06:48 pm by jremington
For informed help, please read and follow the directions in the "How to use this forum" post.

Post your code, using code tags, and explain what it does versus what it should do.

Keep in mind that there are SIX different definitions of yaw, pitch and roll (Euler) angles, and a great deal of incorrect code to be downloaded.  Here is a simplified overview.

TJHUNTER

I added an edit with the code

jremington

#3
Apr 19, 2019, 07:54 pm Last Edit: Apr 19, 2019, 07:56 pm by jremington
Keep trying. People don't like to download code from mysterious sites and I certainly won't.

You can attach code to your post.

MorganS

Look up "Mahoney" or "Madgwick" algorithms to estimate position from this type of sensor.
"The problem is in the code you didn't post."

MarkT

Code: [Select]
// Use the </> button to insert code like this...
[ I will NOT respond to personal messages, I WILL delete them, use the forum please ]

TJHUNTER

#6
Apr 23, 2019, 03:24 pm Last Edit: Apr 23, 2019, 03:25 pm by TJHUNTER
If you would look at the code (no download needed) you would see that it is far over 9000 characters long (even without the banks) and you can only post up to 9000 chars.

jremington

#7
Apr 23, 2019, 04:22 pm Last Edit: Apr 23, 2019, 04:24 pm by jremington
You can attach code to a post.

To get pitch and roll, you don't need more than about 20 lines of code. See link in reply #1.

MarkT

If you would look at the code (no download needed) you would see that it is far over 9000 characters long (even without the banks) and you can only post up to 9000 chars.
You can always break it in two...

You know that to get the angles you want you cannot process the individual angles separately, 3D rotation doesn't work like that.  Typically you get the quaternion and convert to Euler angles.
[ I will NOT respond to personal messages, I WILL delete them, use the forum please ]

TJHUNTER

Code: [Select]
// Debug defines
#define DEBUG

#ifdef DEBUG
 #define DEBUG_START(baud)  Serial.begin(baud)
 #define DEBUG_AVAILABLE()  Serial.available()
 #define DEBUG_PRINT(x)     Serial.print (x)
 #define DEBUG_PRINT(x,y)     Serial.print (x,y)
 #define DEBUG_PRINTDEC(x)  Serial.print (x, DEC)
 #define DEBUG_PRINTLN(x)   Serial.println (x)
 #define DEBUG_PRINTLN(x,y)   Serial.println (x,y)
 #define DEBUG_READ()       Serial.read()
#else
 #define DEBUG_START(baud)
 #define DEBUG_AVAILABLE()
 #define DEBUG_PRINT(x)
 #define DEBUG_PRINT(x,y)
 #define DEBUG_PRINTDEC(x)
 #define DEBUG_PRINTLN(x)
 #define DEBUG_PRINTLN(x,y)
 #define DEBUG_READ()
#endif

// Variables for the I2C functions
byte i2cbyte;                  // Byte to send/receive by I2C
byte i2cinchar[20];            // 20 char buffer for i2cRB()
byte i2coutchar[16];           // 16 char buffer for i2cSB()
byte i2cnumber;                // Number of actual chars in/out of buffer
byte i2cRA;                    // Register
byte i2ccount;                 // General use
byte i2cackbit;                // Usually ignored

// Variables for the memory banks
byte mpubank;                  // Firmware bank (256 bytes) (Reg 6D)
int mpumemadd;                 // Firmware memory addr in bank (Reg 6E)
int mpufifo;                   // Fifo count 16 bit unsigned, full at 1024
boolean mpuprsw = true;        // Print full loading information if set 1

// Storage for the quaternion values
long quat[4];                  // w,x,y,z (32 bit)
long quats[4];                 // quat[] scaled (to 16 bits)

// DMP firmware kept in the flash memory
PROGMEM const byte DMPfirm[]   =  {
  /* bank # 0 */
};

void setup() {
 
  DEBUG_START(9600);         // Start the serial communication
  delay(3000);                // Delay to open the serial monitor

  // Set PORTB outputs to LOW so they can never drive high
  digitalWrite (12, LOW); // Belangrijk!!! i2c mag max 3.3V dus deze pin op high beschadigd de mpu6050
  digitalWrite (11, LOW); // op 4 pinnen want deze locaties zijn afhankelijk van je bord
  digitalWrite (14, LOW);
  digitalWrite (8, LOW);

  // Release i2c bus lines and allow GY-521 2.2K to pull up.
  pinMode (12, INPUT);        //SCL clocks going high
  pinMode (11, INPUT);        //SDA
  pinMode (14, INPUT);        //SCL clocks going high
  pinMode (8, INPUT);         //SDA
  pinMode (9, INPUT);         //Reset knop

  DEBUG_PRINTLN("You can now plug the MPU-6050 safely in.\nSend any data in the serial monitor when you pluged it in.");
  while (DEBUG_AVAILABLE() == 0); //Wait for data
  while (DEBUG_AVAILABLE() != 0) DEBUG_READ(); //Clear the buffer

  // Grab and release bus to ensure clear
  i2cstart ();
  delay(1);
  i2cstop ();

  // Is the MPU asleep? Yes ==> Load the firmware
  i2cRA = 0x6B;
  i2cnumber = 1;
  i2cmultiRB ();
  if (i2cinchar[0] != B00000001) {
    DEBUG_PRINTLN("\nLoad the DMP firmware");
    i2cRA = 0x6B;
    i2coutchar[0] = B00000001;   // Set CLKSEL maar houd in slaap
    i2cnumber = 1;
    i2cmultiSB ();
    delay(100);

    // Load the DMP firmware trough the portal (Undocumented)
    for (mpubank = 0; mpubank < 12; mpubank++) { // Cycle trough all banks

      //Show the current bank
      DEBUG_PRINTLN("\n bank # " + String(mpubank) + ":");
     
      for (mpumemadd = 0; mpumemadd < 241; mpumemadd = mpumemadd + 16) { // Cycle trough all memory addresses
loaderror:   //Try again from here if there is an error
        mpusendbank();           //send bank and mem start
 
        for (int i = 0; i < 16; i++) { //load the I2C send buffer and send
          int k = mpubank * 256;
          k = k + mpumemadd;
          k = k + i;
          i2coutchar[i] = pgm_read_byte_near (DMPfirm + k);
        }                        // repeat for loop if condition is true
       
        i2cRA = 0x6F;            // DMP firmware portal
        i2cnumber = 16;          // 16 bytes
        i2cmultiSB ();           //send 16 bytes via portal

        mpusendbank();           //send same bank and mem start
        i2cmultiRB ();           //read back the 16 bytes into i2cinchar[]

        boolean k = false;
        for (int i = 0; i < 16; i++) { // Confirm correct firmware load byt comparing in and out bytes
          if (i2cinchar[i] != i2coutchar[i]) k = true; // Error
        }                        // repeat for loop if condition is true
        if (k) {            // was there an errror?
          DEBUG_PRINTLN(" error");
          delay(200);
          goto loaderror;
        }
       
        if (mpuprsw) {      // Print the values if this is true (set in setup)
          DEBUG_PRINT("  block " + String(mpumemadd) + ":\t");
         
          for (int i = 0; i < 16; i++) {
            DEBUG_PRINT(" ");
            DEBUG_PRINT(i2cinchar[i], HEX);
          }                      // Repeat for loop while the condition is true
         
          DEBUG_PRINTLN("");     // Print \n\r
        }                        // End of if
      }                          // Next block mpumemadd
    }                            // Next mpubank

    //Regs 112 and 113 (70 & 71) have DMP program start address $0400
    i2cRA = 0x70;                //unlisted Regs
    i2cnumber = 2;
    i2coutchar[0] = 0x04;
    i2coutchar[1] = 0x00;
    i2cmultiSB ();               //send 2 bytes
    DEBUG_PRINTLN("\nDMP loaded");
  }                            //end of "if not loaded"

  i2cRA = 0x1B;                //Reg 27 (1B) gyro config
  i2cnumber = 1;
  i2coutchar[0] = B00011000;   //FSR 2000dps mandated for DMP fusion
  i2cmultiSB ();

  i2cRA = 0x6A;                //Reg 106 (6A) user control
  i2coutchar[0] = B11000000;   //enable DMP and FIFO (reserved bit)
  i2cmultiSB ();
 
}                             //end of setup

TJHUNTER

Code: [Select]

void loop() {
  
  digitalWrite (13, LOW);           // Diagnostic for fifo rate
  i2cRA = 0x72;                     // Reg 114,115 FIFO count
  i2cnumber = 2;
  i2cmultiRB ();
  mpufifo = i2cinchar[0];           // Save the fifo
  mpufifo = mpufifo << 8;           // Shift left as this is an int stored in 2 bytes
  mpufifo = mpufifo | i2cinchar[1];

  if (mpufifo >= 20) {
    getQuaternions ();              // Get the quaternion formated output of the DMP

    // Scale to 16 bits ( = int) and save in quats to to keep integrity test within long arithmetic
    quats[0] = quat[0] >> 16;
    quats[1] = quat[1] >> 16;
    quats[2] = quat[2] >> 16;
    quats[3] = quat[3] >> 16;
    
    long normsq = quats[0] * quats[0] + quats[1] * quats[1] + quats[2] * quats[2] + quats[3] * quats[3];
    
    if (normsq > 285212672 |  normsq < 251658240) {
      //errors seen in quaternion - reset the fifo and bin this input
      i2cRA = 0x6A;                // User control
      i2cnumber = 1;
      i2coutchar[0] = B10000000;   // Enable DMP (reserved bit) and disable FIFO
      i2cmultiSB ();
      i2coutchar[0] = B10000100;   // Enable DMP (reserved bit) and reset FIFO
      i2cmultiSB ();
      i2coutchar[0] = B11000000;   // Enable DMP and FIFO
      i2cmultiSB ();
      DEBUG_PRINT("  reset fifo ");
      return; // Loop is called continius so it will go to the beginning again
    } //end of quaternion error handling


    // Save into new names to make the math easier to overview
    long w = quat[0];
    long x = quat[1];
    long y = quat[2];
    long z = quat[3];

    // Scale factor is equal to the sensor current range
    w = w / 32768.0f;
    x = x / 32768.0f;
    y = y / 32768.0f;
    z = z / 32768.0f;

    // Print scaled quaternion
    /*Serial.print(" w ");
    Serial.print(w);
    Serial.print("  x ");
    Serial.print(x);
    Serial.print("  y ");
    Serial.print(y);
    Serial.print("  z ");
    Serial.print(z);*/

    // Quaternion to Eular
    float gx = 2 * (x * z - w * y);
    float gy = 2 * (w * x + y * z);
    float gz = w * w - x * x - y * y + z * z;
    
    float yaw = atan2(2 * x * y - 2 * w * z, 2 * w * w + 2 * x * x - 1); // Z axis
    float pitch = atan2(gx, sqrt(gy * gy + gz * gz)); // Y axis
    float roll = atan2(gy, gz); // X axis

    // Print Eular angles and modified roll
    DEBUG_PRINT("  yaw ");
    DEBUG_PRINT(yaw * 180/M_PI,2);
    DEBUG_PRINT("  pitch ");
    DEBUG_PRINT(pitch * 180 / M_PI, 2);
    DEBUG_PRINT(" roll ");
    DEBUG_PRINT(roll * 180 / M_PI);
    DEBUG_PRINT("  sideways ");
    if (pitch > 0) DEBUG_PRINTLN((roll * 180 / M_PI) * (1 / (1 + pow(1.293, ((pitch * 180 / M_PI) - 51.57)))), 2);
    else if (pitch == 0) DEBUG_PRINTLN(roll * 180 / M_PI, 2);
    else if (pitch < 0) DEBUG_PRINTLN((roll * 180 / M_PI) * (1 / (1 + pow(1.293, (((pitch) * (-180) / M_PI) - 51.57)))), 2);

  }                            //end of "if fifo >= 20"
}                              //end of loop

// Function to get and save all the quaternions as a long
long getQuaternions () {
  //unload fifo 20 bytes into i2cinchar[]
  i2cRA = 0x74;                   //Reg 116 (74) FIFO data port
  i2cnumber = 20;
  i2cmultiRB ();
  digitalWrite (13, HIGH);        //oscilloscope analysis of rate
    
  quat[0] = ((long)i2cinchar[0] << 24) | ((long)i2cinchar[1] << 16) | ((long)i2cinchar[2] << 8) | i2cinchar[3];   // Join all bytes for the quaternion w aka 1 or real part
  quat[1] = ((long)i2cinchar[4] << 24) | ((long)i2cinchar[5] << 16) | ((long)i2cinchar[6] << 8) | i2cinchar[7];   // Join all bytes for the quaternion x aka i = 2nd dimention ≠ 2D
  quat[2] = ((long)i2cinchar[8] << 24) | ((long)i2cinchar[9] << 16) | ((long)i2cinchar[10] << 8)| i2cinchar[11];  // Join all bytes for the quaternion y aka j = 3rd dimention ≠ 3D
  quat[3] = ((long)i2cinchar[12] << 24)| ((long)i2cinchar[13] << 16)| ((long)i2cinchar[14] << 8)| i2cinchar[15];  // Join all bytes for the quaternion z aka k = 4th dimention ≠ 4D
}

// Function to send bank and mem start address for each block
void mpusendbank () {
  i2cstart ();
  i2cbyte = 0xD0;             // Device address + write
  i2cSB ();
  i2cbyte = 0x6D;             // Unlisted Regs for firmware load
  i2cSB ();
  i2cbyte = mpubank;          // Set the bank number for the DMP firmware portal
  i2cSB ();
  i2cbyte = mpumemadd;        // Set the register number for the DMP firware portal
  i2cSB ();
  i2cstop ();
}

Go Up