# Course notes: IMU BNO055

## Preamble

Note that the serialplot application used in this course is now available for OS X, at /serialplot_OS_X_port. See Porting serialplot to OS X for more details.

## Lesson 1

9-Axis IMU LESSON 1: Introduction to Nine Axis Sensors and Inertial Measurement Units with Arduino

• BNO055 – expensive but better
• Arduino Nano

Pitch, Z axis

roll – rotation

1. Intro
2. Hookup and first program
3. Calibration,
4. Visualisation software
5. How accelerometers (3) work – actually measures forces exerted on a suspended mass
6. How gyros (3) work how
7. How magnetometers (3) work
8. tilt sensor
9. Fuse accelerometer and magnetometers
10. compass
11. Euler angles
12. Quaternions
13. Get data from Arduino to visual python
14. Warnings/Limitations

## Lesson 2

9-Axis IMU LESSON 2: Connecting and Getting Raw Data from the BNO055 9-Axis Sensor

Three types

• Acceleration in x, y, z axes
• Rotational velocity in x, y, z axes
• Strength of the magnetic vector in the x, y, z axes

Hookup via I2C

A4 and A5 on Nano

```Arduino     BNO055

A4            SDA
A5            SCL
5V (12)       VIN
GND (14)      GND```

Run the serial at 115200 (faster than usual) because there is a lot of data.

As the measurements depend upon the temperature, make a temperature measurement.

There are two crystals, an in-chip crystal, and an “external” on-board crystal. Tell the BNO to use that crystal.

Set a sample rate delay of 100 ms

Code:

```#include <Wire.h>
#include <utility/imumaths.h>

#define BNO055_SAMPLERATE_MS 100

void setup() {
Serial.begin(115200);
myIMU.begin();
delay(1000);

int8_t temp = myIMU.getTemp();
Serial.println("Temperature: ");
Serial.println(temp);
myIMU.setExtCrystalUse(true);

}

void loop() {

Serial.print(acc.x());
Serial.print(",");
Serial.print(acc.y());
Serial.print(",");
Serial.print(acc.z());
Serial.print(",");
Serial.print(gyro.x());
Serial.print(",");
Serial.print(gyro.y());
Serial.print(",");
Serial.print(gyro.z());
Serial.print(",");
Serial.print(magnet.x());
Serial.print(",");
Serial.print(magnet.y());
Serial.print(",");
Serial.print(magnet.z());
Serial.print(",");

delay(BNO055_SAMPLERATE_MS);

}```

##### Homework

Examine the x, y, and z components of acceleration, and check for sanity. If there are any anomalies, explain them.

## Lesson 3

How accelerometers work…

9-Axis IMU LESSON 3: Understanding How Accelerometers Work

The Z direction has a standing still acceleration of 10 m/s which is gravity… the X and Y axis have zero, or close to zero.

Code

```// IMU_BNO055_Accelerometer.ino

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

#define BNO055_SAMPLERATE_MS 100

void setup() {
Serial.begin(115200);
myIMU.begin();
delay(1000);

int8_t temp = myIMU.getTemp();
Serial.println("Temperature: ");
Serial.println(temp);
myIMU.setExtCrystalUse(true);
}

void loop() {

Serial.print(acc.x());
Serial.print(",");
Serial.print(acc.y());
Serial.print(",");
Serial.print(acc.z());

Serial.println(" ---");

delay(BNO055_SAMPLERATE_MS);
}```

## Getting the version info from IMU

Note: From David Pilling: BNO 055

```Adafruit_BNO055::adafruit_bno055_rev_info_t data;
bno.getRevInfo(&data);
pr("revisions acc %d madg %d gyro %d sw %d bl %d",data.accel_rev,data.mag_rev,data.gyro_rev,data.sw_rev,data.bl_rev);```

In Arduino

```// IMU_BNO055_DavidPilling.ino
// From David Pilling
// https://www.davidpilling.com/wiki/index.php/BNO055

/*

bno.getRevInfo(&data);
pr("revisions acc %d madg %d gyro %d sw %d bl %d",data.accel_rev,data.mag_rev,data.gyro_rev,data.sw_rev,data.bl_rev);

*/

#include
#include
#include
#include <utility/imumaths.h>

#define BNO055_SAMPLERATE_MS 100

void setup() {
Serial.begin(115200);
bno.begin();
delay(1000);

int8_t temp = bno.getTemp();
Serial.println("Temperature: ");
Serial.println(temp);
bno.setExtCrystalUse(true);
}

void loop() {
bno.getRevInfo(&data);
// Serial.print("revisions acc %d madg %d gyro %d sw %d bl %d",data.accel_rev,data.mag_rev,data.gyro_rev,data.sw_rev,data.bl_rev);
Serial.println("Revisions:");

Serial.print("acc ");
Serial.println(data.accel_rev);

Serial.print("mag ");
Serial.println(data.mag_rev);

Serial.print("sw ");
Serial.println(data.sw_rev);

Serial.print("bl ");
Serial.println(data.bl_rev);
}```

## Lesson 4

Not Calibration

9-Axis IMU LESSON 4: Plotting Serial Data from Arduino with Control of Axis Scale

There are four calibration channels as well as 9 data channels. We can use the Arduino IDE to plot but there is very little control over the y-axis, it autoscales too much, so we need a better method.

## Lesson 5

9-Axis IMU LESSON 5: Calibrating the BNO055 9-axis Inertial Measurement Sensor

Calibration

Starting with the accelerometer code, 9-AXIS IMU LESSON 3: UNDERSTANDING HOW ACCELEROMETERS WORK

The Z-axis shows 10, but it should be showing 9.8, so we need to calibrate.

