I have been trying to get orientation data using only the gyros on my BNO055 IMU for a while now and have run into a few problems. The software procedures that I have coded in look like this: First, the BNO055 assumes it is stationary and gets the sum of 200 gyro measurements in each axis and then divides the sum by 200 to get the average. This is then used as the gyro bias estimate and subtracted from the final gyro estimate. This gyro estimate then goes through quaternion code which converts it into euler angles to get orientation in degrees. I have used my own quaternion library and another one that someone else made and posted onto github. After some examination, It appears that everything is working and that the code performs well if the gyros are not moved much. However, as soon as the gyros are moved at a moderate pace, the attitude estimates drift a lot and can be around 30-40 degrees of deviation which is quite a lot. A kalman filter cannot be used because this is for a rocket application and only the gyros are suitable.
Here is everything that I use:
Raspberry Pi Pico
Adafruit BNO055
Custom BNO055 library that I coded in myself (This is the first library I ever made and it is not that good)
Quaternion Library (I have my own and another one that someone else made. They give the same results)
Some help here would be greatly appreciated as I am not sure what is causing this much drift.
/*
JRD Propulsion thrust vector control software (flight version)
By: Aryan Kapoor
Last updated on: 12/2/22
*/
/*
General information and instructions below(Please Read carefully!):
State indicator:
0 = IMU calibration
1 = Startup functions
2 = Pad idle, waiting for acceleration spike
3 = Liftoff detected, controlled ascent and data logging
4 = Descent detected and fire pyro channels
5 = Nothing, just to get out of state 4
Servo offsets and ratio:
- For the servo offsets, change the X and Y values until the mount is straigt when the computer is vertical
- To tune the servo ratio to tvc mount actuation, mount a MPU6050 on the motor tube with an arduino and calibrate the MPU6050 so that
when the computer is completely vertical, the orientation of the MPU6050 reads 0. Then, actuate the TVC mount by comanding to the move
the servos until the motor tube hits the end of mount. Then, divide the servo actuation value (number of steps it took to reach the end)
by the MPU6050 reading (at what angle was the TVC mount when it touched the end).
Required libraries:
- BNO055.h
- Orientation.h
- servo.h
*/
// Importing libraries
#include <Arduino.h>
#include <BNO055.h>
#include <Orientation.h>
//#include "Servo.h"
// Initialization
BNO055_sensor BNO055;
Orientation orientation;
//Servo servo_x;
//Servo servo_y;
int LED_1 = 6;
uint64_t this_loop = 0;
uint64_t last_update = 0;
int state = 0;
float missionTime = 0;
double accel_x, accel_y, accel_z;
double gyro_x, gyro_y, gyro_z;
double euler_x, euler_y, euler_z;
double gyro_bais_x, gyro_bais_y, gyro_bais_z;
double orientation_x, orientation_y, orientation_z;
void led_blink()
{
digitalWrite(LED_1, HIGH); // Turn on LED
delay(40);
digitalWrite(LED_1, LOW); // Turn off LED
delay(40);
}
void zero_gyro()
{
gyro_x = gyro_y = gyro_z = 0;
}
void gyro_bias()
{
double gyro_x_sum = 0;
double gyro_y_sum = 0;
double gyro_z_sum = 0;
int count = 0;
int sample_count = 200; // Total number of samples collected
while (count < sample_count)
{
led_blink();
BNO055.get_gyro(&gyro_x, &gyro_y, &gyro_z);
gyro_x_sum += gyro_x;
gyro_y_sum += gyro_y;
gyro_z_sum += gyro_z;
Serial.println(gyro_x_sum, 8);
delay(10);
count += 1;
}
if (count == sample_count)
{
Serial.println("new");
Serial.println(gyro_x_sum, 8);
gyro_bais_x = gyro_x_sum / sample_count;
gyro_bais_y = gyro_y_sum / sample_count;
gyro_bais_z = gyro_z_sum / sample_count;
Serial.print("X: ");
Serial.print(gyro_bais_x, 8);
Serial.print(" Y: ");
Serial.print(gyro_bais_y, 8);
Serial.print(" Z: ");
Serial.println(gyro_bais_z, 8);
zero_gyro();
state = 1;
}
}
// the setup routine runs once when you press reset:
void setup()
{
zero_gyro();
pinMode(LED_1, OUTPUT);
digitalWrite(LED_1, HIGH); // Turn on LED
Serial.begin(9600);
BNO055.begin();
this_loop = last_update = micros(); // Set starting time after init/calibration
}
// Main loop function
void loop()
{
this_loop = micros(); // Get new microsecond timestamp for this loop
double dt = (double)(this_loop - last_update) / 1000000; // Finds elapsed microseconds since last update, converts to float, and converts to seconds
last_update = this_loop; // We have updated, set the new timestamp
missionTime += dt;
double gyro_x_cal = gyro_x - gyro_bais_x; // Calibrated x-axis gyro measurement with bias subtracted.
double gyro_y_cal = gyro_y - gyro_bais_y; // Calibrated y-axis gyro measurement with bias subtracted.
double gyro_z_cal = gyro_z - gyro_bais_z; // Calibrated z-axis gyro measurement with bias subtracted.
BNO055.get_gyro(&gyro_x, &gyro_y, &gyro_z);
BNO055.get_accel(&accel_x, &accel_y, &accel_z);
BNO055.get_euler(&euler_x, &euler_y, &euler_z);
orientation.quaternion_update(gyro_x_cal, gyro_y_cal, gyro_z_cal, dt, &orientation_x, &orientation_y, &orientation_z);
if (state == 0)
{
gyro_bias();
}
if (state == 1)
{
digitalWrite(LED_1, HIGH); // Turn on LED
Serial.print("X: ");
Serial.print(euler_x);
Serial.print(" Y: ");
Serial.print(euler_y);
Serial.print(" Z: ");
Serial.println(euler_z);
delay(50);
}
}
/*
BNO055.cpp - Library for retrieving data from the BNO055.
Created by Aryan Kapoor, 11/25/22.
Released into the public domain.
*/
#include "Arduino.h"
#include "hardware/i2c.h"
#include "BNO055.h"
// Initialization:
#define I2C_PORT i2c1 // I2C bus
int SDA = 26; // SDA GPIO pin number
int SCL = 27; // SCL GPIO pin number
void BNO055_sensor::begin()
{
// Configure I2C Communication with BNO055
_i2c_init(I2C_PORT, 100 * 1000); // Add an extra "_" at the front if using Arduino core and hardware/i2c
gpio_set_function(SDA, GPIO_FUNC_I2C);
gpio_set_function(SCL, GPIO_FUNC_I2C);
gpio_pull_up(SDA);
gpio_pull_up(SDA);
delay(5000); // Add a short delay to help BNO005 boot up
uint8_t chipID[1];
i2c_write_blocking(I2C_PORT, BNO055_I2C_ADDR, BNO055_CHIP_ID_ADDR, 1, true);
i2c_read_blocking(I2C_PORT, BNO055_I2C_ADDR, chipID, 1, false);
if(chipID[0] != 0xA0)
{
while(1)
{
Serial.print("Chip ID Not Correct - Check Connection!");
delay(5000);
}
}
// Use internal oscillator
uint8_t data[2];
data[0] = 0x3F;
data[1] = 0x40;
i2c_write_blocking(I2C_PORT, BNO055_I2C_ADDR, data, 2, true);
// Reset all interrupt status bits
data[0] = 0x3F;
data[1] = 0x01;
i2c_write_blocking(I2C_PORT, BNO055_I2C_ADDR, data, 2, true);
// Configure Power Mode
data[0] = 0x3E;
data[1] = 0x00;
i2c_write_blocking(I2C_PORT, BNO055_I2C_ADDR, data, 2, true);
delay(50);
// Default Axis Configuration
data[0] = 0x41;
data[1] = 0x24;
i2c_write_blocking(I2C_PORT, BNO055_I2C_ADDR, data, 2, true);
// Default Axis Signs
data[0] = 0x42;
data[1] = 0x00;
i2c_write_blocking(I2C_PORT, BNO055_I2C_ADDR, data, 2, true);
// Set units to m/s^2
data[0] = 0x3B;
data[1] = 0b0001000;
i2c_write_blocking(I2C_PORT, BNO055_I2C_ADDR, data, 2, true);
delay(30);
// Set operation to NDOF
data[0] = 0x3D;
data[1] = 0x0C;
i2c_write_blocking(I2C_PORT, BNO055_I2C_ADDR, data, 2, true);
delay(100);
}
double BNO055_sensor::get_accel(double *px, double *py, double *pz)
{
uint8_t accel[6]; // Store data from 6 acceleration registers
int16_t accelX, accelY, accelZ; // Combined 3 axis data
float accel_x, accel_y, accel_z; // Float type of acceleration data
uint8_t accel_val = 0x08; // Start register address = 0X08;
i2c_write_blocking(I2C_PORT, BNO055_I2C_ADDR, &accel_val, 1, true); // I2C write function
i2c_read_blocking(I2C_PORT, BNO055_I2C_ADDR, accel, 6, false); // I2C read function
accelX = ((accel[1]<<8) | accel[0]);
accelY = ((accel[5]<<8) | accel[4]);
accelZ = ((accel[3]<<8) | accel[2]);
accel_x = accelX / 100.00;
accel_y = accelY / 100.00;
accel_z = accelZ / 100.00;
*px = accel_x;
*py = accel_y;
*pz = accel_z;
return 0;
}
double BNO055_sensor::get_gyro(double *px, double *py, double *pz)
{
uint8_t gyro[6]; // Store data from 6 gyro registers
int16_t gyroX, gyroY, gyroZ; // Combined 3 axis data
float gyro_x, gyro_y, gyro_z; // Float type of gyro data
uint8_t gyro_val = 0x14; // Start register address = 0X14;
i2c_write_blocking(I2C_PORT, BNO055_I2C_ADDR, &gyro_val, 1, true); // I2C write function
i2c_read_blocking(I2C_PORT, BNO055_I2C_ADDR, gyro, 6, false); // I2C read function
gyroX = ((gyro[1]<<8) | gyro[0]);
gyroY = ((gyro[3]<<8) | gyro[2]);
gyroZ = ((gyro[5]<<8) | gyro[4]);
gyro_x = (DEG_TO_RAD * (gyroX / 16.0));
gyro_y = (DEG_TO_RAD * (gyroY / 16.0));
gyro_z = (DEG_TO_RAD * (gyroZ / 16.0));
*px = gyro_x;
*py = gyro_y;
*pz = gyro_z;
return 0;
}
double BNO055_sensor::get_euler(double *px, double *py, double *pz)
{
uint8_t gyro[6]; // Store data from 6 gyro registers
int16_t gyroX, gyroY, gyroZ; // Combined 3 axis data
float gyro_x, gyro_y, gyro_z; // Float type of gyro data
uint8_t gyro_val = 0x1A; // Start register address = 0X14;
i2c_write_blocking(I2C_PORT, BNO055_I2C_ADDR, &gyro_val, 1, true); // I2C write function
i2c_read_blocking(I2C_PORT, BNO055_I2C_ADDR, gyro, 6, false); // I2C read function
gyroX = ((gyro[1]<<8) | gyro[0]);
gyroY = ((gyro[3]<<8) | gyro[2]);
gyroZ = ((gyro[5]<<8) | gyro[4]);
gyro_x = ((gyroX / 16.0));
gyro_y = ((gyroY / 16.0));
gyro_z = ((gyroZ / 16.0));
*px = gyro_x;
*py = gyro_y;
*pz = gyro_z;
return 0;
}
I have a strong feeling that I am getting issues because the sensor is never truly calibrated. Yes, there is a manual calibration in the flight software but that is just done using simple math, not like what the BNO055 would do if I used the calibration registers and properly calibrated the sensor using I2C.
My impression with the BNO055 is that you can never turn off the calibration, which doesn't work well in any case. If so, the background calibration is working against your efforts. I gave up on that sensor years ago.
Gyro-only integration of the quaternion works surprisingly well if you choose small enough time steps, making sure that the rotation rate is expressed in units of radians/second and the offsets are removed, even with the obsolete MPU-6050. Code to do that can be found here, based on the Mahony filter.
I see. Well, I don't want to make a hardware change as everything is on a custom PCB that took a while to design. I know of other people who were successful using the BNO055 for the same purposes. I think if I switch the mode to non sensor fusion, it will output non calibrated data, as the data sheet clearly says. You may be right.
A common mistake is to integrate degrees/second, rather than radians/second, and that leads to huge jumps.
Yes, I have made sure that is not the case.
Have you actually checked whether the gyro offset correction is stable?
My approach to do so would be to calculate offsets with the sensor lying flat on the table, then in a second pass, subtract them from the data. If the gyro rates remain near zero for some time, pick up the sensor, rotate it around for a few seconds, then lay it back down. The rates should all return to zero.
I mean if I move it around slowly and not that much, everything seems fine. Its only if I jerk it then it will randomly add 50 or something to all 3 axes and thats exactly what I do for the gyro offsets.
My suggested test refers to the raw gyro values, to isolate other possible sources of error.
What you observe has been reported by others (even with the built in AHRS firmware!), but no one has gotten to the bottom of it, as far as I'm aware.
Yes, it appears you are right. The raw gyro measurements are suspiciously constantly hovering around 0.00 degrees and 95% of the time they are equal to that when stationary. Even after I move it, the values go back to zero. So now I need to change the operating mode from NDOF (which is what I was using) to a non-sensor-fusion mode so I can get fully raw data. The Datasheet says that the non-sensor-fusion data doesn't have any calibration.
How would you go about changing from Ndof mode? You write to the register 0x3D and then do what? What do you write to the register?
The only issue is that the errors are often much greater than 10-15 degrees like around 30-40 but sometimes they are like 2-5. But I think this is it.
I would try setting register 3D (OPR_MODE) to non-fusion mode GYROONLY (0x03) to start.
With rapid movements and inappropriate time steps, Euler integration simply won't work as you expect. In that case either make the time steps smaller (if the gyro update rate even allows that) or use a higher order method of integration.
What do you mean by Euler integration?
Euler integration is the simplest possible and least accurate method of numerical integration.
if v = dx/dt, then the Euler method calculates x = x0 + v*(delta T) where delta T is the time step.
Hi,
Don't forget it was the gaming and auto industry that encouraged these "CHEAP" sensors and not necessarily for precision usage.
When a car has an accident the sensor doesn't have to be all that precise, just stable.
I seem to be seeing a lot of questions about the BNO055, but the "Obsolete" 6050 didn't seem to have these complications.
Tom.. 
