BNO055 Quaternion to Euler

BNO055 Quaternion to Euler

In the previous blog, I mentioned getting quaternion output from BMO055 and this time I just confirmed that the quaternion to euler (Pitch, Roll, Yaw) conversion is same as the euler output from BNO055.

The quaternion to euler conversion has several sequences depending on how you rotate the axis. 


For example, X axis - Y axis - Z axis, Y axis - X axis - Z axis, Z axis - X axis- Y axis, and so on.

This time, I just wanted to compare the conversion with the euler output from BNO055, so I simply calculated the euler from quaternion and compared with pre-calculated euler output.


Best Sensor Kit (Recommended!)

       


Equation

So here is the equation.
The sequence used here is Y - X - Z (2 - 1 - 3) and part of my processing code is listed below.

euler[0] = -atan2(2 * q[1] * q[3] + 2 * q[2] * q[0], 1 - 2 * q[1]*q[1] - 2 * q[2] * q[2])*180/PI;
euler[1] = asin(2 * q[2] * q[3] - 2 * q[1] * q[0])*180/PI; 
euler[2] = -atan2(2 * q[1] * q[2] + 2 * q[3] * q[0], 1 - 2 * q[2] * q[2] - 2 * q[3] * q[3])*180/PI;

GPU and 10DOF IMU (Recommended!)

Here, euler[0] is pitch, euler[1] is roll, and euler[2] is yaw angle. Also, q[0] is w, q[1] is x, q[2] is y, and q[3] is z.

You may notice the yaw angle is different around the +180 degree and -180 degree since the euler output from BNO055 is actually 0 degree to 360 degree.

To make it exactly same, you would need to generate case statement to convert the range from +/-180 degree to 0-360 degree.

With this equation, I was able to obtain the same result with the pre-calculated euler angle from BNO055.
So my code is basically just getting quaternion from BNO055 and convert it to euler if needed.

Quaternion is actually better to handle graphical rotations, and now I created simple graphical software using processing.
It is quite messy, so I will consider sharing the source code in the future if demand is high. :)



                                                   


For the people who wants better performance, I would recommend BNO080.
BNO055 is really old components although it is still widely used.


Arduino and BNO055

Arduino (Featherboard M0) and BNO055

BMI055 is the 9 axis MEMS inertial (3 axis Accelerometer + 3 axis Gyroscope + 3 axis Magnetometer) that is used for motion detection, gesture recognition, pedometer, orientation tracking, and so on. It has analog-to--digital converter inside and supports digital interface (SPI or I2C) to obtain the inertial data of gravity and/or acceleration as G and angular velocity as DPS (degree per second). In addition, there is a DSP that runs sensor fusion and you can obtain the calculated results as either Quaternion or Euler Angle (Pitch, Roll, and Yaw).
The driver code is available from here.
The breakout board is available from here.


Best Sensor Kit (Recommended!)

       


Code

Since I am just using Arduino IDE for this test using Adafruit feather M0 board, I downloaded the driver code from the above link and added extracted files under the libraries folder of Arduino IDE.
I would recommend to try one of the example code that Adafruit BNO055 library provides which is "restore_offsets".
This is very nice sample code to calibrate the sensor offsets before starting the coding deeply. This code actually give you the steps to calibrate the sensor, store the data to flash memory, and load the values to sensor to compensate the raw sensor outputs.
The calibration procedure is not so complicated but it took a little bit of time especially for accelerometer calibration. Gyro is easy and you just keep the device still. Rotating the device as you figure 8 will calibrate the magnetometer. Each sensor has the calibration level, 1, 2, and 3, and you will see all 3 when it is fully calibrated. You can refer to the setup function for this process as listed below.
    else
    {
        Serial.println("Please Calibrate Sensor: ");
        while (!bno.isFullyCalibrated())
        {
            bno.getEvent(&event);
            Serial.print("X: ");
            Serial.print(event.orientation.x, 4);
            Serial.print("\tY: ");
            Serial.print(event.orientation.y, 4);
            Serial.print("\tZ: ");
            Serial.print(event.orientation.z, 4);
            /* Optional: Display calibration status */
            displayCalStatus();
            /* New line for the next sample */
            Serial.println("");
            /* Wait the specified delay before requesting new data */
            delay(BNO055_SAMPLERATE_DELAY_MS);
        }
    }
