BNO055 Rotation Matrix

[DjangoTango]

Hi,

I am using an Arduino Uno wifi rev2. Initially, I did it to get the acceleration from the integrated sensor of the board. Then I bought a BNO055 to record the angle of an oscillation, thus the yaw with almost no drift. It works properly because I need to recognize the maximum and the minimum of the object that does the oscillation. The problem is when the object change direction. Imagine having a pendulum that drives in a direction and then comes back in the opposite direction. The inversion creates a jump 0-360 or 360-0 of the Euler.x value and the threshold to recognize the max and min are in a complete another range in which they don't work. Can you give me some tips or useful links, please? Also, pieces of sketches could be very appreciated if someone has just experienced it.

Thank You!!

[jps2000]

You should not use euler angles from the BNO055. They do some strange math that adopts the algorithm depending on the orientation.
Use quaternions instead and calculate eulers from them.

[DjangoTango]

Really thank you for the interest. I tried to do it, here is the code. It seems that it doesn't work properly. By the way I would like to post an image of the first result with the only euler data. In red a way, in black the opposite one. As you can see there is the switch 0-360. I need to put one threshold for the maximum and one for he minimum. Can you help me to achieve this aim?

Code: Select all

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>

/* Set the delay between fresh samples */
#define BNO055_SAMPLERATE_DELAY_MS (100)

Adafruit_BNO055 bno = Adafruit_BNO055(55);

/**************************************************************************/
/*
    Displays some basic information on this sensor from the unified
    sensor API sensor_t type (see Adafruit_Sensor for more information)
*/
/**************************************************************************/
void displaySensorDetails(void)
{
  sensor_t sensor;
  bno.getSensor(&sensor);
  Serial.println("------------------------------------");
  Serial.print  ("Sensor:       "); Serial.println(sensor.name);
  Serial.print  ("Driver Ver:   "); Serial.println(sensor.version);
  Serial.print  ("Unique ID:    "); Serial.println(sensor.sensor_id);
  Serial.print  ("Max Value:    "); Serial.print(sensor.max_value); Serial.println(" xxx");
  Serial.print  ("Min Value:    "); Serial.print(sensor.min_value); Serial.println(" xxx");
  Serial.print  ("Resolution:   "); Serial.print(sensor.resolution); Serial.println(" xxx");
  Serial.println("------------------------------------");
  Serial.println("");
  delay(500);
}

/**************************************************************************/
/*
    Arduino setup function (automatically called at startup)
*/
/**************************************************************************/
void setup(void)
{
  Serial.begin(115200);
  Serial.println("Orientation Sensor Test"); Serial.println("");

  /* Initialise the sensor */
  if(!bno.begin())
  {
    /* There was a problem detecting the BNO055 ... check your connections */
    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
    while(1);
  }
  
uint8_t system, gyro, accel, mg = 0;
  bno.getCalibration(&system, &gyro, &accel, &mg);

  int calibration = system+gyro+accel+mg;


  while (calibration != 12) {
    uint8_t system, gyro, accel, mg = 0;
    bno.getCalibration(&system, &gyro, &accel, &mg);
    calibration = system+gyro+accel+mg;

    Serial.print(system);
    Serial.print(", ");
    Serial.print(gyro);
    Serial.print(", ");
    Serial.print(accel);
    Serial.print(", ");
    Serial.print(mg);
    Serial.print(", ");
    Serial.println(calibration);
    }
  delay(1000);
 
  /* Display some basic information on this sensor */
  displaySensorDetails();

}

/**************************************************************************/
/*
    Arduino loop function, called once 'setup' is complete (your own code
    should go here)
*/
/**************************************************************************/
void loop()
{
  /* Get a new sensor event */
  sensors_event_t event;
  bno.getEvent(&event);

  /* Board layout:
         +----------+
         |         *| RST   PITCH  ROLL  HEADING
     ADR |*        *| SCL
     INT |*        *| SDA     ^            /->
     PS1 |*        *| GND     |            |
     PS0 |*        *| 3VO     Y    Z-->    \-X
         |         *| VIN
         +----------+
  */

  /* Convert quaternion to Euler, because BNO055 Euler data is broken */
  imu::Quaternion q = bno.getQuat();
  q.normalize();
  float temp = q.x();  q.x() = -q.y();  q.y() = temp;
  q.z() = -q.z();
  imu::Vector<3> euler = q.toEuler();
  Serial.print(F("Orientation: "));
  Serial.print(-180/M_PI * euler.x());  // heading, nose-right is positive, z-axis points up
  Serial.print(F(" "));
  Serial.print(-180/M_PI * euler.y());  // roll, rightwing-up is positive, y-axis points forward
  Serial.print(F(" "));
  Serial.print(-180/M_PI * euler.z());  // pitch, nose-down is positive, x-axis points right
  Serial.println(F(""));



  delay(BNO055_SAMPLERATE_DELAY_MS);
}

[gammaburst]

Be careful when borrowing code snippets. Your quaternion-to-Euler conversion code came from my 2016 example, and it contains a deliberate math bug that counteracts an opposite math bug in cuberotate.pde. (It swaps and flips some axes.) Here's the original discussion:
viewtopic.php?f=19&t=91723#p462337

Your graph shows only one quaternion component and one Euler component. Euler angles have three components, quaternions have four. Troubleshooting requires all components.

Which brand/model sensor module are you using? Which way is it mounted? Which way is it swinging?

Maximum clarity is important when discussing 3D orientation problems. This isn't easy!

