Hi Adafruiters
I recently got the BNO055 sensor (9-DOF IMU) which seems to be a really great IMU - ease of use, update rate, precision.
But after some tinkering around, I have a question:
How can I find magnetic north with this sensor? I'm using the Unified Sensor Driver from Adafruit, and the Euler angles returned always seem to init to 0. Is there a way to get the absolute x angle (magnetic north) with this sensor?
Thanks
protonstorm
BNO055 IMU - Find magnetic north
Moderators: adafruit_support_bill, adafruit
Please be positive and constructive with your questions and comments.
- adafruit_support_bill
- Posts: 88154
- Joined: Sat Feb 07, 2009 10:11 am
Re: BNO055 IMU - Find magnetic north
Take a look at the RawData example in the library. If you edit it to use VECTOR_MAGNETOMETER , it will return the raw magnetometer data.
- protonstorm
- Posts: 12
- Joined: Wed Jun 17, 2015 5:37 am
Re: BNO055 IMU - Find magnetic north
Thanks for your reply.
According to the BNO055 library, VECTOR_MAGNETOMETER returns micro Teslas. I got the BNO055 to get around all the math, I suppose getting absolute heading from uT returned is not a simple task?
According to the BNO055 library, VECTOR_MAGNETOMETER returns micro Teslas. I got the BNO055 to get around all the math, I suppose getting absolute heading from uT returned is not a simple task?
- adafruit_support_bill
- Posts: 88154
- Joined: Sat Feb 07, 2009 10:11 am
Re: BNO055 IMU - Find magnetic north
Not so difficult:
heading = atan2(magnetometer.y, magnetometer.x);
heading = atan2(magnetometer.y, magnetometer.x);
- protonstorm
- Posts: 12
- Joined: Wed Jun 17, 2015 5:37 am
Re: BNO055 IMU - Find magnetic north
Thanks for your reply
As far as I understand this only works if the sensor is level, no?
As far as I understand this only works if the sensor is level, no?
- adafruit_support_bill
- Posts: 88154
- Joined: Sat Feb 07, 2009 10:11 am
Re: BNO055 IMU - Find magnetic north
That is correct. A tilt-compensated heading calculation is a bit more involved:
http://www.timzaman.com/2011/04/heading ... d-compass/
http://www.timzaman.com/2011/04/heading ... d-compass/
- protonstorm
- Posts: 12
- Joined: Wed Jun 17, 2015 5:37 am
Re: BNO055 IMU - Find magnetic north
Ah, that doesn't look too hard though.
Thanks!
Thanks!
- frankjoshua
- Posts: 1
- Joined: Tue Jun 09, 2015 9:20 pm
Re: BNO055 IMU - Find magnetic north
Did any one get this working yet?
I have tried this but no joy so far. Also I don't need tilt compensation but it would be nice.
I have been working on this for a few weeks and this is as close as I have been so far. I can't be the only one in the world who needs a magnetic heading.
I have tried this but no joy so far. Also I don't need tilt compensation but it would be nice.
Code: Select all
imu::Vector<3> vector;
float mCompassHeading;
void updateCompassHeading(){
vector = bno.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);
mCompassHeading = atan2(vector.y(), vector.x());
// Correct for when signs are reversed.
if(mCompassHeading < 0)
mCompassHeading += 2*PI;
// Convert radians to degrees
mCompassHeading = mCompassHeading * 180/M_PI;
}
- protonstorm
- Posts: 12
- Joined: Wed Jun 17, 2015 5:37 am
Re: BNO055 IMU - Find magnetic north
Yeah I been trying it this way, too. The reason it doesn't work is (I think) that the sensor is put into a Fusion Mode by default. Which means that it's output for Euler is already "fused" with other sensors.
You can use a different fusion mode which gives you compass heading (but gyro is off in this mode) like this:
(See the .h file of the library near the end)
Change:
to:
This will lead to absolute euler angles. Note that the sensor needs to be calibrated after each power cycle by doing some random figure 8 patterns (until somebody implements saving of calibration in the library.....I might do it if I find the time)
Edit: It just occured to me that the manual way (getting the magnetometer data) might actually work if you also do the calibration patterns. Will try this out tomorrow.
Hope this helps a bit.
Protonstorm
You can use a different fusion mode which gives you compass heading (but gyro is off in this mode) like this:
(See the .h file of the library near the end)
Change:
Code: Select all
bool begin ( adafruit_bno055_opmode_t mode = OPERATION_MODE_NDOF );
Code: Select all
bool begin ( adafruit_bno055_opmode_t mode = OPERATION_MODE_COMPASS );
Edit: It just occured to me that the manual way (getting the magnetometer data) might actually work if you also do the calibration patterns. Will try this out tomorrow.
Hope this helps a bit.
Protonstorm
- davegun
- Posts: 89
- Joined: Sun Sep 08, 2013 11:00 pm
Re: BNO055 IMU - Find magnetic north
I got it to work! The magnetic compass points north and is tilt compensated. I also found out how to calibrate the sensor so it points north on startup without calibration. I ended up modifying the library to make this work. It's not the best solution, and I think it could be written more efficiently, but it works well. I post it here:
http://forums.adafruit.com/viewtopic.php?f=19&t=73014
This is a very cool sensor and it's rock solid. Once it's calibrated for your project, it points north right at starup without having to twirl it around.
Dave
http://forums.adafruit.com/viewtopic.php?f=19&t=73014
This is a very cool sensor and it's rock solid. Once it's calibrated for your project, it points north right at starup without having to twirl it around.
Dave
- protonstorm
- Posts: 12
- Joined: Wed Jun 17, 2015 5:37 am
Re: BNO055 IMU - Find magnetic north
@davegun I basically just did all this, too. I think your way is really not very efficient but probably works well enough ;)
The issue I have atm is, that most of my configuration values are 0 (And yours seem not) I don't know if thats a good thing or if I'm missing something. Did you run accross this issue? I see you wait for all the CALIB_STAT bits to be 1, not just the SYS bits, so that might be my mistake.
The main thing I learned so far is that with OPERATION_MODE_NDOF as default the sensor will initialize at 0,0,0. After a couple 8-figures the x component will actually show the actual compass heading (+/- 2.5°) So you can use this with the original library as a compass, but you will have to move it around a bit first.
If you start the sensor with OPERATION_MODE_COMPASS:
the vector will show some initial (kinda wrong) heading and after a couple 8-figures it will represent true north with a higher accuracy (in my experiments it's like +/-0.2°)
I plan to extend their library to support calibration cleanly.
The issue I have atm is, that most of my configuration values are 0 (And yours seem not) I don't know if thats a good thing or if I'm missing something. Did you run accross this issue? I see you wait for all the CALIB_STAT bits to be 1, not just the SYS bits, so that might be my mistake.
The main thing I learned so far is that with OPERATION_MODE_NDOF as default the sensor will initialize at 0,0,0. After a couple 8-figures the x component will actually show the actual compass heading (+/- 2.5°) So you can use this with the original library as a compass, but you will have to move it around a bit first.
If you start the sensor with OPERATION_MODE_COMPASS:
Code: Select all
if (!bno.begin(Adafruit_BNO055::OPERATION_MODE_COMPASS))
I plan to extend their library to support calibration cleanly.
- davegun
- Posts: 89
- Joined: Sun Sep 08, 2013 11:00 pm
Re: BNO055 IMU - Find magnetic north
Protonstorm,
The magnetometer is very sensitive so I have found you need to find your calibration values once you have the sensor mounted in your project. If you change or add anything, you may need to redo the calibration values. If you just want to calibrate the compass, you should only need to wait until the magnetic calibration status of 3. To get full calibration, it only takes a few figure 8's. It takes a lot more fiddling around to get all 3 sensors calibrated. I have been calibrating all 3, but only use the magnetic calibration data.
You must not be getting good calibration values. My values range all over the place, as an example, here are the values for my current project:
Once you get these values based on your installation, it should work every time without recalibration.
If you don't have good calibration values, it will act just as you said. I will show a wrong direction until you move it around. Sometime it only takes a little moving, other times a few figure 8s.
Dave
The magnetometer is very sensitive so I have found you need to find your calibration values once you have the sensor mounted in your project. If you change or add anything, you may need to redo the calibration values. If you just want to calibrate the compass, you should only need to wait until the magnetic calibration status of 3. To get full calibration, it only takes a few figure 8's. It takes a lot more fiddling around to get all 3 sensors calibrated. I have been calibrating all 3, but only use the magnetic calibration data.
You must not be getting good calibration values. My values range all over the place, as an example, here are the values for my current project:
Code: Select all
/**********************************
// Writes calibration data to 9DOF sensor//
void setCal(){
byte calData;
bno.setMode( bno.OPERATION_MODE_CONFIG ); // Put into CONFIG_Mode
delay(25);
calData = bno.setCalvalMRL(39);
calData = bno.setCalvalMRM(3);
calData = bno.setCalvalMOXL(151);
calData = bno.setCalvalMOXM(255);
calData = bno.setCalvalMOYL(70);
calData = bno.setCalvalMOYM(254);
calData = bno.setCalvalMOZL(44);
calData = bno.setCalvalMOZM(0);
bno.setMode( bno.OPERATION_MODE_NDOF ); // Put into NDOF Mode
delay(25);
}
If you don't have good calibration values, it will act just as you said. I will show a wrong direction until you move it around. Sometime it only takes a little moving, other times a few figure 8s.
Dave
- protonstorm
- Posts: 12
- Joined: Wed Jun 17, 2015 5:37 am
Re: BNO055 IMU - Find magnetic north
@Dave
stupid me, after searching for like an hour, I saw that I need to change into config mode for the read too, not just for write. I got meaningful values now :)
stupid me, after searching for like an hour, I saw that I need to change into config mode for the read too, not just for write. I got meaningful values now :)
- protonstorm
- Posts: 12
- Joined: Wed Jun 17, 2015 5:37 am
Re: BNO055 IMU - Find magnetic north
I finished my version of the library update. Maybe somebody wants to test it. I will probably contribute it to Github or get in contact with Kevin Townsend first.
To use this, overwrite the library cpp/h files and use the sketch (read the comments!)
Adafruit_BNO055.h:
Adafruit_BNO055.cpp:
Example Sketch for calibration:
To use this, overwrite the library cpp/h files and use the sketch (read the comments!)
Adafruit_BNO055.h:
Code: Select all
/***************************************************************************
This is a library for the BNO055 orientation sensor
Designed specifically to work with the Adafruit BNO055 Breakout.
Pick one up today in the adafruit shop!
------> http://www.adafruit.com/products
These sensors use I2C to communicate, 2 pins are required to interface.
Adafruit invests time and resources providing this open source code,
please support Adafruit andopen-source hardware by purchasing products
from Adafruit!
Written by KTOWN for Adafruit Industries.
MIT license, all text above must be included in any redistribution
***************************************************************************/
#ifndef __ADAFRUIT_BNO055_H__
#define __ADAFRUIT_BNO055_H__
#if (ARDUINO >= 100)
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#ifdef __AVR_ATtiny85__
#include <TinyWireM.h>
#define Wire TinyWireM
#else
#include <Wire.h>
#endif
#include <Adafruit_Sensor.h>
#include <utility/imumaths.h>
#define BNO055_ADDRESS_A (0x28)
#define BNO055_ADDRESS_B (0x29)
#define BNO055_ID (0xA0)
class Adafruit_BNO055 : public Adafruit_Sensor
{
public:
typedef enum
{
/* Page id register definition */
BNO055_PAGE_ID_ADDR = 0X07,
/* PAGE0 REGISTER DEFINITION START*/
BNO055_CHIP_ID_ADDR = 0x00,
BNO055_ACCEL_REV_ID_ADDR = 0x01,
BNO055_MAG_REV_ID_ADDR = 0x02,
BNO055_GYRO_REV_ID_ADDR = 0x03,
BNO055_SW_REV_ID_LSB_ADDR = 0x04,
BNO055_SW_REV_ID_MSB_ADDR = 0x05,
BNO055_BL_REV_ID_ADDR = 0X06,
/* Accel data register */
BNO055_ACCEL_DATA_X_LSB_ADDR = 0X08,
BNO055_ACCEL_DATA_X_MSB_ADDR = 0X09,
BNO055_ACCEL_DATA_Y_LSB_ADDR = 0X0A,
BNO055_ACCEL_DATA_Y_MSB_ADDR = 0X0B,
BNO055_ACCEL_DATA_Z_LSB_ADDR = 0X0C,
BNO055_ACCEL_DATA_Z_MSB_ADDR = 0X0D,
/* Mag data register */
BNO055_MAG_DATA_X_LSB_ADDR = 0X0E,
BNO055_MAG_DATA_X_MSB_ADDR = 0X0F,
BNO055_MAG_DATA_Y_LSB_ADDR = 0X10,
BNO055_MAG_DATA_Y_MSB_ADDR = 0X11,
BNO055_MAG_DATA_Z_LSB_ADDR = 0X12,
BNO055_MAG_DATA_Z_MSB_ADDR = 0X13,
/* Gyro data registers */
BNO055_GYRO_DATA_X_LSB_ADDR = 0X14,
BNO055_GYRO_DATA_X_MSB_ADDR = 0X15,
BNO055_GYRO_DATA_Y_LSB_ADDR = 0X16,
BNO055_GYRO_DATA_Y_MSB_ADDR = 0X17,
BNO055_GYRO_DATA_Z_LSB_ADDR = 0X18,
BNO055_GYRO_DATA_Z_MSB_ADDR = 0X19,
/* Euler data registers */
BNO055_EULER_H_LSB_ADDR = 0X1A,
BNO055_EULER_H_MSB_ADDR = 0X1B,
BNO055_EULER_R_LSB_ADDR = 0X1C,
BNO055_EULER_R_MSB_ADDR = 0X1D,
BNO055_EULER_P_LSB_ADDR = 0X1E,
BNO055_EULER_P_MSB_ADDR = 0X1F,
/* Quaternion data registers */
BNO055_QUATERNION_DATA_W_LSB_ADDR = 0X20,
BNO055_QUATERNION_DATA_W_MSB_ADDR = 0X21,
BNO055_QUATERNION_DATA_X_LSB_ADDR = 0X22,
BNO055_QUATERNION_DATA_X_MSB_ADDR = 0X23,
BNO055_QUATERNION_DATA_Y_LSB_ADDR = 0X24,
BNO055_QUATERNION_DATA_Y_MSB_ADDR = 0X25,
BNO055_QUATERNION_DATA_Z_LSB_ADDR = 0X26,
BNO055_QUATERNION_DATA_Z_MSB_ADDR = 0X27,
/* Linear acceleration data registers */
BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR = 0X28,
BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR = 0X29,
BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR = 0X2A,
BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR = 0X2B,
BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR = 0X2C,
BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR = 0X2D,
/* Gravity data registers */
BNO055_GRAVITY_DATA_X_LSB_ADDR = 0X2E,
BNO055_GRAVITY_DATA_X_MSB_ADDR = 0X2F,
BNO055_GRAVITY_DATA_Y_LSB_ADDR = 0X30,
BNO055_GRAVITY_DATA_Y_MSB_ADDR = 0X31,
BNO055_GRAVITY_DATA_Z_LSB_ADDR = 0X32,
BNO055_GRAVITY_DATA_Z_MSB_ADDR = 0X33,
/* Temperature data register */
BNO055_TEMP_ADDR = 0X34,
/* Status registers */
BNO055_CALIB_STAT_ADDR = 0X35,
BNO055_SELFTEST_RESULT_ADDR = 0X36,
BNO055_INTR_STAT_ADDR = 0X37,
BNO055_SYS_CLK_STAT_ADDR = 0X38,
BNO055_SYS_STAT_ADDR = 0X39,
BNO055_SYS_ERR_ADDR = 0X3A,
/* Unit selection register */
BNO055_UNIT_SEL_ADDR = 0X3B,
BNO055_DATA_SELECT_ADDR = 0X3C,
/* Mode registers */
BNO055_OPR_MODE_ADDR = 0X3D,
BNO055_PWR_MODE_ADDR = 0X3E,
BNO055_SYS_TRIGGER_ADDR = 0X3F,
BNO055_TEMP_SOURCE_ADDR = 0X40,
/* Axis remap registers */
BNO055_AXIS_MAP_CONFIG_ADDR = 0X41,
BNO055_AXIS_MAP_SIGN_ADDR = 0X42,
/* SIC registers */
BNO055_SIC_MATRIX_0_LSB_ADDR = 0X43,
BNO055_SIC_MATRIX_0_MSB_ADDR = 0X44,
BNO055_SIC_MATRIX_1_LSB_ADDR = 0X45,
BNO055_SIC_MATRIX_1_MSB_ADDR = 0X46,
BNO055_SIC_MATRIX_2_LSB_ADDR = 0X47,
BNO055_SIC_MATRIX_2_MSB_ADDR = 0X48,
BNO055_SIC_MATRIX_3_LSB_ADDR = 0X49,
BNO055_SIC_MATRIX_3_MSB_ADDR = 0X4A,
BNO055_SIC_MATRIX_4_LSB_ADDR = 0X4B,
BNO055_SIC_MATRIX_4_MSB_ADDR = 0X4C,
BNO055_SIC_MATRIX_5_LSB_ADDR = 0X4D,
BNO055_SIC_MATRIX_5_MSB_ADDR = 0X4E,
BNO055_SIC_MATRIX_6_LSB_ADDR = 0X4F,
BNO055_SIC_MATRIX_6_MSB_ADDR = 0X50,
BNO055_SIC_MATRIX_7_LSB_ADDR = 0X51,
BNO055_SIC_MATRIX_7_MSB_ADDR = 0X52,
BNO055_SIC_MATRIX_8_LSB_ADDR = 0X53,
BNO055_SIC_MATRIX_8_MSB_ADDR = 0X54,
/* Accelerometer Offset registers */
ACCEL_OFFSET_X_LSB_ADDR = 0X55,
ACCEL_OFFSET_X_MSB_ADDR = 0X56,
ACCEL_OFFSET_Y_LSB_ADDR = 0X57,
ACCEL_OFFSET_Y_MSB_ADDR = 0X58,
ACCEL_OFFSET_Z_LSB_ADDR = 0X59,
ACCEL_OFFSET_Z_MSB_ADDR = 0X5A,
/* Magnetometer Offset registers */
MAG_OFFSET_X_LSB_ADDR = 0X5B,
MAG_OFFSET_X_MSB_ADDR = 0X5C,
MAG_OFFSET_Y_LSB_ADDR = 0X5D,
MAG_OFFSET_Y_MSB_ADDR = 0X5E,
MAG_OFFSET_Z_LSB_ADDR = 0X5F,
MAG_OFFSET_Z_MSB_ADDR = 0X60,
/* Gyroscope Offset register s*/
GYRO_OFFSET_X_LSB_ADDR = 0X61,
GYRO_OFFSET_X_MSB_ADDR = 0X62,
GYRO_OFFSET_Y_LSB_ADDR = 0X63,
GYRO_OFFSET_Y_MSB_ADDR = 0X64,
GYRO_OFFSET_Z_LSB_ADDR = 0X65,
GYRO_OFFSET_Z_MSB_ADDR = 0X66,
/* Radius registers */
ACCEL_RADIUS_LSB_ADDR = 0X67,
ACCEL_RADIUS_MSB_ADDR = 0X68,
MAG_RADIUS_LSB_ADDR = 0X69,
MAG_RADIUS_MSB_ADDR = 0X6A
} adafruit_bno055_reg_t;
typedef enum
{
POWER_MODE_NORMAL = 0X00,
POWER_MODE_LOWPOWER = 0X01,
POWER_MODE_SUSPEND = 0X02
} adafruit_bno055_powermode_t;
typedef enum
{
/* Operation mode settings*/
OPERATION_MODE_CONFIG = 0X00,
OPERATION_MODE_ACCONLY = 0X01,
OPERATION_MODE_MAGONLY = 0X02,
OPERATION_MODE_GYRONLY = 0X03,
OPERATION_MODE_ACCMAG = 0X04,
OPERATION_MODE_ACCGYRO = 0X05,
OPERATION_MODE_MAGGYRO = 0X06,
OPERATION_MODE_AMG = 0X07,
OPERATION_MODE_IMUPLUS = 0X08,
OPERATION_MODE_COMPASS = 0X09,
OPERATION_MODE_M4G = 0X0A,
OPERATION_MODE_NDOF_FMC_OFF = 0X0B,
OPERATION_MODE_NDOF = 0X0C
} adafruit_bno055_opmode_t;
typedef struct
{
uint8_t accel_rev;
uint8_t mag_rev;
uint8_t gyro_rev;
uint16_t sw_rev;
uint8_t bl_rev;
} adafruit_bno055_rev_info_t;
typedef enum
{
/* bit masks to read the CALIB_STAT register */
CALIB_STAT_SYS = 0xC0,
CALIB_STAT_GYR = 0x30,
CALIB_STAT_ACC = 0x0C,
CALIB_STAT_MAG = 0x03
} adafruit_bno055_calib_stat_t;
typedef enum
{
VECTOR_ACCELEROMETER = BNO055_ACCEL_DATA_X_LSB_ADDR,
VECTOR_MAGNETOMETER = BNO055_MAG_DATA_X_LSB_ADDR,
VECTOR_GYROSCOPE = BNO055_GYRO_DATA_X_LSB_ADDR,
VECTOR_EULER = BNO055_EULER_H_LSB_ADDR,
VECTOR_LINEARACCEL = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR,
VECTOR_GRAVITY = BNO055_GRAVITY_DATA_X_LSB_ADDR
} adafruit_vector_type_t;
Adafruit_BNO055 ( int32_t sensorID = -1, uint8_t address = BNO055_ADDRESS_A );
bool begin ( adafruit_bno055_opmode_t mode = OPERATION_MODE_NDOF );
bool getCalibState ( adafruit_bno055_calib_stat_t );
void getCalibData ( byte* pbuffer);
void setCalibData ( byte* pbuffer);
void setMode ( adafruit_bno055_opmode_t mode );
void getRevInfo ( adafruit_bno055_rev_info_t* );
void displayRevInfo ( void );
void setExtCrystalUse ( boolean usextal );
void getSystemStatus ( uint8_t *system_status,
uint8_t *self_test_result,
uint8_t *system_error);
void displaySystemStatus ( void );
imu::Vector<3> getVector ( adafruit_vector_type_t vector_type );
imu::Quaternion getQuat ( void );
int8_t getTemp ( void );
/* Adafruit_Sensor implementation */
bool getEvent ( sensors_event_t* );
void getSensor ( sensor_t* );
private:
byte read8 ( adafruit_bno055_reg_t );
bool readLen ( adafruit_bno055_reg_t, byte* buffer, uint8_t len );
bool write8 ( adafruit_bno055_reg_t, byte value );
uint8_t _address;
int32_t _sensorID;
adafruit_bno055_opmode_t _mode;
};
#endif
Code: Select all
/***************************************************************************
This is a library for the BNO055 orientation sensor
Designed specifically to work with the Adafruit BNO055 Breakout.
Pick one up today in the adafruit shop!
------> http://www.adafruit.com/products
These sensors use I2C to communicate, 2 pins are required to interface.
Adafruit invests time and resources providing this open source code,
please support Adafruit andopen-source hardware by purchasing products
from Adafruit!
Written by KTOWN for Adafruit Industries.
MIT license, all text above must be included in any redistribution
***************************************************************************/
#if ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#include <math.h>
#include <limits.h>
#include "Adafruit_BNO055.h"
/***************************************************************************
CONSTRUCTOR
***************************************************************************/
/**************************************************************************/
/*!
@brief Instantiates a new Adafruit_BNO055 class
*/
/**************************************************************************/
Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address)
{
_sensorID = sensorID;
_address = address;
}
/***************************************************************************
PUBLIC FUNCTIONS
***************************************************************************/
/**************************************************************************/
/*!
@brief Sets up the HW
*/
/**************************************************************************/
bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode)
{
/* Enable I2C */
Wire.begin();
/* Make sure we have the right device */
uint8_t id = read8(BNO055_CHIP_ID_ADDR);
if(id != BNO055_ID)
{
delay(1000); // hold on for boot
id = read8(BNO055_CHIP_ID_ADDR);
if(id != BNO055_ID) {
return false; // still not? ok bail
}
}
/* Switch to config mode (just in case since this is the default) */
setMode(OPERATION_MODE_CONFIG);
/* Reset */
write8(BNO055_SYS_TRIGGER_ADDR, 0x20);
while (read8(BNO055_CHIP_ID_ADDR) != BNO055_ID)
{
delay(10);
}
delay(50);
/* Set to normal power mode */
write8(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL);
delay(10);
write8(BNO055_PAGE_ID_ADDR, 0);
/* Set the output units */
/*
uint8_t unitsel = (0 << 7) | // Orientation = Android
(0 << 4) | // Temperature = Celsius
(0 << 2) | // Euler = Degrees
(1 << 1) | // Gyro = Rads
(0 << 0); // Accelerometer = m/s^2
write8(BNO055_UNIT_SEL_ADDR, unitsel);
*/
write8(BNO055_SYS_TRIGGER_ADDR, 0x0);
delay(10);
/* Set the requested operating mode (see section 3.3) */
setMode(mode);
delay(20);
return true;
}
/**************************************************************************/
/*!
@brief Puts the chip in the specified operating mode
*/
/**************************************************************************/
void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t mode)
{
_mode = mode;
write8(BNO055_OPR_MODE_ADDR, _mode);
delay(30);
}
/**************************************************************************/
/*!
@brief Use the external 32.768KHz crystal
*/
/**************************************************************************/
void Adafruit_BNO055::setExtCrystalUse(boolean usextal)
{
adafruit_bno055_opmode_t modeback = _mode;
/* Switch to config mode (just in case since this is the default) */
setMode(OPERATION_MODE_CONFIG);
delay(25);
write8(BNO055_PAGE_ID_ADDR, 0);
if (usextal) {
write8(BNO055_SYS_TRIGGER_ADDR, 0x80);
} else {
write8(BNO055_SYS_TRIGGER_ADDR, 0x00);
}
delay(10);
/* Set the requested operating mode (see section 3.3) */
setMode(modeback);
delay(20);
}
/**************************************************************************/
/*!
@brief Gets the latest system status info
*/
/**************************************************************************/
void Adafruit_BNO055::getSystemStatus(uint8_t *system_status, uint8_t *self_test_result, uint8_t *system_error)
{
adafruit_bno055_opmode_t backupmode = _mode;
setMode(OPERATION_MODE_CONFIG);
delay(20);
write8(BNO055_PAGE_ID_ADDR, 0);
write8(BNO055_SYS_TRIGGER_ADDR, read8(BNO055_SYS_TRIGGER_ADDR) | 0x1);
delay(1000);
/* System Status (see section 4.3.58)
---------------------------------
0 = Idle
1 = System Error
2 = Initializing Peripherals
3 = System Iniitalization
4 = Executing Self-Test
5 = Sensor fusio algorithm running
6 = System running without fusion algorithms */
if (system_status != 0)
*system_status = read8(BNO055_SYS_STAT_ADDR);
/* Self Test Results (see section )
--------------------------------
1 = test passed, 0 = test failed
Bit 0 = Accelerometer self test
Bit 1 = Magnetometer self test
Bit 2 = Gyroscope self test
Bit 3 = MCU self test
0x0F = all good! */
if (self_test_result != 0)
*self_test_result = read8(BNO055_SELFTEST_RESULT_ADDR);
/* System Error (see section 4.3.59)
---------------------------------
0 = No error
1 = Peripheral initialization error
2 = System initialization error
3 = Self test result failed
4 = Register map value out of range
5 = Register map address out of range
6 = Register map write error
7 = BNO low power mode not available for selected operat ion mode
8 = Accelerometer power mode not available
9 = Fusion algorithm configuration error
A = Sensor configuration error */
if (system_error != 0)
*system_error = read8(BNO055_SYS_ERR_ADDR);
setMode(backupmode);
delay(20);
}
/**************************************************************************/
/*!
@brief Gets the chip revision numbers
*/
/**************************************************************************/
void Adafruit_BNO055::getRevInfo(adafruit_bno055_rev_info_t* info)
{
uint8_t a, b;
memset(info, 0, sizeof(adafruit_bno055_rev_info_t));
/* Check the accelerometer revision */
info->accel_rev = read8(BNO055_ACCEL_REV_ID_ADDR);
/* Check the magnetometer revision */
info->mag_rev = read8(BNO055_MAG_REV_ID_ADDR);
/* Check the gyroscope revision */
info->gyro_rev = read8(BNO055_GYRO_REV_ID_ADDR);
/* Check the SW revision */
info->bl_rev = read8(BNO055_BL_REV_ID_ADDR);
a = read8(BNO055_SW_REV_ID_LSB_ADDR);
b = read8(BNO055_SW_REV_ID_MSB_ADDR);
info->sw_rev = (((uint16_t)b) << 8) | ((uint16_t)a);
}
/**************************************************************************/
/*!
@brief Reads the calibration state (CALIB_STAT)
*/
/**************************************************************************/
bool Adafruit_BNO055::getCalibState(adafruit_bno055_calib_stat_t mask)
{
/* See section 3.10 */
uint8_t a;
a = read8(BNO055_CALIB_STAT_ADDR);
return ((a & mask) == mask);
}
/**************************************************************************/
/*!
@brief Reads the calibration data
*/
/**************************************************************************/
void Adafruit_BNO055::getCalibData(byte * pbuffer)
{
adafruit_bno055_opmode_t backupmode = _mode;
setMode(OPERATION_MODE_CONFIG);
delay(25);
readLen(ACCEL_OFFSET_X_LSB_ADDR, pbuffer, 22); //read the whole range of offsets
setMode(backupmode); //return to previous opmode
delay(25);
}
/**************************************************************************/
/*!
@brief Writes previously acquired calibration data
*/
/**************************************************************************/
void Adafruit_BNO055::setCalibData(byte * pbuffer)
{
adafruit_bno055_opmode_t backupmode = _mode;
setMode(OPERATION_MODE_CONFIG);
delay(25);
for(uint8_t i = 0; i < 22; i++)
{
Serial.println(pbuffer[i]);
write8((adafruit_bno055_reg_t)((uint8_t)ACCEL_OFFSET_X_LSB_ADDR+i), pbuffer[i]); //write the whole range of offsets
}
setMode(backupmode); //return to previous opmode
delay(25);
}
/**************************************************************************/
/*!
@brief Gets teh temperature in degrees celsius
*/
/**************************************************************************/
int8_t Adafruit_BNO055::getTemp(void)
{
int8_t temp = (int8_t)(read8(BNO055_TEMP_ADDR));
return temp;
}
/**************************************************************************/
/*!
@brief Gets a vector reading from the specified source
*/
/**************************************************************************/
imu::Vector<3> Adafruit_BNO055::getVector(adafruit_vector_type_t vector_type)
{
imu::Vector<3> xyz;
uint8_t buffer[6];
memset (buffer, 0, 6);
int16_t x, y, z;
x = y = z = 0;
/* Read vector data (6 bytes) */
readLen((adafruit_bno055_reg_t)vector_type, buffer, 6);
x = ((int16_t)buffer[0]) | (((int16_t)buffer[1]) << 8);
y = ((int16_t)buffer[2]) | (((int16_t)buffer[3]) << 8);
z = ((int16_t)buffer[4]) | (((int16_t)buffer[5]) << 8);
/* Convert the value to an appropriate range (section 3.6.4) */
/* and assign the value to the Vector type */
switch(vector_type)
{
case VECTOR_MAGNETOMETER:
/* 1uT = 16 LSB */
xyz[0] = ((double)x)/16.0;
xyz[1] = ((double)y)/16.0;
xyz[2] = ((double)z)/16.0;
break;
case VECTOR_GYROSCOPE:
/* 1rps = 900 LSB */
xyz[0] = ((double)x)/900.0;
xyz[1] = ((double)y)/900.0;
xyz[2] = ((double)z)/900.0;
break;
case VECTOR_EULER:
/* 1 degree = 16 LSB */
xyz[0] = ((double)x)/16.0;
xyz[1] = ((double)y)/16.0;
xyz[2] = ((double)z)/16.0;
break;
case VECTOR_ACCELEROMETER:
case VECTOR_LINEARACCEL:
case VECTOR_GRAVITY:
/* 1m/s^2 = 100 LSB */
xyz[0] = ((double)x)/100.0;
xyz[1] = ((double)y)/100.0;
xyz[2] = ((double)z)/100.0;
break;
}
return xyz;
}
/**************************************************************************/
/*!
@brief Gets a quaternion reading from the specified source
*/
/**************************************************************************/
imu::Quaternion Adafruit_BNO055::getQuat(void)
{
uint8_t buffer[8];
memset (buffer, 0, 8);
int16_t x, y, z, w;
x = y = z = w = 0;
/* Read quat data (8 bytes) */
readLen(BNO055_QUATERNION_DATA_W_LSB_ADDR, buffer, 8);
w = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]);
x = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]);
y = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]);
z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]);
/* Assign to Quaternion */
/* See http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_12~1.pdf
3.6.5.5 Orientation (Quaternion) */
const double scale = (1.0 / (1<<14));
imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z);
return quat;
}
/**************************************************************************/
/*!
@brief Provides the sensor_t data for this sensor
*/
/**************************************************************************/
void Adafruit_BNO055::getSensor(sensor_t *sensor)
{
/* Clear the sensor_t object */
memset(sensor, 0, sizeof(sensor_t));
/* Insert the sensor name in the fixed length char array */
strncpy (sensor->name, "BNO055", sizeof(sensor->name) - 1);
sensor->name[sizeof(sensor->name)- 1] = 0;
sensor->version = 1;
sensor->sensor_id = _sensorID;
sensor->type = SENSOR_TYPE_ORIENTATION;
sensor->min_delay = 0;
sensor->max_value = 0.0F;
sensor->min_value = 0.0F;
sensor->resolution = 0.01F;
}
/**************************************************************************/
/*!
@brief Reads the sensor and returns the data as a sensors_event_t
*/
/**************************************************************************/
bool Adafruit_BNO055::getEvent(sensors_event_t *event)
{
/* Clear the event */
memset(event, 0, sizeof(sensors_event_t));
event->version = sizeof(sensors_event_t);
event->sensor_id = _sensorID;
event->type = SENSOR_TYPE_ORIENTATION;
event->timestamp = millis();
/* Get a Euler angle sample for orientation */
imu::Vector<3> euler = getVector(Adafruit_BNO055::VECTOR_EULER);
event->orientation.x = euler.x();
event->orientation.y = euler.y();
event->orientation.z = euler.z();
return true;
}
/***************************************************************************
PRIVATE FUNCTIONS
***************************************************************************/
/**************************************************************************/
/*!
@brief Writes an 8 bit value over I2C
*/
/**************************************************************************/
bool Adafruit_BNO055::write8(adafruit_bno055_reg_t reg, byte value)
{
Wire.beginTransmission(_address);
#if ARDUINO >= 100
Wire.write((uint8_t)reg);
Wire.write((uint8_t)value);
#else
Wire.send(reg);
Wire.send(value);
#endif
Wire.endTransmission();
/* ToDo: Check for error! */
return true;
}
/**************************************************************************/
/*!
@brief Reads an 8 bit value over I2C
*/
/**************************************************************************/
byte Adafruit_BNO055::read8(adafruit_bno055_reg_t reg )
{
byte value = 0;
Wire.beginTransmission(_address);
#if ARDUINO >= 100
Wire.write((uint8_t)reg);
#else
Wire.send(reg);
#endif
Wire.endTransmission();
Wire.requestFrom(_address, (byte)1);
#if ARDUINO >= 100
value = Wire.read();
#else
value = Wire.receive();
#endif
return value;
}
/**************************************************************************/
/*!
@brief Reads the specified number of bytes over I2C
*/
/**************************************************************************/
bool Adafruit_BNO055::readLen(adafruit_bno055_reg_t reg, byte * buffer, uint8_t len)
{
Wire.beginTransmission(_address);
#if ARDUINO >= 100
Wire.write((uint8_t)reg);
#else
Wire.send(reg);
#endif
Wire.endTransmission();
Wire.requestFrom(_address, (byte)len);
/* Wait until data is available */
while (Wire.available() < len);
for (uint8_t i = 0; i < len; i++)
{
#if ARDUINO >= 100
buffer[i] = Wire.read();
#else
buffer[i] = Wire.receive();
#endif
}
/* ToDo: Check for errors! */
return true;
}
Code: Select all
/*******************************************************************************************************************************************************************************************
* Adafruit BNO055 Sensor Calibration sample
* Refer to http://www.adafruit.com/datasheets/BST_BNO055_DS000_12.pdf
* Section 3.10 on how to calibrate your sensor:
* -Run this sketch
* -To calibrate the Gyroscope, just let the sensor sit for a couple of seconds until GYR Calibration shows 1
* -To calibrate the Magnetometer, move your sensor in random figure 8 patterns until MAG Calibration shows 1
* -If you want to calibrate the acceleration sensor, move the sensor to 6 stable positions slowly, 3 of those should be in the XY/XZ/YZ plane
* -Once you are happy with the readings you get, copy the c_data = .... line from serial.console to the setup() of your sketch and write it to the sensor with "bno.setCalibData(c_data);"
********************************************************************************************************************************************************************************************/
#include <Adafruit_Sensor.h>
#include <utility/imumaths.h>
#include <Wire.h>
#include <Adafruit_BNO055.h>
Adafruit_BNO055 bno = Adafruit_BNO055(55);
void setup()
{
Serial.begin(9600);
Serial.println("Orientation Sensor Calibration"); Serial.println("");
/* Initialise the sensor */
if (!bno.begin(Adafruit_BNO055::OPERATION_MODE_NDOF)) //if you want to calibrate using another mode, set it here. OPERATION_MODE_COMPASS for a precise tilt compensated compass (Section 3.3.2 / 3.3.3)
{
/* There was a problem detecting the BNO055 ... check your connections */
Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
while (1);
}
//You will need the next two lines in your own sketch, _immediatly_ after bno.begin() to use a predefined calibration
//byte c_data[22] = {0, 0, 0, 0, 0, 0, 172, 250, 112, 255, 52, 253, 0, 0, 253, 255, 255, 255, 232, 3, 240, 2}; //replace this line with the serial output of this sketch
//bno.setCalibData(c_data);
delay(1000);
bno.setExtCrystalUse(true);
}
void loop()
{
sensors_event_t event;
bno.getEvent(&event);
//print the euler angles for reference
Serial.print("X: "); //"heading"
Serial.print(event.orientation.x, 4);
Serial.print(" Y: ");
Serial.print(event.orientation.y, 4);
Serial.print(" Z: ");
Serial.print(event.orientation.z, 4);
Serial.println();
//show the calibration states for all 3 sensors + system. Optimally, in NDOF mode you want all these to read "1" before you assume good calibration (Note: Some sensors are off in other modes)
bool bMAG = bno.getCalibState(Adafruit_BNO055::CALIB_STAT_MAG);
Serial.print("MAG Calibration: ");
Serial.println(bMAG);
bool bGYR = bno.getCalibState(Adafruit_BNO055::CALIB_STAT_GYR);
Serial.print("GYR Calibration: ");
Serial.println(bGYR);
bool bACC = bno.getCalibState(Adafruit_BNO055::CALIB_STAT_ACC);
Serial.print("ACC Calibration: ");
Serial.println(bACC);
bool b = bno.getCalibState(Adafruit_BNO055::CALIB_STAT_SYS);
Serial.print("SYS Calibration: ");
Serial.println(b);
byte buffer[22] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
bno.getCalibData(buffer);
//print the actual line to paste into your sketch (see setup() above)
Serial.print("byte c_data[22] = {");
for (int i = 0; i < 22; i++)
{
Serial.print(buffer[i]);
if(i!=21) Serial.print(", ");
}
Serial.println("};");
delay(500);
}
- davegun
- Posts: 89
- Joined: Sun Sep 08, 2013 11:00 pm
Re: BNO055 IMU - Find magnetic north
protonstorm,
Very nice!! Your version has much cleaner than mine :) I'm going to look at it closer, I'm sure I will learn something.
Without calibration, this sensor is just about useless unless you only need to make a bunny jump around the screen. With calibration, it enables the sensor to perform as it should and do amazing things. It would be nice to see your modifications added to the official library!
I have copied the files, and will give it a test when I can. I'm in the middle of a project now, so it my be a bit.
Thank You!!
Dave
PS - I made a short video showing a demo of the compass using the BNO055. This shows how stable it is even when tilted. The NNO055 is under the Ultimate GPS so it isn't visible. Note the red indicator, this is a north pointing arrow. I have calibration values that are loaded at startup, so it gives correct magnetic values when it powers on.
http://youtu.be/815veaVXh2g
Very nice!! Your version has much cleaner than mine :) I'm going to look at it closer, I'm sure I will learn something.
Without calibration, this sensor is just about useless unless you only need to make a bunny jump around the screen. With calibration, it enables the sensor to perform as it should and do amazing things. It would be nice to see your modifications added to the official library!
I have copied the files, and will give it a test when I can. I'm in the middle of a project now, so it my be a bit.
Thank You!!
Dave
PS - I made a short video showing a demo of the compass using the BNO055. This shows how stable it is even when tilted. The NNO055 is under the Ultimate GPS so it isn't visible. Note the red indicator, this is a north pointing arrow. I have calibration values that are loaded at startup, so it gives correct magnetic values when it powers on.
http://youtu.be/815veaVXh2g
Please be positive and constructive with your questions and comments.