Once the calibration is completed, this program stores the data to memory. Next time when you run the program, it will load the stored values from the memory and write the values to the offset registers of the sensor. You can refer to the beginning of the setup function in "restore_offsets.ino" as listed below.
    /*
    *  Look for the sensor's unique ID at the beginning of EEPROM.
    *  This isn't foolproof, but it's better than nothing.
    */
    bno.getSensor(&sensor);
    if (bnoID != sensor.sensor_id)
    {
        Serial.println("\nNo Calibration Data for this sensor exists in EEPROM");
        delay(500);
    }
    else
    {
        Serial.println("\nFound Calibration for this sensor in EEPROM.");
        eeAddress += sizeof(long);
        EEPROM.get(eeAddress, calibrationData);
        displaySensorOffsets(calibrationData);
        Serial.println("\n\nRestoring Calibration data to the BNO055...");
        bno.setSensorOffsets(calibrationData);
        Serial.println("\n\nCalibration data loaded into BNO055");
        foundCalib = true;
    }

GPU and 10DOF IMU (Recommended!)

Next is the loop function and it is very simple.
void loop() {
    /* Get a new sensor event */
    sensors_event_t event;
    bno.getEvent(&event);
    /* Display the floating point data */
    Serial.print("X: ");
    Serial.print(event.orientation.x, 4);
    Serial.print("\tY: ");
    Serial.print(event.orientation.y, 4);
    Serial.print("\tZ: ");
    Serial.print(event.orientation.z, 4);
    /* Optional: Display calibration status */
    displayCalStatus();
    /* Optional: Display sensor status (debug only) */
    //displaySensorStatus();
    /* New line for the next sample */
    Serial.println("");
    /* Wait the specified delay before requesting new data */
    delay(BNO055_SAMPLERATE_DELAY_MS);
}

It is basically calling the "getEvent" function in the Adafruit library and this gives you the Euler angle which is pitch, roll, and yaw as you can refer to the function below.
bool Adafruit_BNO055::getEvent(sensors_event_t *event)
{
  /* Clear the event */
  memset(event, 0, sizeof(sensors_event_t));
  event->version   = sizeof(sensors_event_t);
  event->sensor_id = _sensorID;
  event->type      = SENSOR_TYPE_ORIENTATION;
  event->timestamp = millis();
  /* Get a Euler angle sample for orientation */
  imu::Vector<3> euler = getVector(Adafruit_BNO055::VECTOR_EULER);
  event->orientation.x = euler.x();
  event->orientation.y = euler.y();
  event->orientation.z = euler.z();
  return true;
}

                                                     


"displayCalStatus" is just getting the calibration level (1-3) and printing out to the terminal.
This is useful to see if the calibration level is always good when you are acquiring sensor data.
You can change the definition of "BNO055_SAMPLERATE_DELAY_MS" which decides the sampling rate of the event loop. The default number is 100 which is 10Hz.

If you want to get the quaternion instead, you can call the "getQuat" function as listed below.
imu::Quaternion Adafruit_BNO055::getQuat(void)
{
  uint8_t buffer[8];
  memset (buffer, 0, 8);
  int16_t x, y, z, w;
  x = y = z = w = 0;
  /* Read quat data (8 bytes) */
  readLen(BNO055_QUATERNION_DATA_W_LSB_ADDR, buffer, 8);
  w = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]);
  x = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]);
  y = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]);
  z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]);
  /* Assign to Quaternion */
  /* See http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_12~1.pdf
     3.6.5.5 Orientation (Quaternion)  */
  const double scale = (1.0 / (1<<14));
  imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z);
  return quat;
}

You can add something like the following code in your loop function.
  // Quaternion data
  imu::Quaternion quat = bno.getQuat();
  Serial.print(quat.w(), 4);
  Serial.print(" ");
  Serial.print(quat.y(), 4);
  Serial.print(" ");
  Serial.print(quat.x(), 4);
  Serial.print(" ");
  Serial.println(quat.z(), 4);

Some people might want to convert from Quaternion to Euler and you can refer to another post here for the quick reference.



Featherboard M0 and BMI160

Featherboard M0 and BMI160

BMI160 is the 6 axis MEMS inertial (3 axis Accelerometer + 3 axis Gyroscope) that is used for motion detection, gesture recognition, pedometer, orientation tracking, and so on. It has analog-to--digital converter inside and supports digital interface (SPI or I2C) to obtain the inertial data of gravity and/or acceleration as G and angular velocity as DPS (degree per second).
The driver code is available from here.
The breakout board is available from here.

Code

Since I am just using Arduino IDE for the test, I downloaded the driver code from the above link and renamed bmi160.c as bmi160.ino.
I did not add the driver file to the Arduino library folder this time.
My project folder includes bmi160-test.ino, bmi160_support.h, bmi160.ino, and bmi160.h.
bmi160-test.ino is basically referring to the bmi160_support.c which is included in the sample driver files. The functions defined in the bmi160_support.h are defined in the bmi160-test.ino.
Now let's see the each key functions.