[DjangoTango]

My bad.
I was interesting only to the heading and so I show plotted it. Practically I would like to wear one Imu sensor of adafruit bno055 at the level of the thigh and during my gait I want to measure the oscillation of leg and recognize the maximum and minimum. I thought that relative orientation is the way. Can you guide me over the solution, please?

[sj_remington]

The BNO055 sensor is supposed to produce absolute orientation, fixed to an Earth frame of reference, which makes it quite complicated to measure a swinging motion in an arbitrary plane.

For your purpose, relative orientation is probably more useful, and for that all you need is a rate gyro. Integrate the gyro output with respect to time to accomplish the task. If you are interested, I can post code that demonstrates how to do that. Data analysis will not be trivial, no matter how you make the measurement.

[jps2000]

You may use the gravity vector and calculate the angle to vertical. Sensor should be however, mounted on a leg.
https://github.com/jps2000/BNO080/blob/master/utilities

[DjangoTango]

I tried to use the gyroscope, but interprating data is weird. Am I doing it good? Indeed there is no logic on the output

Code: Select all

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>

/* Set the delay between fresh samples */
#define BNO055_SAMPLERATE_DELAY_MS (100)

// Check I2C device address and correct line below (by default address is 0x29 or 0x28)
//                                   id, address
Adafruit_BNO055 bno = Adafruit_BNO055(-1, 0x28);

void setup(void)
{
  Serial.begin(115200);
  Serial.println("Orientation Sensor Raw Data Test"); Serial.println("");

  /* Initialise the sensor */
  if(!bno.begin())
  {
    /* There was a problem detecting the BNO055 ... check your connections */
    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
    while(1);
  }

  delay(1000);

  bno.setExtCrystalUse(true);

  Serial.println("Calibration status values: 0=uncalibrated, 3=fully calibrated");
  uint8_t system, gyro, accel, mg = 0;
  bno.getCalibration(&system, &gyro, &accel, &mg);

  int calibration = system+gyro+accel+mg;

  while (calibration != 12) {
    uint8_t system, gyro, accel, mg = 0;
    bno.getCalibration(&system, &gyro, &accel, &mg);
    calibration = system+gyro+accel+mg;

    Serial.println(calibration);
   
  }
}


/**************************************************************************/
/*
    Arduino loop function, called once 'setup' is complete (your own code
    should go here)
*/
/**************************************************************************/
void loop(void)
{
  // Possible vector values can be:
  // - VECTOR_ACCELEROMETER - m/s^2
  // - VECTOR_MAGNETOMETER  - uT
  // - VECTOR_GYROSCOPE     - rad/s
  // - VECTOR_EULER         - degrees
  // - VECTOR_LINEARACCEL   - m/s^2
  // - VECTOR_GRAVITY       - m/s^2
  imu::Vector<3> gyro = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);

  /* Display the floating point data */
  Serial.print("X: ");
  Serial.print(gyro.x());
  Serial.print(" Y: ");
  Serial.print(gyro.y());
  Serial.print(" Z: ");
  Serial.print(gyro.z());
  Serial.println("\t\t");

  delay(BNO055_SAMPLERATE_DELAY_MS);
}

[DjangoTango]

You may use the gravity vector and calculate the angle to vertical. Sensor should be however, mounted on a leg.
https://github.com/jps2000/BNO080/blob/master/utilities

I'm sorry but I need more explanation to manage your code. Can you be patient with me?

[DjangoTango]

I think that if you can show me your type of solution code I can discuss better about it and understand better than now. Can I ask you this favor?

[jps2000]

you´re welcome
The gravity vector points always to the center of earth. It is a unity vector.
You get the three components (x,y,z) from quaternions

Code: Select all

 
// Gravity vector from quaternion
  
  gx = q1 * q3 - q0 * q2;
  gy = q0 * q1 + q2 * q3;
  gz = q0 * q0 + q3 * q3 -0.5; 
  norm = sqrt(gx * gx + gy * gy + gz * gz);                                                              
  norm = 1.0 / norm;
  gx *= norm; gy *= norm; gz *= norm;

quaternions above are meant like this
(q0,q1,q2,q3) = (w,ix,jy,kz)
when you have your sensor flat on the table then the z component is vertical.
So you can calculate

Code: Select all

 phi = acos(gz); phi *= radtodeg;

to get the angle of your sensor to vertical direction

[DjangoTango]

It gives me 180 only. So how can I use It to compare with the angle that I am doing in real time?

Code: Select all

void loop () {

imu::Quaternion q = bno.getQuat();
   
  gx = q1 * q3 - q0 * q2;
  gy = q0 * q1 + q2 * q3;
  gz = q0 * q0 + q3 * q3 -0.5f; 
  norm = sqrtf(gx * gx + gy * gy + gz * gz);                                                              
  norm = 1.0f / norm;
  gx *= norm; gy *= norm; gz *= norm;
  phi = acos(gz); phi = phi* 57296 / 1000;
  Serial.println(phi);
 

}

[jps2000]

your code can not work because
this is missing

Code: Select all

// Quaternion data
  imu::Quaternion quat = bno.getQuat();
q0= quat.w();
q1=quat.x();
and so on
  ;

[DjangoTango]

You are obviously right. I was confusing looking at only q and not the general "quat". Indeed I fix it but in the serial monitor the output is completely blank. After the setup it seems to be stopped

[jps2000]

debug it step by step.
so plot quaternions first