Three levels of calibration – 0 (uncal) up to 3. When all three sensors are at three then the system is at three also.

To calibrate:

• gyro: sit still for a few seconds
• mag: swig it around, only takes a few seconds
• accel:  move and hold three seconds, move and hold, move and hold (30 second to minute)
```// IMU_BNO055_Accelerometer_Calibration.ino
// IMU_BNO055_Accelerometer.ino

// IMU_BNO055_BareMinimum.ino

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

#define BNO055_SAMPLERATE_MS 100

void setup() {
Serial.begin(115200);
myIMU.begin();
delay(1000);

int8_t temp = myIMU.getTemp();
Serial.println("Temperature: ");
Serial.println(temp);
myIMU.setExtCrystalUse(true);
}

void loop() {

uint8_t system, gyro, accel, mg = 0;
myIMU.getCalibration(&system, &gyro, &accel, &mg);

Serial.print(acc.x());
Serial.print(",");
Serial.print(acc.y());
Serial.print(",");
Serial.print(acc.z());
Serial.print(",");
Serial.print(accel);
Serial.print(",");
Serial.print(gyro);
Serial.print(",");
Serial.print(mg);
Serial.print(",");
Serial.println(system);

delay(BNO055_SAMPLERATE_MS);
}```

You see (upon a reset) all of the calibration values at 0, then gyro goes to 3, then (after swinging the IMU around for about 3 seconds) the mag goes to 3 (and system goes to 3 – even though the accelerometer isn’t yet calibrated).

If gyro on shaking vehicle then it will have trouble to calibrate, it must be still, initially.

Accel: ±45° on x axis, then ±45° on y axis, then upside down,  you will get a level of 1, and then back to rest and the level goes to 3.

Now the acceleration Z axis shows 9.8.

If you bring a magnet (i..e a stepepr motor) close then the magnetometer (and hence the system) will lose calibration. To recalibrate: move magnet away, this should put the magnetometer back to 3, although the system may stay at 1 or 2. Do a figure of eight movement, and/or swing it around for a bit.

Show graphing the calibration channels (Y-axis scale -1 to 4 and turn off the x,y,z accelerometers), reset and then manually recalibrate to see the calibration levels.

##### Homework

Tilt measurement tool, using the Z axis only (or the Y axis). Use trig (SOHCAHTOA)

## Lesson 6

9-Axis IMU LESSON 6: Determine Tilt From 3-axis Accelerometer

Tilt:

```sin(ø) = ax/1
cos(ø) = az/1
tan(ø) = ax/az

so (most accurate)

ø = arctan(ax/az)```

Normalise to 1G (= x/9.8)

Roll:

```sin(ø) = ay/1
cos(ø) = az/1
tan(ø) = ay/az

so (most accurate)

ø = arctan(ay/az)```

code

```  // float theta_tilt = atan2(acc.x(), acc.z()); // Radians
float theta_tilt = atan2(acc.x(), acc.z())/2/3.142*360;  // Degrees
// float theta_roll = atan2(acc.y(), acc.z());  // Radians
float theta_roll = atan2(acc.y(), acc.z())/2/3.142*360;  // Degrees```

May need to change the sign

```  // float theta_tilt = -atan2(acc.x(), acc.z()); // Radians
float theta_tilt = -atan2(acc.x(), acc.z())/2/3.142*360;  // Degrees
// float theta_roll = -atan2(acc.y(), acc.z());  // Radians
float theta_roll = -atan2(acc.y(), acc.z())/2/3.142*360;  // Degrees```

Best for angles < 45°. tan has asymptotic behaviour, at 90° it flips… not good.

Note: when not tilts or rolling, but move (vibrates) in x or y axis, movement/acceleration is still measured, so it is not accurate as a “tiltometer” (inclinometer).

##### Homework

How to compensate for vibration being measured as tilt.

## Lesson 7

9-Axis IMU LESSON 7: Understanding Low Pass Filters

Last lesson code: 9-AXIS IMU LESSON 6: DETERMINE TILT FROM 3-AXIS ACCELEROMETER

```ønew = P1*øold + P2*ømeasured

where, P1 + P2 = 1

ønew = 0.9*øold + 0.1*ømeasured```

Slower response, but noise is reduced greatly.

Other values:

• 0.75 and 0.25 – too much noise
• 0.99 and 0.01 – too slow
##### Homework

Make it quicker without introducing noise.

## Lesson 8

9-Axis IMU LESSON 8: Using Gyros for Measuring Rotational Velocity and Angle

Using the gyros

Pitch from gyro ØG_pitch

`øG_pitch = øG_pitch_old + ω.∂t`

In video

```θG = θG(old) + ωy.∂t
φG = φG(old) + ωx.∂t```

ω is the movement.

Once the movement has stopped then the reading goes back to zero, only instantaneous changes are shown. Need to change the movement into an angle. So we need to change in time, ∂t.

```  delta_t = (millis() -millis_old)/1000.0;
millis_old = millis();
theta_gyro_tilt = theta_gyro_tilt + gyr.y()*delta_t;
theta_gyro_roll = theta_gyro_roll + gyr.x()*delta_t;```

May need to change the sign:

```  delta_t = (millis() -millis_old)/1000.0;
millis_old = millis();
theta_gyro_tilt = theta_gyro_tilt + gyr.y()*delta_t;
theta_gyro_roll = theta_gyro_roll - gyr.x()*delta_t;```

The readings from the gyro are not susceptible to vibration (unlike the accelerometers). However, it does have drift, at rest it can show  -5° or -15° (which the accelerometers do not exhibit).

So we need to combine the two data streams

##### Homework

Use gyro and accelerometer together to combine advantages

## Lesson 9