I2C write function is listed below.
s8 bmi160_i2c_bus_write(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt)
{
  s32 ierror = BMI160_INIT_VALUE;
  Wire.beginTransmission(dev_addr);
  Wire.write((uint8_t)reg_addr);

  for (uint8_t i = 0; i < cnt; i++)
  {
       Wire.write(reg_data[i]);
  }
 
  Wire.endTransmission();
  return (s8)ierror;
}

I2C read functions is like this.
s8 bmi160_i2c_bus_read(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt)
{
  s32 ierror = BMI160_INIT_VALUE;

  Wire.beginTransmission(dev_addr);
  Wire.write((uint8_t)reg_addr);
  Wire.endTransmission();
  Wire.requestFrom(dev_addr, (byte)cnt);
  for (uint8_t i = 0; i < cnt; i++)
  {
      reg_data[i] = Wire.read();
  }
  return (s8)ierror;
}

The above 2 functions are referred by the "i2c_routine" function and make sure the function name is same as the one referred from this function.

Next is the setup function.
void setup()
{
  Serial.begin(115200);
  Wire.begin();
  while(!Serial);

  bmi160_read_reg(BMI160_USER_CHIP_ID__REG, &id, BMI160_GEN_READ_WRITE_DATA_LENGTH);
  Serial.print("Device ID: ");
  Serial.println(id, HEX);
  delay(1000);
  Serial.println("Start Initialization");
  bmi160_initialize_sensor();
  Serial.println("End Initialization");
}
It is very useful to add "while(!Serial)" before you print out any debug comment. This sentence waits until the serial terminal is connected so you will not miss the debug messages.
The function called "bmi160_initialize_sensor()" is the same as the one defined in the original "bmi160_support.c" file and you can just modify the following line depending on what mode you want to use.

com_rslt += bmi160_config_running_mode(STANDARD_UI_IMU);
 In my case, I am using "STANDARD_UI_IMU" mode and as you can see the comments in the original "bmi160_support.c" file, the following modes are supported in this code.
 *      Description                                                     |  value
 * ----------------------------------------------------------|----------
 *  STANDARD_UI_9DOF_FIFO                         |   0
 *  STANDARD_UI_IMU_FIFO                           |   1
 *  STANDARD_UI_IMU                                      |   2
 *  STANDARD_UI_ADVANCEPOWERSAVE   |   3
 *  ACCEL_PEDOMETER                                     |   4
 *  APPLICATION_HEAD_TRACKING              |   5
 *  APPLICATION_NAVIGATION                        |   6
 *  APPLICATION_REMOTE_CONTROL           |   7
 *  APPLICATION_INDOOR_NAVIGATION       |   8
Next is the loop function.
void loop()
{
  // If data ready bit set, all data registers have new data
  bmi160_read_reg(0x1B, &dr, BMI160_GEN_READ_WRITE_DATA_LENGTH);
  if(dr & 0xC0) {  // check if data ready interrupt  
    bmi160_read_gyro_xyz(&gxyz);
    bmi160_read_accel_xyz(&axyz);
    Now = micros();
  }

    gx = (float)gxyz.x/GYRO_SENS;
    gy = (float)gxyz.y/GYRO_SENS;
    gz = (float)gxyz.z/GYRO_SENS;
    ax = (float)axyz.x/ACC_SENS;
    ay = (float)axyz.y/ACC_SENS;
    az = (float)axyz.z/ACC_SENS;
    deltat = ((Now - lastUpdate)/1000000.0f);
    lastUpdate = Now;
 
    Serial.print("ax = "); Serial.print(ax);
    Serial.print(" ay = "); Serial.print(ay);
    Serial.print(" az = "); Serial.print(az); Serial.println(" g");
    Serial.print("gx = "); Serial.print( gx, 2);
    Serial.print(" gy = "); Serial.print( gy, 2);
    Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s");
               
    Serial.print("rate = "); Serial.print((float)1/deltat, 2); Serial.println(" Hz");
    Serial.println("");
}
GYRO_SENS and ACC_SENS are the sensitivity value of Gyroscope and Accelerometer respectively. I am using the default register settings for those 2 parameters, so simply referring to the datasheet and defined the values as below.
ACC_SENS = 16384 (LSB/g)
GYRO_SENS = 16.4 (LSB/dps)
 By dividing the raw data of each sensor values, you can convert unit from the ADC counts to g or dps units.

Test

There are some bias/offset on both Gyroscope and Accelerometer axis. This will cause errors if you need to calculate orientation from the raw values. Therefore, it is required to do the calibration to cancel the initial bias values. Next step is to implement this to see how the output looks like.



                 

[AKM Chip Booster] Audio ADC AK5704 PCB Design

Designing a PCB prototype with AK5704 is not so difficult and I show an example with my design. People who are not familiar with AK5704,...