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.



                 

OV7670 + Lepton Module

OV7670 + Lepton Using Featherboard M0

This is camera streaming test using Featherboard M0OV7670, and also Lepton.
I previously tested OV7670 and Lepton individually, and this time I tried to combine both OV7670 and Lepton so that I can see the RGB image and thermal image at the same time.
I was assuming that it would not work well because of the mechanical alignment but this quick test is just to see how it works and get an idea for the next step.

Connection

This time, I decided to use an interface board between featherboard and each sensors rather than using wires.
Here is the hardware setup of my test.

The green board is the interface board between OV7670 module, Lepton module and Featherboard M0.
OV7670 and Lepton sensor do not align perfectly, but it is much better than using jumper wires that I have been using the previous camera tests.

You can enable thermal image with your phone!

Test

Here is the test video. I am using Arduino IDE and Processing for this test.


Left side image is OV7670 module and right side image is Lepton module.
The frame rate is not high at this moment, but this would be something that can be improved in the future.


FLIR Lepton Module

FLIR Lepton Module Test Using Featherboard M0

This is thermal image sensor streaming test using Featherboard M0 and FLIR Lepton sensor.
FLIR lepton sensor is 80x60 resolution and you can use I2C and SPI to communicate with the sensor. The module breakout board is available here.

Connection

Here is the connection.
Lepton Featherboard
CS GND
MOSI MOSI
MISO MISO
CLK SCK
GND GND
VIN 3.3V
SDA
SCL
I am not using I2C at this moment and just getting the image data with the default settings.

You can enable thermal image with your phone!

Test

Here is the video and I am using Arduino IDE and Processing.


                                                     

OV7670 Camera Module with FIFO

OV7670 Camera Module with FIFO Using Featherboard M0

This is camera streaming test using Featherboard M0 and OV7670.

First of all, it is difficult to stream video using this M0 core.. so I decided to use the camera module that has FIFO.
With this implementation, MCU can just grab the images from the FIFO and send it to the host without considering the timing of camera interface.

OV7670 camera module I am using is has AL422B FIFO behind the camera and you can find the datasheets here and here.


Connection

My connection between Featherboard M0 and OV7670 module is listed below.
OV7670 Featherboard
1 VDD
2 GND
3 SCL
4 SDA
5 9
6
7 6
8 5
9 A5
10 A4
11 A3
12 A2
13 A1
14 A0
15 VDD
16 GND
17
18 10
19 11
20 GND
21 13
22 12

I2C bus requires the pull-up registers and I have 4.7k-ohm registers to 3.3V on both SDA and SCL.

Test

Here is the video of the test code.

As you can see in the video, frame rate is not so fast at this moment (less than 4fps), but it is good enough to use this as small camera project like monitoring system.



                 

Featherboard M0 Basic ~GPIO~

Featherboard M0 Basic ~GPIO~


This is the GPIO test of Featherboard M0 using Arduino IDE. 
I sometimes need to toggle the GPIO as fast as possible, and it is difficult to achieve the requirements using Arduino API, such as digitalWrite function.
In this case, you need to check the necessary source code under the Arduino package like \hardware\samd\1.x.x\cores\arduino and find the functions to optimize the behavior.


digitalWrite Function

digitalWrite function is defined in the wiring_digital.c file.
void digitalWrite( uint32_t ulPin, uint32_t ulVal )
{
  // Handle the case the pin isn't usable as PIO
  if ( g_APinDescription[ulPin].ulPinType == PIO_NOT_A_PIN )
  {
    return ;
  }

  // Enable pull-up resistor
  PORT->Group[g_APinDescription[ulPin].ulPort].PINCFG[g_APinDescription[ulPin].ulPin].reg=(uint8_t)(PORT_PINCFG_PULLEN) ;

  switch ( ulVal )
  {
    case LOW:
      PORT->Group[g_APinDescription[ulPin].ulPort].OUTCLR.reg = (1ul << g_APinDescription[ulPin].ulPin) ;
    break ;

    default:
      PORT->Group[g_APinDescription[ulPin].ulPort].OUTSET.reg = (1ul << g_APinDescription[ulPin].ulPin) ;
    break ;
  }

  return ;
}
As you can see, ulval is used in the switch statement and if it is 0, it clears the port, and if it is 1, it sets the port.
If you are sure which port you access and the pin configuration, the lines before the switch statement would not be needed which saves the time used for those operations.
Also, if you do not need to have the function and directly control the pins from the main code, you can just reuse the lines written in the switch statement.
PORT->Group[g_APinDescription[ulPin].ulPort].OUTCLR.reg = (1ul << g_APinDescription[ulPin].ulPin) ;
PORT->Group[g_APinDescription[ulPin].ulPort].OUTSET.reg = (1ul << g_APinDescription[ulPin].ulPin) ;
When you use those lines in the main function, you need to replace ulPin with the actual values based on the pin you want to use.
You can find the variant.cpp file and WVariant.h.
If you need it to be more faster, you can assign specific bit values instead of the bit shift operation and also assign the right number to the Group[].
For example, using digital pin 13, the set and clear can be done something like below.
PORT->Group[0].OUTCLR.reg = 0x00020000;
PORT->Group[0].OUTSET.reg = 0x00020000;
This will make it much faster than using digitalWrite function.

Grid-Eye Module Test

Grid-Eye Module Test

Panasonic Grid-Eye is 8x8 infrared array sensor.
This sensor supports I2C interface and you can get each 64 elements temperature data at 10Hz rate. 
There is a great software example to test the sensor using Arduino here and the processing code is available here.

Test Setup

Here is my test setup. 
Using Arduino Uno for this quick test.

Grid-Eye has two versions of power supply voltage 3.3V and 5.0V.
I am using 3.3V version. 
VIH of Arduino seems to be 3.0V, so as long as the I2C pull-up resistor value and bus speed are not so high, it should work properly. 
(Since it only has interrupt output and I2C, I just checked VIH.)



You can enable thermal image with your phone!

       

Test Run

Here is the video running the processing code.

There is 8x8 grid and the number in each grid is the temperature reading from the sensor.
I am moving my finger left and right, and as you can see, the red part (high temperature) is moving left and right as well.


If you want more pixel or resolution, use lepton and check this post!




                 

Featherboard M0 Basic Proto

Featherboard M0 Basic Proto


Featherboard M0 Basic Proto is very simple design and easy to use ARM M0 based board.
Arduino IDE supports this board and most of the sample code works with it.
The great feature is that it has a battery connector and it can be used as portable devices without having extra board like other MCU boards.
The MCU is actually same as Arduino M0/Zero which is  ATSAMD21G18 48MHz, but the board is much smaller. The technical details are available here.

Featherboard has other versions with WiFi, Bluetooth, and so on, so depending on what you would like to do, you can get the one with those features.

Setup


Follow the setup instruction here and here.
How it works is a little bit different from the Arduino, but once it set up, it would be very easy to switch between Arduino and Featherboard.

Once you install the necessary software, it generates most of the source files under your user folder which is \AppData\Local\Arduino15\packages.
If you prefer not to use the predefined APIs, you can refer to these files and generate your own functions. 

[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,...