9-Axis IMU LESSON 9: Accurate and Stable Tilt Using Accelerometers, Gyros and a Complimentary Filter

Combine accel and gyro:

• Accel – trust long term data, LPF
• Gyro – trust short term data, HPF

So

```θ = [θG + ωy.∂t]*0.95 + θA
φ = [φG + ωx.∂t]*0.95 + φA```

Note:

• Why not use just ωy.∂t? As the old θG has big drift
• Why not use the LPF acceleratometer data, as it has no noise.
``` theta_tilt = (theta_tilt + gyr.y()*delta_t)*0.95 + theta_tilt_measured*0.05;
theta_roll = (theta_roll + gyr.x()*delta_t)*0.95 + theta_roll_measured*0.05;

```

Note: Sudden changes causes overshoot, caused by the gyros.

##### Homework

Tweak the filter values. Also, use the filtered accelerometer.

Note: The smaller dt is, the better. Removing the print statements would make it quicker.

## Lesson 10

9-Axis IMU LESSON 10: Making a Tilt Compensated Compass with Arduino

We have pitch and roll, but no heading (yaw).

`tan ψ = ym/xm`

This is only for the BNO055 when flat.

```  // psi = atan2(mgt.y(), mgt.x());  // Radians
psi = atan2(mgt.y(), mgt.x()) / 2 / 3.142 * 360; // Degrees```

If tilted or rolled then the readings are inaccurate and a 3D space needs to be considered.

Also, roll and tilt will inaccuracy show a heading change, even when there is none.

In 3D

```  // In 3D
psi = atan2(Ym, mgt.x()) / 2 / 3.142 * 360; // Degrees```

If you roll very quickly then the magnetometer will register a blip. This can be removed with a low pass filter.

So now we have the Euler angles:

• Tilt
• Roll

## Lesson 11

9-Axis IMU LESSON 11: Install Python

Python 3.7

## Lesson 12

9-Axis IMU LESSON 12: Passing Data From Arduino to Python

`pip3 install pyserial`

Arduino

```// IMU_BNO055_Acc_gyro_mag_short.ino
// IMU_BNO055_Acc_gyro_mag.ino
// IMU_BNO055_Accelerometer_gyro_combined.ino

// IMU_BNO055_Accelerometer_tilt_filtered_gyro.ino
// IMU_BNO055_Accelerometer_tilt_filtered.ino
// IMU_BNO055_Accelerometer_tilt.ino

// IMU_BNO055_Accelerometer_Calibration.ino
// IMU_BNO055_Accelerometer.ino

// IMU_BNO055_BareMinimum.ino

#include
#include
#include
#include <utility/imumaths.h>
#include

#define BNO055_SAMPLERATE_MS 100

float theta_tilt_filtered_old;
float theta_tilt_filtered_new;
float theta_roll_filtered_old;
float theta_roll_filtered_new;

float theta_gyro_tilt;
float theta_gyro_roll;

float theta_tilt;
float theta_roll;

float psi;

float delta_t;
unsigned long millis_old;

void setup() {
Serial.begin(115200);
myIMU.begin();
delay(1000);

int8_t temp = myIMU.getTemp();
Serial.println("Temperature: ");
Serial.println(temp);
myIMU.setExtCrystalUse(true);

millis_old = millis();
}

void loop() {

uint8_t system_cal, gyro, accel, mg = 0;
myIMU.getCalibration(&system_cal, &gyro, &accel, &mg);

// float theta_tilt = atan2(acc.x(), acc.z()); // Radians
float theta_tilt_measured = atan2(acc.x(), acc.z()) / 2 / 3.142 * 360; // Degrees
// float theta_roll = atan2(acc.y(), acc.z());  // Radians
float theta_roll_measured = atan2(acc.y(), acc.z()) / 2 / 3.142 * 360; // Degrees

theta_tilt_filtered_new = 0.9 * theta_tilt_filtered_old + 0.1 * theta_tilt_measured;
theta_roll_filtered_new = 0.9 * theta_roll_filtered_old + 0.1 * theta_roll_measured;

delta_t = (millis() -millis_old)/1000.0;
millis_old = millis();
theta_gyro_tilt = theta_gyro_tilt + gyr.y()*delta_t;
theta_gyro_roll = theta_gyro_roll + gyr.x()*delta_t;

theta_tilt = (theta_tilt + gyr.y()*delta_t)*0.95 + theta_tilt_measured*0.05;
theta_roll = (theta_roll + gyr.x()*delta_t)*0.95 + theta_roll_measured*0.05;

// In 2D
// psi = atan2(mgt.y(), mgt.x());  // Radians
psi = atan2(mgt.y(), mgt.x()) / 2 / 3.142 * 360; // Degrees

// In 3D
psi = atan2(Ym, mgt.x()) / 2 / 3.142 * 360; // Degrees

Serial.print(accel);
Serial.print(",");
Serial.print(gyro);
Serial.print(",");
Serial.print(mg);
Serial.print(",");
Serial.print(system_cal);
Serial.print(",");
Serial.print(theta_tilt);
Serial.print(",");
Serial.print(theta_roll);
Serial.print(",");
Serial.println(psi);

delay(BNO055_SAMPLERATE_MS);

theta_tilt_filtered_old = theta_tilt_filtered_new;
theta_roll_filtered_old = theta_roll_filtered_new;
}```

Python

