Qooniee
Posted on May 6, 2022
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
2.BNO055 9DoF Sensor
3.Wiring
4.Coding
5.Lab test
6.Actual vehicle test
7.Discussion
8.Conclusion
9.Reference
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
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 [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[3]
Furthermore you can see demonstration on this link
★Adafruit BNO055 Demo[5]
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[6]
I wired Jetson nano and BNO055 like below.
4. Coding
First I share my code published on GitHub.
measurement_system_app
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[7]
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.
Also vehicle coordinate system of ISO is below.[8]
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.
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.
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.
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.
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)
Also these graphs are difference timing against above graphs.(offset data)
Rotational angle calculated by quaternion is much better than value which directly output from BNO055 as euler angle.
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.
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
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.
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.
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.
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.
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.
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.
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
9. Reference
[1] Overview of BNO055
[2] Adafruit BNO055 appearance
[3] Adafruit_CircuitPython_BNO055
[4] Adafruit_BNO055
[5] Adafruit BNO055 Demo
[6] Jetson nano J41 Pin layout
[7] Euler (gimbal lock) Explained
[8] Coordinate Systems in Automated Driving Toolbox
[9] phyphox
[10] phyphox Sensor Database
Posted on May 6, 2022
Join Our Newsletter. No Spam, Only the good stuff.
Sign up to receive the latest update from our blog.
Related