Develop simple data logger using BNO055 and Jetson nano
Tags: python, nvidia, jetson
Published: 2022-05-06 17:38:28


# Background
 Recently smartphone is becoming a popular day by day so I think most of the people have smartphone. Smartphone has many sensor which are gyro, accelerometer, GPS and so on. By using these sensor many app can collect user behavior. From this kind of circumstance I also think that I want to develop apps used sensor.
 But I don't have Macbook and Android device. So to develop smartphone app I have to buy Android device or Macbook. On the other hand, I have jetson nano so I decided to buy sensor which is BNO055 provided by adafruit and developed software using BNO055 and jetson nano.
 I am software engineer in automotive industry so it is good project to develop data logger system using BNO055 and jetson nano.

# Table of Contents
[1.Requirements](#Requirements)
[2.BNO055 9DoF Sensor](#BNO055 9DoF Sensor)
[3.Wiring](#Wiring)
[4.Coding](#Coding)
[5.Lab test](#Lab test)
[6.Actual vehicle test](#Actual vehicle test)
[7.Discussion](#Discussion)
[8.Conclusion](#Conclusion)
[9.Reference](#Reference)


<a id="Requirements"></a>
# 1. Requirements
 - python 3.x
 - adafruit_bno055
 - busio
 - board
 - threading
 - time
 - numpy
 - pandas
 - matplotlib
 - os
 - IPython
 - ipywidgets
 - datetime
 - Jetson nano B02 4GB
 - Adafruit BNO055
 - Wires for connect computer and sensor

<a id="BNO055 9DoF Sensor"></a>
# 2. BNO055 9DoF Sensor
 BNO055 can measure roll, pitch and yaw rotational motion using gyro and forward, lateral and vertical acceleration using accelerometer. Also, this sensor can measure directional data with compass.[1]

- Absolute Orientation (Euler Vector, 100Hz)
- Three axis orientation data based on a 360° sphere
- Absolute Orientation (Quaterion, 100Hz)
- Four point quaternion output for more accurate data manipulation
- Angular Velocity Vector (100Hz)
- Three axis of 'rotation speed' in rad/s
- Acceleration Vector (100Hz)
- Three axis of acceleration (gravity + linear motion) in m/s^2
- Magnetic Field Strength Vector (20Hz)
- Three axis of magnetic field sensing in micro Tesla (uT)
- Linear Acceleration Vector (100Hz)
- Three axis of linear acceleration data (acceleration minus gravity) in m/s^2
- Gravity Vector (100Hz)
- Three axis of gravitational acceleration (minus any movement) in m/s^2
- Temperature (1Hz)
- Ambient temperature in degrees celsius

 You can see appearance of BNO055 in below links. 
[★Adafruit BNO055 appearance ](https://cdn-shop.adafruit.com/970x728/2472-01.jpg)[2]

This sensor is small and adafruits provides library for python which is called Adafruits_CircuitPython. Library which was coded by C-lang also is provided by them.

[★Adafruit_CircuitPython_BNO055](https://github.com/adafruit/Adafruit_CircuitPython_BNO055)[3]

[★Adafruit_BNO055](https://github.com/adafruit/Adafruit_BNO055)[4]

 Furthermore you can see demonstration on this link
[★Adafruit BNO055 Demo](https://cdn-shop.adafruit.com/product-videos/1024x768/2472-04.mp4)[5]

<a id="Wiring"></a>
# 3. Wiring
 To transmit data from BNO055 to Jetson nano, It's need to connect them using GPIO.
GPIO information of Jetson nano is here.
[★Jetson nano J41 Pin layout](https://jetsonhacks.com/nvidia-jetson-nano-j41-header-pinout/)[6] 

I wired Jetson nano and BNO055 like below.
![Image description](https://dev-to-uploads.s3.amazonaws.com/uploads/articles/dwsawx0zlxwy99tv252j.png)
 

<a id="Coding"></a>
# 4. Coding
First I share my code published on GitHub.
[measurement_system_app](https://github.com/cayk326/measurement_system)

This code implemented on Jupyter Notebook and describe each function briefly.

#### 4-1. Structure

Structure of this system is like below.
- measurement_system_app.ipynb: Main code
- Model/measurement.py: Functions for measuring data using BNO055
- Model/signalprocessing.py: Toolkit to process signals

Measured data will be stored on data folder. 



#### 4-2. measurement class

 This class consist for getting data from BNO055. connect() is to establish connection with BNO055 sensor and get_data() is for sampling data from BNO055. Note that BNO055 outputs euler angle in the order of Yaw, Roll and Pitch. 
 So this function stores euler angles rearrange in the order of  Roll, Pitch and Yaw based on vehicle coordinate system ISO.
Also this class has function which is able to calculate euler angle using quaternion. This is importance point when you use BNO055. BNO055 can directly output euler angle but error might be big for your project.

- LSB is 16 bit so it prone to occur error
- Yaw value is unstable when roll and pitch angle bigger than ±45deg 

To overcome disadvantage it's beneficial to utilize quaternion because quaternion can express all rotation.

More information, please refer this video.
[Euler (gimbal lock) Explained](https://www.youtube.com/watch?v=zc8b2Jo7mno)[7]

```python
import os
path = os.getcwd()
from collections import deque
import numpy as np
import adafruit_bno055
import busio
import board

class MeasBNO055:
    
    def __init__(self, BNO_UPDATE_FREQUENCY_HZ=10, INTERVAL=1000):
        
        self.COLUMNS = ["Time","euler_x", "euler_y", "euler_z", "gyro_x", "gyro_y", "gyro_z", "gravity_x", "gravity_y", "gravity_z",
                        "linear_accel_x", "linear_accel_y", "linear_accel_z","accel_x", "accel_y", "accel_z",
                        "quaternion_1", "quaternion_2", "quaternion_3", "quaternion_4", 
                        "calibstat_sys", "calibstat_gyro", "calibstat_accel", "calibstat_mag",
                        "Roll", "Pitch", "Yaw"]
    
        self.BNO_UPDATE_FREQUENCY_HZ = BNO_UPDATE_FREQUENCY_HZ
        self.exepath = path + '/measurement_system'
        self.datapath = path + '/data'
    
    
        self.INTERVAL = INTERVAL
        self.INIT_LEN = self.INTERVAL // self.BNO_UPDATE_FREQUENCY_HZ

        self.Time_data = deque(np.zeros(self.INIT_LEN))# Time
        self.linear_accel_x_data = deque(np.zeros(self.INIT_LEN))# linear_accel_x
        self.linear_accel_y_data = deque(np.zeros(self.INIT_LEN))# linear_accel_y
        self.linear_accel_z_data = deque(np.zeros(self.INIT_LEN))# linear_accel_z
        self.gyro_x_data = deque(np.zeros(self.INIT_LEN))# gyro_x
        self.gyro_y_data = deque(np.zeros(self.INIT_LEN))# gyro_y
        self.gyro_z_data = deque(np.zeros(self.INIT_LEN))# gyro_z
        
        self.assy_data = None
        
        self.i2c, self.bno = self.connect()

            
    def connect(self):
        i2c = busio.I2C(board.SCL, board.SDA)# Access i2c using SCL and SDA
        bno = adafruit_bno055.BNO055_I2C(i2c)# Connect BNO055. Default: NDOF_MODE(12)
        #self.bno.use_external_crystal = True
        print("Established connection with BNO055")
        return i2c, bno
    
    def get_data(self, bno):
        '''
        Get data from new value from BNO055
        
        '''    
        euler_z, euler_y, euler_x= [val for val in bno.euler]# Euler angle[deg] (Yaw, Roll, Pitch) -> euler_x,euler_y,euler_z = (Roll, Pitch, Yaw)
        euler_x = (-1.0) * euler_x
        euler_y = (-1.0) * euler_y
        euler_z = euler_z - 180.0# 180deg offset
        gyro_x, gyro_y, gyro_z = [val for val in bno.gyro]# Gyro[rad/s]
        gravity_x, gravity_y, gravity_z = [val for val in bno.gravity]# Gravitaional aceleration[m/s^2]
        linear_accel_x, linear_accel_y, linear_accel_z = [val for val in bno.linear_acceleration]# Linear acceleration[m/s^2]
        accel_x, accel_y, accel_z = [val for val in bno.acceleration]# Three axis of acceleration(Gravity + Linear motion)[m/s^2]
        quaternion_1, quaternion_2, quaternion_3, quaternion_4 = [val for val in bno.quaternion]# Quaternion
        calibstat_sys, calibstat_gyro, calibstat_accel, calibstat_mag = [val for val in bno.calibration_status]# Status of calibration
        roll, pitch, yaw = self.calcEulerfromQuaternion(quaternion_1, quaternion_2, quaternion_3, quaternion_4)# Cal Euler angle from quaternion
        return euler_x, euler_y, euler_z, gyro_x, gyro_y, gyro_z, gravity_x, gravity_y, gravity_z,\
                linear_accel_x, linear_accel_y, linear_accel_z, accel_x, accel_y, accel_z,\
                quaternion_1, quaternion_2, quaternion_3, quaternion_4,\
                calibstat_sys, calibstat_gyro, calibstat_accel, calibstat_mag,\
                roll, pitch, yaw
        
    def calcEulerfromQuaternion(self, _w, _x, _y, _z):
        sqw = _w ** 2
        sqx = _x ** 2
        sqy = _y ** 2
        sqz = _z ** 2
        COEF_EULER2DEG = 57.2957795131
        yaw = COEF_EULER2DEG * (np.arctan2(2.0 * (_x * _y + _z * _w), (sqx - sqy - sqz + sqw)))# Yaw
        pitch = COEF_EULER2DEG * (np.arcsin(-2.0 * (_x * _z - _y * _w) / (sqx + sqy + sqz + sqw)))# Pitch
        roll = COEF_EULER2DEG * (np.arctan2(2.0 * (_y * _z + _x * _w), (-sqx - sqy + sqz + sqw)))# Roll
        return roll, pitch, yaw
```

Below is sensor coordinate of BNO055.
![Image description](https://dev-to-uploads.s3.amazonaws.com/uploads/articles/ffn4ypjr4r5qkadrw05q.png)
 
Also vehicle coordinate system of ISO is below.[8]
![Image description](https://dev-to-uploads.s3.amazonaws.com/uploads/articles/oo8b6vj59pmm99n9g2h8.png)
 
 
#### 4-3. measurement_control class
 This class for realize interactive operation using ipwidget on Jupyter notebook.
After executing this class, "Start/Stop", "Save" and "Calib" button will be appeared.

- Start/Stop: Start and Stop measurement
- Save: Save data after preprocessing(preprocessing menu are offset and lowpass filter)
- Calib: Confirm status of calibration. This function can only use if measurement is not started.

<a id="Lab test"></a>
# 5. Lab test
This test objective is to confirm below things.
- Coordinate system is properly or not.
- Validate accuracy against measurement app works on iphone


#### 5-1. Method of test
I evaluated my code using phyphox[9] which is smartphone app.
This app can measure many physics values using smartphone, very useful for me.
I fixed sensor on iphone like below.

![Image description](https://dev-to-uploads.s3.amazonaws.com/uploads/articles/g4h4whv5a1d55asvgxys.JPG)

After that, I move smartphone in each direction of coordinate. Signal shape is sine wave. The order of movement is Forward / Lateral /Vertical / Roll / Pitch / Yaw.
Note that sensor coordinate and vehicle coordinate is not same. So my tool converts sensor coordinate to ISO type vehicle coordinate system. Sampling frequency is 10Hz.

Coordinate system of iphone is below. 
![Image description](https://dev-to-uploads.s3.amazonaws.com/uploads/articles/nivzis6u5ssdqlqlnct1.png)

#### 5-2. Lab test result

Here is result of lab test. Blue line indicates iphone app data and orange line indicates data logger app which I developed. Yellow line is calibration status of BNO055, 3 is fully calibrated. This value is just reference.

![Image description](https://dev-to-uploads.s3.amazonaws.com/uploads/articles/wftz4papqsocts1mcfhk.png)
 
 
Trend of all physics values is well. It seems that, to a certain extent, it could be utilized for analyzing handling of vehicle.
Especially gyro data is really well. 
On the other hand, shape difference of acceleration between orange line and blue line might be difference of influence against acceleration from gyro motion. 
For instance if it is able to put sensor on center of gravity, lateral acceleration will not affected by roll velocity but realizing this is to difficult. So lateral acceleration is usually affected from roll velocity. There are some method to compensate but it is laborious.


Here are rotational angle at same timing of previous graphs.(offset data)

![Image description](https://dev-to-uploads.s3.amazonaws.com/uploads/articles/5hgp6hcbuuqt1ioqes05.png)
 

Also these graphs are difference timing against above graphs.(offset data)

![Image description](https://dev-to-uploads.s3.amazonaws.com/uploads/articles/lkw6gde84bcitt3fcx7l.png)

Rotational angle calculated by quaternion is much better than value which directly output from BNO055 as euler angle.



<a id="Actual vehicle test"></a>
# 6. Actual vehicle test
As next step, I measured vehicle behavior using data logger I developed and phyphox.
This is result of measurement.
Blue line indicates iphone app data and orange line indicates data logger app which I developed. 

#### 6-1. Overview of comparison(Sampling frequency: 10Hz)

- Especially yaw velocity is match with phyphox data 
- Absolute value of all signals are not match among iphone and  BNO055. But trend of all signals are well.

![Image description](https://dev-to-uploads.s3.amazonaws.com/uploads/articles/vnq3b0d82tkrw5ci0c30.png)

![Image description](https://dev-to-uploads.s3.amazonaws.com/uploads/articles/r7a67fa7y7d2tom1e89p.png)
 
 

#### 6-2. Comparison in Point 3
This point is intersection so includes start of vehicle from speed as 0 km/h and cornering.


- Absolute value is not same between iphone data and BNO055 data about forward acceleration. But trend is good
- Trend of Other signals is good
- It can see the motion of the pitch during acceleration
- These graphs also indicate lateral, roll and yaw motion in cornering scene

![Image description](https://dev-to-uploads.s3.amazonaws.com/uploads/articles/00z1vi3ad7pbd3nc4unx.png)

![Image description](https://dev-to-uploads.s3.amazonaws.com/uploads/articles/xm3iav5pzrjih0gns2qm.png)


<a id="Discussion"></a>
# 7. Discussion

Basically it is possible to analysis vehicle behavior using application I developed. But this system has some room for improvement.  

This system can measure data with sampling frequency until about 40Hz. But BNO055 has ability to measure with sampling frequency as 100Hz.[1]
Also phyphox can measure gyro and acceleration with sampling frequency as 100Hz.[10]
So I thought this issue is problem on implementation method.


#### 7-1. Additional experiment about execution time

Issue regarding sampling frequency is related to get_data function of while loop in start method.
While loop manage clock of this system based on sampling frequency but if executed time of get_data() is bigger than clock time, this result lead to drop sampling.

While loop structure can be regarded series connection of getting each sensor data of BNO055.

![Image description](https://dev-to-uploads.s3.amazonaws.com/uploads/articles/27cchlh6tfqhc9pw1x2d.png)

So I investigated elapsed time for getting each sensor data.
For investigating elapsed time I used time.time(). I also evaluated execution time of time.time() as same as evaluating execution time of getting sensor data.

![Image description](https://dev-to-uploads.s3.amazonaws.com/uploads/articles/7dcwefv7nxl122wf0faq.png)

Let's say get_data() needs 0.021[s] to finish all process, this mean this application can not realize measuremet with sampling frequency as 100 Hz.

On the other hands, execution time of each process for getting sensor data such as euler, gyro gravity and so on is about 0.0055[s].
So if get_data() can be realize parallel processing to get sensor data, this issue might be solved.

![Image description](https://dev-to-uploads.s3.amazonaws.com/uploads/articles/612purdi3i8xbz9lzdfm.png)
 
In my case of project, get_data() is just get physics data from sensor. Each function for getting data takes 0.0054 sec as worst. Also these functions doesn't include I/O process.

#### 7-2. Investigation of parallel processing

I evaluated execution time of below method.
Let's say execution time of single process which is accessing sensor data like gyro is 0.005 sec.
I evaluated:
[DIL timeout is 0.005sec]
- single process
- stream processing of 7 functions
- stream processing of 4 functions
- parallel processing of 7 functions

[DIL timeout is 0.0001sec]
- single process
- stream processing of 7 functions
- stream processing of 4 functions
- parallel processing of 7 functions

This is code for evaluation.
```python
import time
from time import perf_counter
import sys

import random

import numpy as np
from concurrent.futures import ThreadPoolExecutor, ProcessPoolExecutor

class BNO055:
    def __init__(self):
        self.val = 1
        self.name = 'bno055'


def wait_process(wait_sec):
    until = perf_counter() + wait_sec
    while perf_counter() < until:
        pass
    return

def get_euler(waittime):
    wait_process(waittime)
    x, y, z = 0.1, 0.1, 0.1
    return x, y, z

def get_gyro(waittime):
    wait_process(waittime)
    x,y,z = 2,2,2
    return x,y,z

def get_accel(waittime):
    wait_process(waittime)
    x, y, z = 1, 1, 1
    return x, y, z

def get_liner_acc(waittime):
    wait_process(waittime)
    x, y, z = 10, 10, 10
    return x, y, z

def get_gravity(waittime):
    wait_process(waittime)
    x, y, z = 5, 5, 5
    return x, y, z

def get_quaternion(waittime):
    wait_process(waittime)
    w, x, y, z = 0.1, 0.1, 0.1, 0.1
    return w, x, y, z

def get_calib_status(waittime):
    wait_process(waittime)
    w, x, y, z = 3, 3, 3, 3
    return w, x, y, z




def single_process(waittime):
    start = time.time()
    get_gyro(waittime)
    end = time.time()
    print('elepsed_time of single:{:.6f}[s]'.format(end-start))

def stream_process(waittime = 0.005):
    start = time.time()
    ex, ey, ez = get_euler(waittime)
    gx, gy, gz = get_gravity(waittime)
    ax, ay, az = get_accel(waittime)
    lax, lay, laz = get_liner_acc(waittime)
    rx, ry, rz = get_gyro(waittime)
    qw, qx, qy, qz = get_quaternion(waittime)
    a, b ,c, d = get_calib_status(waittime)
    end = time.time()
    print('elepsed_time of stream:{:.6f}[s]'.format(end-start))
    return ex, ey, ez, gx, gy, gz, ax, ay, az, lax, lay, laz, rx, ry, rz, qw, qx, qy, qz, a, b, c, d

def stream_process_less(waittime = 0.005):
    start = time.time()
    lax, lay, laz = get_liner_acc(waittime)
    rx, ry, rz = get_gyro(waittime)
    qw, qx, qy, qz = get_quaternion(waittime)
    a, b ,c, d = get_calib_status(waittime)
    end = time.time()
    print('elepsed_time of stream less:{:.6f}[s]'.format(end-start))
    return lax, lay, laz, rx, ry, rz, qw, qx, qy, qz, a, b, c, d
def get_all_data(params, waittime=0.005):
    bno = params[0]
    sensor = params[1]
    #print(bno)
    if sensor == 'euler':
        x, y, z = get_euler(waittime)
        return x, y, z

    elif sensor == 'gyro':
        x, y, z = get_gyro(waittime)
        return x,y,z

    elif sensor == 'linear_acc':
        x,y,z=get_liner_acc(waittime)
        return x,y,z

    elif sensor == 'gravity':
        x, y, z = get_gravity(waittime)
        return x,y,z

    elif sensor == 'acceleration':
        x, y, z =  get_accel(waittime)
        return x, y, z

    elif sensor == 'quaternion':
        w, x, y, z = get_quaternion(waittime)
        return w, x, y, z
    elif sensor == 'calib_status':
        a, b, c, d = get_calib_status(waittime)
        return a, b, c, d
    else:
        pass



def pararel_process(bno, n):
    start = time.time()
    sensors = ['euler', 'gyro', 'gravity', 'acceleration', 'linear_acc', 'quaternion', 'calib_status']
    #sensors = ['gyro', 'linear_acc', 'quaternion', 'calib_status']#  [(2, 2, 2), (10, 10, 10), (0.1, 0.1, 0.1, 0.1), (3, 3, 3, 3)]
    result = []
    #print(bno)
    #params = [bno, sensors]
    bno_list = [bno for i in range(len(sensors))]
    params = zip(bno_list,sensors)
    with ThreadPoolExecutor(max_workers=n) as e:
        ret = e.map(get_all_data, params, chunksize=1)
    sms_multi = [r for r in ret]
    for res in sms_multi:
        for i in range(len(res)):
            result.append(res[i])

    end = time.time()
    print('elepsed_time of pararel:{:.6f}[s]'.format(end - start))


import multiprocessing
def multiproc(bno):
    start = time.time()
    sensors = ['gyro', 'linear_acc', 'quaternion', 'calib_status']
    result = []
    print(bno)
    # params = [bno, sensors]
    bno_list = [bno for i in range(len(sensors))]
    params = zip(bno_list, sensors)
    with multiprocessing.Pool() as pool:
        ret = pool.map(get_all_data, params, chunksize=1)
    sms_multi = [r for r in ret]
    for res in sms_multi:
        for i in range(len(res)):
            result.append(res[i])#gyro/linear_accel/quat/calib_status
    end = time.time()
    print('elepsed_time:{:.6f}[s]'.format(end - start))
if __name__ == '__main__':


    waittime = 0.005
    bno = BNO055()
    worker = 7

    print(sys.getswitchinterval())  # GIL timeout default is 5ms
    single_process(waittime)# Single
    stream_process(waittime)# Stream processing
    stream_process_less(waittime)  # Stream processing with reducing access of sensor value
    pararel_process(bno, n=worker) # parallel processing using ThreadPoolExecutor
    print('----------Change DIL timeout---------------')
    sys.setswitchinterval(0.0001)# 0.1ms
    print(sys.getswitchinterval())
    single_process(waittime)# Single
    stream_process(waittime)# Stream processing
    stream_process_less(waittime)  # Stream processing with reducing access of sensor value
    pararel_process(bno, n=worker) # parallel processing using ThreadPoolExecutor

    #multiproc(bno)# multi process

```


Here is result.

![Image description](https://dev-to-uploads.s3.amazonaws.com/uploads/articles/novnbz5v9mjo8jdi4crw.png)

According to this table, if DIL timeout is 0.005sec it couldn't get advantage by applying parallel processing. 
On the other hands, if DIL timeout set as 0.00001sec parallel processing is beneficial for reduce execution time.

After experiment, I implemented parallel processing function using ThreadPoolExecutor for get_data() but get_data() became to return NULL frequently. Also, order of data was not same.

On the other hand, actually I don't need euler angle comes from bno.euler. Because this system calculate euler angle using quaternion. Also I only need linear acceleration. So number of accessing data can be reduce from 7 to 4. Then, get_data() become more faster. Influence of changing DIL timeout is nothing.
By reducing measurement data channel this system was able to measure data with sampling frequency as 50Hz.
Sampling frequency from 30 Hz to 50Hz is enough to analysis overview of vehicle dynamics.

<a id="Conclusion"></a>
# 8. Conclusion

- I implemented data logger system using Jetson nano and BNO055.
- Measured data could be utilize for analyzing vehicle dynamics.
- Quaternion is beneficial to calculate euler angle.
- Practical max sampling frequency for measurement is 50Hz. 
- This system has room for improvement regarding realizing faster execution

<a id="Reference"></a>
# 9. Reference
[1] [Overview of BNO055](https://learn.adafruit.com/adafruit-bno055-absolute-orientation-sensor)
[2] [Adafruit BNO055 appearance](https://cdn-shop.adafruit.com/970x728/2472-01.jpg)
[3] [Adafruit_CircuitPython_BNO055](https://github.com/adafruit/Adafruit_CircuitPython_BNO055)
[4] [Adafruit_BNO055](https://github.com/adafruit/Adafruit_BNO055)
[5] [Adafruit BNO055 Demo](https://cdn-shop.adafruit.com/product-videos/1024x768/2472-04.mp4)
[6] [Jetson nano J41 Pin layout](https://jetsonhacks.com/nvidia-jetson-nano-j41-header-pinout/)
[7] [Euler (gimbal lock) Explained](https://www.youtube.com/watch?v=zc8b2Jo7mno)
[8] [Coordinate Systems in Automated Driving Toolbox](https://jp.mathworks.com/help/driving/ug/coordinate-systems.html)
[9] [phyphox](https://phyphox.org/)
[10] [phyphox Sensor Database](https://phyphox.org/sensordb/)