```# IMU_Arduino2Python-3.py
# Use with IMU_BNO055_Acc_gyro_mag_short.ino

# IMU_Arduino2Python-2.py
# Use with // IMU_Arduino2Python-1.ino

# IMU_Arduino2Python-1.py

import serial
import time

arduinoData = serial.Serial('COM5', 115200)
time.sleep(1)
while True:
while arduinoData.inWaiting() == 0:
pass
dataPacket = str(dataPacket, 'utf8')
splitPacket = dataPacket.split(",")

cal_accl = float(splitPacket)
cal_gyro = float(splitPacket)
cal_magt = float(splitPacket)
cal_system = float(splitPacket)
pitch = float(splitPacket)
roll = float(splitPacket)
yaw = float(splitPacket)

print(f"acc: {cal_accl}, gyr: {cal_gyro}, mag: {cal_magt}, Sys: {cal_system}, pitch: {pitch}, roll: {roll}, yaw: {yaw}")```

## Lesson 13

9-Axis IMU LESSON 13: Introduction to Visual Python (Vpython)

`pip3 install vpython`

Then code

```# IMU_VPython-1.py

from vpython import *

# x=box()
# x=sphere()

# x=box(length=6, width=2, height=0.2)

x = box(length=6, width=2, height=0.2, color=color.red)

On OSX ctrl-click to drag objects.

##### Homework

Make Bread board as realistic as possible

## Lesson 14

9-Axis IMU LESSON 14: Introduction to Animating Objects in Vpython

Must put a `rate()` statement in an animation loop. Use 20 times per second:

`rate(20)`

Code

```from vpython import *
import numpy as np

x = box(length=6, width=2, height=0.2, opacity=0.2)
y = cylinder(length=6, radius=0.5, pos=vector(-3, 0, 0), color=color.red, opacity=0.5)
z = sphere(radius=0.75, pos=vector(-3, 0, 0), color=color.red, opacity=0.5)

x_arrow = arrow(axis=vector(1, 0, 0), length=2, shaftwidth=0.2, color=color.red)
y_arrow = arrow(axis=vector(0, 1, 0), length=2, shaftwidth=0.2, color=color.green)
z_arrow = arrow(axis=vector(0, 0, 1), length=2, shaftwidth=0.2, color=color.blue)

while True:
for j in np.arange(1, 6, 0.5):
rate(20)
y.length = j
for j in np.arange(6, 1, -0.5):
rate(20)
y.length = j```
##### Homework

Make a cube, put marbles in it and bounce them around. Clue: Have to use 6 boxes as the wall of the cube

## Lesson 15

9-Axis IMU LESSON 15: Dynamic 3D Visualizations in Vpython

6 boxes as walls

End goal: rotating objects

##### Homework

Add more marbles, on independent paths

## Lesson 16

9-Axis IMU LESSON 16: How to Install and Configure Pyscripter

• Windows only
• Background
• templates
• code
• file
• code folding – turn off
• tools – editor options- word wrap on

## Lesson 17

9-Axis IMU LESSON 17: Review of Basic Trigonometry

• Rotation – psi, φ
• tilt/pitch – theta, θ
• roll – phi

Assume the board being rotated is actually a vector, then make a right angle triangle with the x and z axes:

```adjacent = h*cos(φ) = x
opposite = h*sin(φ) = z```

If we assume that the vector (i.e. the hypotenuse) has a length of 1 – if there is no pitch/tilt.

```adjacent = cos(φ) = x
opposite = sin(φ) = z```

X axis is North, Z axis is East. 0° yaw is North. To draw the vector in VPython we need the x and z co-ords.

If there is pitch, then the vector (in the X-Z plane) is no longer of length 1, but instead, `cos(θ)`. This is because in the vertical plane, the vector is still of length 1, hence the adjacent in the horizontal plane is `cos(θ)`.

So, now:

```adjacent = cos(φ).cos(θ) = x
opposite = sin(φ).cos(θ) = z```

and in the vertical (y) plane

`opposite = sin(θ) = y`

Now with the three components we can draw the vector in VPython.

## Lesson 18

9-Axis IMU LESSON 18: Visualizing Pitch and Yaw in Vpython

Make a scene of ±5 and set the scene

```# Setting the scene
scene.range = 5
scene.forward = vector(-1, -1, -1)
scene.height = 600
scene.width = 600```

```# Conversion
toRad = 2 * np.pi / 360

k vector of the board:

`k = vector(cos(yaw) * cos(pitch), sin(pitch), sin(yaw) * cos(pitch))`

Cross product: cross two vectors gives you the perpendicular vector, use right hand rule (cross (thumb, index) => middle)

28:00

Get the other two vectors for free, using a Y axis vector (video uses `v` for `up_vector`, and `s` for `side_vector`)

```y = vector(0,1,0)
side_vector = cross(k, y)
up_vector = cross(side_vector,k)```

assign to the arrows

```front_arrow.axis=k
side_arrow.axis = side_vector
up_arrow.axis = up_vector```

However, assigning the axes makes the length equal to one. So, you have to reset the lengths again:

```front_arrow.axis = k
side_arrow.axis = side_vector
up_arrow.axis = up_vector
front_arrow.length = 4
side_arrow.length = 4
up_arrow.length = 4```

45:00

## Lesson 19

9-Axis IMU LESSON 19: Vpython Visualization of Pitch and Yaw

Use vpython code from previous lesson 18: TTB: 9-AXIS IMU LESSON 18: VISUALIZING PITCH AND YAW IN VPYTHON

Use the Arduino code from lesson 10: TTB: 9-AXIS IMU LESSON 10: MAKING A TILT COMPENSATED COMPASS WITH ARDUINO

Use short version of Arduino code, and reorder the calibration to the end of the print CSV, so that the actual position data is sent first. Reorder to natural order: roll, pitch, yaw (previously, pitch roll, yaw – in the order of being developed).

Arduino side is in degrees. Will need to convert to radians for VPython.

Need to add Pi (`np.pi` ) to yaw if you have made the circuit (or placed the IMU) the wrong way around.

Final note: Trig can/will break at greater than 45°- due to assumptions made in the trig.

## Lesson 20

9-Axis IMU LESSON 20: Vpython Visualization of Roll, Pitch, and Yaw

Using the previous lesson 19 code for Arduino and Python: TTB: 9-AXIS IMU LESSON 19: VPYTHON VISUALIZATION OF PITCH AND YAW

Note : `v`is the `up_vector`

Rotate vector `v` around `k`, using Rodrigues’ formula (which uses theta, for what we call phi)

However, the dot product of orthogonal vectors (our `k` and `v`) is zero, so the last term disappears.

```# v_rot = v*cos(roll)+ cross(k, v)*sin(roll)
up_vecotr_rot_rot = up_vector*cos(roll)+ cross(k, up_vector)*sin(roll)```

Also change the side vector

`side_vector_rot = cross(k, up_vector_rot)`

Now substitute `up_vector` for `up_vector_rot` and likewise for `side_vector` for `side_vector_rot`.

Final note: Trig can/will break at greater than 45° – due to assumptions made in the trig. Need to use quaternions

## Lesson 21

9-Axis IMU LESSON 21: Visualizing 3D Rotations in Vpython using Quaternions

For example at 90° tilt the board can suddenly flip to face the wrong direction.

Limitation of the Euler angles sort of need a 4th number for the rotation, and there are ambiguities (many ways of reaching the same location):

• Yaw 90°, is the same as
• roll 90°, pitch 90°, roll -90°
• NESW are ok in the horizontal X-Y plane, but if you roll -90° then NESW means less as you are no longer in the horizontal plane.
• Euler angles have limitations when describing rotation in the 3D space.

Imaginary numbers, i:

`i = √-1`

then

```i = i
i2 = -1
i3 = -i
i4 = 1
```

Rotation:

```i => 90°
i1/2 => 45°```

so

`iθ/90 => θ° rotation`

and with a vector

`v.iθ/90 => θ° rotation`

Which is much easier than matrices.

So this works well, for just yaw. But having also roll, pitch and yaw, we need a set of three imaginary numbers (i, j and k).

```i = i
i2 = -1
j2 = -i
k2 = -1
i.j = k
j.k = i
k.i = j
i.j.k = k.k = -1```

Note that `i.j = k` is the same as, or similar to, the cross product and follows the right hand rule.

Also, not commutative

```j.i = -k
k.j = -i
i.k = -j
```

However, previously we said that 4 numbers are required, so there is W which is a real number. Hence, there are four quaternions: W, i, j, k.

So, returning back to the Arduino code for the previous lesson 20: TTB: 9-AXIS IMU LESSON 20: VPYTHON VISUALIZATION OF ROLL, PITCH, AND YAW

We can just pull the quaternions from the IMU – this is why it is \$30 and not \$5.

` imu::Vector<4> quat = myIMU.getVector(Adafruit_BNO055::VECTOR_QUATERNION);`

Send as quaternions to VPython,

```  Serial.print(quat.w());
Serial.print(",");
Serial.print(quat.x());
Serial.print(",");
Serial.print(quat.y());
Serial.print(",");
Serial.print(quat.z());
Serial.print(",");
Serial.print(accel);
Serial.print(",");
Serial.print(gyro);
Serial.print(",");
Serial.print(mg);
Serial.print(",");
Serial.println(system_cal);```

Finally, convert back to Euler angles. There are many different methods, but use the Wikipedia: Conversion between quaternions and Euler angles

```q0 = float(splitPacket)  # W
q1 = float(splitPacket)  # i
q2 = float(splitPacket)  # j
q3 = float(splitPacket)  # k

# Convert quaternions to Euler, using https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
# roll = atan2(2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2))
# pitch = asin(2*(q0*q2-q3*q1))
# yaw = atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3))

roll = -atan2(2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2))
pitch = asin(2*(q0*q2-q3*q1))
yaw = -atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3)) - np.pi/2```

Note the tweaking of the direction and orientation in software. In the real world it is better to orient the IMU correctly

Now the glitching has gone

Now the 90° rolls work correctly, without flipping of faces

May need to use `try/catch` in the code, as if a bad data point occurs the code dies.

Slo nosedive kills it, maybe due to the arctan asymptote.

Code with try/except:

```# IMU_VPython_quat_arduino-2.py

# IMU_VPython_quat_arduino-1.py

# IMU_VPython_trig_arduino-1.py

# IMU_VPython_trig-1.py

from vpython import *
from time import *
import numpy as np
# import math
import serial

arduino_data = serial.Serial("COM5", 115200)
sleep(1)

# Conversion
toRad = 2 * np.pi / 360

# Setting the scene
scene.range = 5
scene.forward = vector(-1, -1, -1)
scene.height = 600
scene.width = 600

# For orientation (debug)
x_arrow = arrow(axis=vector(1, 0, 0), length=2, shaftwidth=0.2, color=color.red)
y_arrow = arrow(axis=vector(0, 1, 0), length=2, shaftwidth=0.2, color=color.green)
z_arrow = arrow(axis=vector(0, 0, 1), length=2, shaftwidth=0.2, color=color.blue)

# For the vector components
front_arrow = arrow(axis=vector(1, 0, 0), length=4, shaftwidth=0.2, color=color.magenta)
up_arrow = arrow(axis=vector(0, 1, 0), length=4, shaftwidth=0.2, color=color.cyan)
side_arrow = arrow(axis=vector(0, 0, 1), length=4, shaftwidth=0.2, color=color.orange)

bBoard = box(length=6, width=2, height=0.2, opacity=0.8, pos=vector(0, 0, 0))
bno = box(length=1, width=0.75, height=.1, pos=vector(-0.5, 0.1 + 0.05, 0), color=color.blue)
nano = box(length=1.75, width=0.6, height=.1, pos=vector(-2, 0.1 + 0.05, 0), color=color.blue)
# Make one object
myObject = compound([bBoard, bno, nano])

while True:
try:
while arduino_data.inWaiting() == 0:
pass
dataPacket = str(dataPacket, 'utf-8')
splitPacket = dataPacket.split(',')

q0 = float(splitPacket)  # W
q1 = float(splitPacket)  # i
q2 = float(splitPacket)  # j
q3 = float(splitPacket)  # k

# Convert quaternions to Euler, using https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
# roll = atan2(2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2))
# pitch = asin(2*(q0*q2-q3*q1))
# yaw = atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3))
roll = -atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2))
pitch = asin(2 * (q0 * q2 - q3 * q1))
yaw = -atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3)) - np.pi / 2

rate(20)  # rate(50)  ???

k = vector(cos(yaw) * cos(pitch), sin(pitch), sin(yaw) * cos(pitch))
y = vector(0, 1, 0)
side_vector = cross(k, y)  # aka "s"
up_vector = cross(side_vector, k)  # aka "v"

# v_rot = v*cos(roll)+ cross(k, v)*sin(roll)
up_vector_rot = up_vector * cos(roll) + cross(k, up_vector) * sin(roll)

side_vector_rot = cross(k, up_vector_rot)

front_arrow.axis = k
side_arrow.axis = side_vector
up_arrow.axis = up_vector
front_arrow.length = 4
side_arrow.length = 4
up_arrow.length = 4

myObject.axis = k
myObject.up = up_vector
# except: # Has two warnings: don't use bare word except, and; too broad exception class
except e:
pass```

Note from this comment, send 4 decimal places from the Arduino:

`Serial.print(quat.x(),4);`

Note: To add “N”, “E”, and “U”, from this comment:

```Xarrow=arrow(axis=vector(1,0,0),length=2,shaftwidth=.1,color=color.red)
Xtext=text(text='N',pos=vector(2,0,0),height=.2,color=color.red)
Yarrow=arrow(axis=vector(0,1,0),length=2,shaftwidth=.1,color=color.green)
Ytext=text(text='U',pos=vector(-.075,2,0),height=.2,color=color.green)
Zarrow=arrow(axis=vector(0,0,1),length=2,shaftwidth=.1,color=color.blue)
Ztext=text(text='E',pos=vector(-.075,0,2),height=.2,color=color.blue)```

Apparently, according to this comment, the try/except is required because:

That you had to make a try and exeption there is because of the calculation of the rollwhen its at -+90°. arcsin isnt defined there. In Wikipedia its standing that this situation ist called gimbal lock. Better solution is in the c++ code implemantation in the article. In Python you had to add:

```temproll=2*(q0*q2-q3*q1)
if (abs(temproll) >= 1):
roll= copysign(np.pi/2, temproll) # use 90 degrees if out of range
else:
roll= asin(temproll)```

which should actually be pitch, (not for roll, as stated in the comment):

```sin_temp_pitch = 2 * (q0 * q2 - q3 * q1)
if abs(sin_temp_pitch) >= 1:
pitch = copysign(np.pi / 2, sin_temp_pitch)  # use 90 degrees if out of range
else:
pitch = asin(sin_temp_pitch)```

This long comment:

I tried the BNO055 as red USB stick version,
and my findings , problems are
1. its a good to study the BOSCH bno055 PDF: just google for “BST-BNO055-DS000-12” and you will find that PDF
2. with the stock calculations , that paul gives us, we can get endless rotation , yeahha
BUT
endless rotation is destroyed as
3. i tried to redefine turning orientations , using -1*(…)
4. interchanged, remapped axles, using remapping in code

Paul said, that the BNO Chip orientation on the board isnt documented
BUT look in the PDF on PAGE-25 and PAGE-24, with that information we can solve that.
the BNO055 allows us to remap axles in the chip itself (AXIS_MAP_CONFIG) !
and it allows to define/revert the turn orientation (AXIS_MAP_SIGN) !

Attention:
PAGE-30
here we see Fusion data output format is possible in two ways, in Windows and in Android format,
that is “only” important for Pitch, see “Table 3-13: Rotation angle conventions” !
that does NOT affect Quaterions, because we can NOT set UNIT_SEL for Quaterions, see “Table 3-11: unit selection”.
AND
here we see that Pitch and Yaw have a range with 360degree.. but surprise Roll comes only with 180degree?!
Quaternions are not delivered in degree (their value range is -1…0…+1)
BUT
i see that one quaternion looks always to be only half in size.. i supose its the Roll !

Checking and adjusting these 3 Features (AXIS_MAP_CONFIG, AXIS_MAP_SIGN, )

Quaternion value Range and the Pitch calculation -> math domain error
because math.asin() argument has to be -1 to +1
and my BNO055 delivered values for q0,q1,q2,q3
in sharp edge cases only, that result in values smaller -1 and greater +1 .. and math.asin throws exception error
try:
pitch = math.asin( 2*(q0*q2 – q3*q1) )
except:
pass

btw, i use:
BNO055 USB Stick Linux Python Driver, bno055_usb_stick_py, version=0.9.5, from Dr. Konstantin Selyunin,
with small code changes on windows, python3.

## Lesson 22

9-Axis IMU LESSON 22: How to Create a Tilt Stabilized Platform with Arduino

Project 1: Pitch and roll stabiliser, which requires:

• 2 servos MG996, but the HiTech – HS-422 are better
• U and C bracket kit
• Breadboard power supply – included in the Lego MindStorm kit, on the other Arduino course
• 9V 1A PSU

Project 2: Camera pointer (put iMU on your head and camera follows)

## Lesson 23

9-Axis IMU LESSON 23: Self Leveling Platform Using BNO055 and Arduino Controlled Servos

• Control1 -> D2
• Control2 -> D3

Set the servos to 90° before assembly.

Fit C horn to servo, perpendiculcar to the length of the long servo side

Wide lip of bearing to outside of U bracket, bolt from inside to outside, nut on outside.

Servo lips on outside of C.bolts from outside to inside, nuts on inside.

horn on U on inside. Bolts outside to inside into the horn.

Fit U horn to servo, parallel to the length of the long servo side

U is tilt

C is roll (?), not pan

The test code, set the min to 30 and not 0°, and max to 145 to 180°, for the tilt servo, to avoid crash

## Lesson 24

Possibly one of the worst lesson, due to one handed typing, holding the damn servo crap in the other hand…

Axis IMU LESSON 24: How To Build a Self Leveling Platform with Arduino

Use the code from Lesson 21: TTB: 9-AXIS IMU LESSON 21: VISUALIZING 3D ROTATIONS IN VPYTHON USING QUATERNIONS

```  q0 = quot.w();
q1 = quot.x();
q2 = quot.y();
q3 = quot.z();```

Use the two lines from the python code as well for pitch and roll

Desired roll/pitch, is the desired roll/pitch at rest, usually zero.

`PI` is defined in `Arduino.h`

Horrible piece of code.

```// IMU_BNO055_Acc_gyro_mag_quaternions_tilt_roll.ino
// IMU_BNO055_Acc_gyro_mag_quaternions.ino
// IMU_BNO055_Acc_gyro_mag_short.ino
// IMU_BNO055_Acc_gyro_mag.ino
// IMU_BNO055_Accelerometer_gyro_combined.ino

// IMU_BNO055_Accelerometer_tilt_filtered_gyro.ino
// IMU_BNO055_Accelerometer_tilt_filtered.ino
// IMU_BNO055_Accelerometer_tilt.ino

// IMU_BNO055_Accelerometer_Calibration.ino
// IMU_BNO055_Accelerometer.ino

// IMU_BNO055_BareMinimum.ino

#include <Wire.h>
#include <utility/imumaths.h>
#include <math.h>
#include <Servo.h>

Servo servoPitch;
Servo servoRoll;

float rollDesired = 0;
float pitchDesired = 0;
float rollServo = 90;
float pitchServo = 90;

#define BNO055_SAMPLERATE_MS 100

void setup() {
Serial.begin(115200);
myIMU.begin();
delay(1000);

int8_t temp = myIMU.getTemp();
Serial.println("Temperature: ");
Serial.println(temp);
myIMU.setExtCrystalUse(true);

servoRoll.attach(2);
servoPitch.attach(3);

servoRoll.write(rollDesired);
delay(20);
servoPitch.write(pitchDesired);
delay(20);
}

void loop() {
uint8_t system_cal, gyro, accel, mg = 0;
myIMU.getCalibration(&system_cal, &gyro, &accel, &mg);

imu::Quaternion quat = myIMU.getQuat();

float q0 = quat.w();
float q1 = quat.x();
float q2 = quat.y();
float q3 = quat.z();

float rollError;
float pitchError;

float rollActual = -atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2));
float pitchActual = asin(2 * (q0 * q2 - q3 * q1));

rollActual = (rollActual / 2 * PI) * 360;
pitchActual = (pitchActual / 2 * PI) * 360;

rollError = rollActual - rollDesired;
pitchError = pitchActual - pitchDesired;

if (pitchError > 1.5) {
pitchServo += 1;
servoPitch.write(pitchServo);
delay(20);
}

if (pitchError < 1.5) {
pitchServo -= 1;
servoPitch.write(pitchServo);
delay(20);
}

if (rollError > 1.5) {
rollServo += 1;
servoRoll.write(rollServo);
delay(20);
}
if (rollError < 1.5) {
rollServo -= 1;
servoRoll.write(rollServo);
delay(20);
}

//  Serial.print(quat.w());
//  Serial.print(",");
//  Serial.print(quat.x());
//  // Serial.print(quat.x(),4);  // Apparently better
//  Serial.print(",");
//  Serial.print(quat.y());
//  Serial.print(",");
//  Serial.print(quat.z());
//    Serial.print(",");

Serial.print(rollDesired);
Serial.print(",");
Serial.print(rollActual);
// Serial.print(quat.x(),4);  // Apparently better
Serial.print(",");
Serial.print(pitchDesired);
Serial.print(",");
Serial.print(pitchActual);
Serial.print(",");
Serial.print(accel);
Serial.print(",");
Serial.print(gyro);
Serial.print(",");
Serial.print(mg);
Serial.print(",");
Serial.println(system_cal);

delay(BNO055_SAMPLERATE_MS);

}```

No ringing, but it is slow… we change by 1. The  correction is the same if the error is 3° or 45°, there is no feedback. If change is increased, more snappy, but oscillates

##### Homework

Tweak control part (clue – maybe four if statements not required)

## Chapter 25

9-Axis IMU LESSON 25: Proportional Control System for Self Leveling Platform

Correction should be proportion to the magnitude of the error. Proportional feedback.

9-AXIS IMU LESSON 24: HOW TO BUILD A SELF LEVELING PLATFORM WITH ARDUINO

Proportional

```  rollError = rollActual - rollDesired;
pitchError = pitchActual - pitchDesired;

if (abs(pitchError > 1.5)) {
pitchServo += pitchError/4;  # faster - /2
servoPitch.write(pitchServo);
delay(20);
}

if (abs(rollError > 1.5)) {
rollServo += rollError/4;   # faster - /2
servoRoll.write(rollServo);
delay(20);
}
```

## Lesson 26

9-Axis IMU LESSON 26: Understanding PID Control systems with Arduino

Control is managing error – difference between where I want to be and where I am.

• Constant correction – move 1° if error is 5° or 50°
• Very simple
• very stable
• very slow
• Proportinial correction – 10° error, move 5° then 2° then 1°
• Make it too fast and you get ringing
• The ringing can be longer than the constant correction would be
• Add too much and you get oscillation
• Derivative: Consider not only the magnitude of the error but also the Rate of change of error (slope) or derivative
• When the error is increasing the derivative is positive, and so the error is reduced by that positive
• When the error is being corrected, it is reducing, and so the slope/derivative is negative, and hence the correction is being reduced, or slowed – so that the chance of overshooting, and ringing is reduced.
• On the way up and the way down th derivative is adding to the stability.
• Short term correction
• Integral error – this deals with constant steady state error offsets.
• A constant error has no slope, therefore the Differential is zero
• A small constant error may be too small for the proportional control to fix
• The area under the constant error, will increase over time, and so the error correction is long term
`Correction = k1*Error + k2*(dError/dt) + k3*(Area+error*dt)`

Starting with the code from lesson 25, TTB: 9-AXIS IMU LESSON 25: PROPORTIONAL CONTROL SYSTEM FOR SELF LEVELING PLATFORM

Need to add three new variables,

```float k1 = 0.5;
float k2 = 250;
float k3 = 0.001;```

Why the difference in size? First is taking half the error. Second is bigger because dt is in milliseconds and hence is big (as there are a lot of milliseconds), and this is the denominator and so the result will be small, and thus needs “amplifying by a large constant,

Third is big,  because multiplied by dt, and so is very big, hence the constant needs to be small.

Basically, the third constants are trying to give equal weight to each term

```// Proportional variables
float rollDesired = 0;
float pitchDesired = 0;
float rollServo = 90;
float pitchServo = 90;
// Proportional variables declared locally to loop()
//  float rollError;
//  float pitchError;
//  float rollActual;
//  float pitchActual;
// Differential errors
// Some more errors could declared locally to loop() -> Change, Area and Slope.
float rollErrorOld;
float pitchErrorOld;
float rollErrorChange;  // delta, dE
float pitchErrorChange;  // delta, dE
float rollErrorSlope = 0;  // Set to zero to avoid any unexpected initial control
float pitchErrorSlope = 0;  // Set to zero to avoid any unexpected initial control
// Integral errors
float rollErrorArea = 0;  // Set to zero to avoid any unexpected initial control
float pitchErrorArea = 0;  // Set to zero to avoid any unexpected initial control```

Get first reading of `millisNew` in `setup()`. Shouldn’t this be `millisOld`?

` millisOld = millis();`

Delete the following lines of the previous proportional control

```  rollError = rollActual - rollDesired;
pitchError = pitchActual - pitchDesired;

if (abs(pitchError > 1.5)) {
pitchServo += pitchError/4;
servoPitch.write(pitchServo);
delay(20);
}

if (abs(rollError > 1.5)) {
rollServo += rollError/4;
servoRoll.write(rollServo);
delay(20);
}```

and put these fucking lines (man I am just so sick of the this lesson and course):

```  // Calc Error

millisOld = millisNew;
millisNew = millis();
dt = millisNew - millisOld;

rollErrorOld = rollError;
rollError = rollDesired - rollActual;
rollErrorChange = rollError - rollErrorOld;
rollErrorSlope = rollErrorChange/dt;
rollErrorArea = rollErrorArea + rollError*dt;

pitchErrorOld = pitchError;
pitchError = pitchDesired - pitchActual;
pitchErrorChange = pitchError - pitchErrorOld;
pitchErrorSlope = pitchErrorChange/dt;
pitchErrorArea = pitchErrorArea + pitchError*dt;```

Ringing may be caused by the integral, so reduce its effect from 0.001 to 0.00001. The Integral is not really needed in this project (but may be useful in others)

Also a derivative of 70 may not be slowing the “brake effect” enough, so increase to 200-250. The higher the faster it will react. However, this produces an oscillation. So reduce to 100. But still ringing.

So reduce the proportional to 0.2 and allow the differential to do most of the work. But now glitching on the way backdown as the diff is working a little too well.

So try 0.4 and 80. Better, but still a glitch on the way back down, the diff is working too hard.

Back to 0.5 and 70, the starting values. This shows bad ringing on the way back down.

So reduce diff to 55. Now, no overshoot nor ringing nor glitching on the way down.

There’s no real formula for this, just plug in values (empirically). However, as K1 is increased, k2 need to increase as well… until you reach a point of no stability. If the values are too low then you don’t get the best performance from the control system.

##### Homework

Find your k1 and k2. You want as quick as possible and as stable as possible.

## Homework – IMU

• Lesson 2 – Examine the accelerometer three channels and explain the values
• Lesson 5 – Tilt measurement tool, using the Z axis only
• Lesson 6 – How to compensate for vibration being measured as tilt.
• Lesson 7 – Make it quicker without introducing noise.
• Lesson 8 – Use gyro and accelerometer together to combine advantages
• Lesson 9 – Tweak the filter values. Also, use the filtered accelerometer.
• Lesson 13 – Make Bread board as realistic as possible – Done
• Lesson 14 – Make a cube, put marbles in it and bounce them around – Done
• Lesson 15 – Add more marbles, on independent paths
• Lesson 24 – Tweak control part (maybe four if statements not required)
• Lesson 26 – Find your k1 and k2.

This is the end, my friend