BNO055 Reuse of Calibration Profile in Arduino 9-Axis Shield

I am using a BNO055 in a large 2-axis gimbal. The gimbal configuration is Elevation over Azimuth and the gimbal rates are about 1 degree per second. The problem I have is that the BNO055 never gets to a system calibration state better than zero unless I unmount the controller and move it around. This is not very practical. I would like to save the configuration like described in section 3.11.4 "Reuse of Calibration Profile" of the data sheet.

Has anyone done this? I was looking to modify the library to include new functions to read and save the information.

Thanks

Kurt

I modified the Bosch library to implement reading and writing the calibration state. For those that are interested the modifications are.

Near the end of NAxisMotion.h add:

/****************************** ADDED CLASS FUNCTIONS **************************************/
/*******************************************************************************************
*Description: This function is used to read the magnetic offsets the BNO055
*Input Parameters: struct bno055_mag_offset_t    *mag_offset;
*Return Parameter: @retval 0 -> Success
*                  @retval 1 -> Error
*******************************************************************************************/
int8_t  NAxisMotion::read_mag_offset(struct bno055_mag_offset_t  *mag_offset);

/*******************************************************************************************
*Description: This function is used to read the gyro offsets the BNO055
*Input Parameters: struct bno055_gyro_offset_t   *gyro_offset;       A place to store Offset Values
*Return Parameter: @retval 0 -> Success
*                  @retval 1 -> Error
*******************************************************************************************/
int8_t  NAxisMotion::read_gyro_offset(struct bno055_gyro_offset_t   *gyro_offset);

/*******************************************************************************************
*Description: This function is used to read the gyro offsets the BNO055
*Input Parameters: struct bno055_accel_offset_t  *accel_offset;    A place for Offset and Radius Values
       A place to store Offset Values
*Return Parameter: @retval 0 -> Success
*                  @retval 1 -> Error
*******************************************************************************************/
int8_t  NAxisMotion::read_accel_offset(struct bno055_accel_offset_t  *accel_offset);


/*******************************************************************************************
*Description: This function is used to write the magnetic offsets the BNO055
*Input Parameters: struct bno055_mag_offset_t    *mag_offset;
*Return Parameter: @retval 0 -> Success
*                  @retval 1 -> Error
*******************************************************************************************/
int8_t  NAxisMotion::write_mag_offset(struct bno055_mag_offset_t  *mag_offset);

/*******************************************************************************************
*Description: This function is used to write the gyro offsets the BNO055
*Input Parameters: struct bno055_gyro_offset_t   *gyro_offset;       A place to store Offset Values
*Return Parameter: @retval 0 -> Success
*                  @retval 1 -> Error
*******************************************************************************************/
int8_t  NAxisMotion::write_gyro_offset(struct bno055_gyro_offset_t   *gyro_offset);

/*******************************************************************************************
*Description: This function is used to write the gyro offsets the BNO055
*Input Parameters: struct bno055_accel_offset_t  *accel_offset;    A place for Offset and Radius Values
       A place to store Offset Values
*Return Parameter: @retval 0 -> Success
*                  @retval 1 -> Error
*******************************************************************************************/
int8_t  NAxisMotion::write_accel_offset(struct bno055_accel_offset_t  *accel_offset);

};

To the end of NAxisMotion.cpp I added:

/*********************************** ADDED CLASS FUNCTIONS *********************************/
/*******************************************************************************************
*Description: This function is used to read the magnetic offsets the BNO055
*Input Parameters: struct bno055_mag_offset_t    *mag_offset;
*Return Parameter: @retval 0 -> Success
*                  @retval 1 -> Error
*******************************************************************************************/
int8_t  NAxisMotion::read_mag_offset(struct bno055_mag_offset_t  *mag_offset)
{
return(bno055_read_mag_offset(mag_offset));
}
/*******************************************************************************************
*Description: This function is used to read the gyro offsets the BNO055
*Input Parameters: struct bno055_gyro_offset_t   *gyro_offset;       A place to store Offset Values
*Return Parameter: @retval 0 -> Success
*                  @retval 1 -> Error
*******************************************************************************************/
int8_t  NAxisMotion::read_gyro_offset(struct bno055_gyro_offset_t   *gyro_offset)
{
return(bno055_read_gyro_offset(gyro_offset));
}
/*******************************************************************************************
*Description: This function is used to read the gyro offsets the BNO055
*Input Parameters: struct bno055_accel_offset_t  *accel_offset;    A place for Offset and Radius Values
       A place to store Offset Values
*Return Parameter: @retval 0 -> Success
*                  @retval 1 -> Error
*******************************************************************************************/
int8_t  NAxisMotion::read_accel_offset(struct bno055_accel_offset_t  *accel_offset)
{
return(bno055_read_accel_offset(accel_offset));
}

/*******************************************************************************************
*Description: This function is used to write the magnetic offsets the BNO055
*Input Parameters: struct bno055_mag_offset_t    *mag_offset;
*Return Parameter: @retval 0 -> Success
*                  @retval 1 -> Error
*******************************************************************************************/
int8_t  NAxisMotion::write_mag_offset(struct bno055_mag_offset_t  *mag_offset)
{
return(bno055_write_mag_offset(mag_offset));
}
/*******************************************************************************************
*Description: This function is used to write the gyro offsets the BNO055
*Input Parameters: struct bno055_gyro_offset_t   *gyro_offset;       A place to store Offset Values
*Return Parameter: @retval 0 -> Success
*                  @retval 1 -> Error
*******************************************************************************************/
int8_t  NAxisMotion::write_gyro_offset(struct bno055_gyro_offset_t   *gyro_offset)
{
return(bno055_write_gyro_offset(gyro_offset));
}
/*******************************************************************************************
*Description: This function is used to write the gyro offsets the BNO055
*Input Parameters: struct bno055_accel_offset_t  *accel_offset;    A place for Offset and Radius Values
       A place to store Offset Values
*Return Parameter: @retval 0 -> Success
*                  @retval 1 -> Error
*******************************************************************************************/
int8_t  NAxisMotion::write_accel_offset(struct bno055_accel_offset_t  *accel_offset)
{
return(bno055_write_accel_offset(accel_offset));
}

Functions that read and writes the Cal Registers using my new class functions is:

struct bno055_mag_offset_t    mag_offset ={-53,209,-523,836};                                              // Offset and Radius Values
/* s_16 x; < Mag offset x data
   s_16 y; < Mag offset y data
   s_16 z; < Mag offset z data
   s_16 r; < Mag radius x data */
struct bno055_accel_offset_t  accel_offset ={-24,2,7,1000};                                                 // Offset and Radius Values
/* s_16 x; < Accel offset x data
   s_16 y; < Accel offset y data
   s_16 z; < Accel offset z data
   s_16 r; < Accel radius x data */
struct bno055_gyro_offset_t   gyro_offset ={-1,-4,-1};                                                      // Offset Values
/* s_16 x; < Gyro offset x data
   s_16 y; < Gyro offset y data
   s_16 z; < Gyro offset z data  */
/* @return results of bus communication function
   @retval 0 . Success
   @retval 1 . Error */
   // 4/3/17 ReadBNO055Calibration: readmag_offset 0 { -44,204,-515,811} readgyro_offset 0 { -1,-4,-2} readaccel_offset 0 { -21,1,11,1000} 

/**********************************************************************************************************************************************************/
void ReadBNO055Calibration() //This code is executed once
/**********************************************************************************************************************************************************/
{
  BNO055_RETURN_FUNCTION_TYPE readmag_offset   = 1;
  BNO055_RETURN_FUNCTION_TYPE readaccel_offset = 1;
  BNO055_RETURN_FUNCTION_TYPE readgyro_offset  = 1;

  mag_offset.x = 0;
  mag_offset.y = 0;
  mag_offset.z = 0;
  mag_offset.r = 0;

  accel_offset.x = 0;
  accel_offset.y = 0;
  accel_offset.z = 0;
  accel_offset.r = 0;
 
  gyro_offset.x = 0;
  gyro_offset.y = 0;
  gyro_offset.z = 0;
  
  //if(mySensor.readSystemCalibStatus() < 3) return;                                                 // Wait till claibrated

 
  mySensor.setOperationMode(OPERATION_MODE_CONFIG);                                                  // Set CONFIG Mode


  readmag_offset   = mySensor.read_mag_offset(&mag_offset);                                             // Read Offset and Radius

  Console.print(F(" mag ")); Console.print(readmag_offset); 
  Console.print(" {");
  Console.print(mag_offset.x); Console.print(","); 
  Console.print(mag_offset.y); Console.print(","); 
  Console.print(mag_offset.z); Console.print(","); 
  Console.print(mag_offset.r);
  Console.print("} ");
  Console.flush();
  
  readgyro_offset  = mySensor.read_gyro_offset(&gyro_offset);                                         // Read Offset

  Console.print(F("gyro ")); Console.print(readgyro_offset); 
  Console.print(" {");
  Console.print(gyro_offset.x); Console.print(",");
  Console.print(gyro_offset.y); Console.print(",");
  Console.print(gyro_offset.z);
  Console.print("} ");

  readaccel_offset = mySensor.read_accel_offset(&accel_offset);                                        // Read Offset and Radius

  Console.print(F("accel ")); Console.print(readaccel_offset);
  Console.print(" {");
  Console.print(accel_offset.x); Console.print(",");
  Console.print(accel_offset.y); Console.print(",");
  Console.print(accel_offset.z); Console.print(",");
  Console.print(accel_offset.r);
  Console.println("} ");

  mySensor.setOperationMode(BNO055_OPERATING_MODE);                                                // Can be configured to other operation modes as desired mode is 9 Degrees of Freedom Sensor Fusion with fast magcal



}

/**********************************************************************************************************************************************************/
void WriteBNO055Calibration() //This code is executed once
/**********************************************************************************************************************************************************/
{


// Sensor Initialization 4/4/17 ReadBNO055Calibration: readmag_offset 0 { -83,152,152,868} readgyro_offset 0 { -1,-4,-1} readaccel_offset 0 { -25,2,9,1000} 
// WiFi= 35  KBs _2017.03.23.06.57-MEGA_BNO-055  @ 4/5/2017 15:00:13  = 41861  El=ENA Az=ENA S3 G3 A1 M3 EL-Off Az-Off Az 242.95 El 54.50 C 321.15 R -0.00 N 1471 W 5.29 mag 0 {-53,209,-523,836} gyro 0 {-1,-4,-1} accel 0 {-24,2,7,1000} 
// WiFi= 52  KBs _2017.03.23.06.57-MEGA_BNO-055  @ 4/5/2017 15:44:24  = 41982  El=ENA Az=ENA S0 G3 A1 M3 EL-Off Az-Off Az 49.71 El 137.94 C 49.71 R 131.25 N 137 W 5.62 mag 0 {-40,268,-452,832} gyro 0 {-1,-3,0} accel 0 {-23,2,6,1000} 

struct bno055_mag_offset_t    saved_mag_offset ={-53,209,-523,836};                               // Offset and Radius Values
struct bno055_accel_offset_t  saved_accel_offset ={-24,2,7,1000};                                 // Offset and Radius Values
struct bno055_gyro_offset_t   saved_gyro_offset ={-1,-4,-1};                                      // Offset Values


  mySensor.setOperationMode(OPERATION_MODE_CONFIG);                                               // Set CONFIG Mode

  mySensor.write_mag_offset(&saved_mag_offset);
  mySensor.write_accel_offset(&saved_accel_offset);
  mySensor.write_gyro_offset(&saved_gyro_offset);

  mySensor.setOperationMode(BNO055_OPERATING_MODE);                                              // Can be configured to other operation modes as desired mode is 9 Degrees of Freedom Sensor Fusion with fast magcal

}

In this code BNO055_OPERATING_MODE is a #define that is the mode that you are running the BNO055 in since the code needs to restore the mode after it sets the mode to OPERATION_MODE_CONFIG. In my application this is:
#define BNO055_OPERATING_MODE OPERATION_MODE_NDOF_FMC_OFF

After I restore the Calibration values I get a System .readSystemCalibStatus of 3 but this tends to decay back to zero so I added then following code to fix this. It is kind of clunky but it seems to work.

    switch (mySensor.readSystemCalibStatus())
    {
      case 0:
        RebootBNO055();                                                                          //  Accel Gyro Setup
        Console.print(F(" Cal was 0 RebootBNO055 now ="));
        Console.print(mySensor.readSystemCalibStatus());
        Console.print(F(" compass ="));
        Console.println(Compass());
        ValidCompass = false;
        break;
      case 1:
        if(IJustWroteCal) break;                                                                // Only write once
        WriteBNO055Calibration();
        IJustWroteCal = true;
        ValidCompass = false;
        Console.print(F(" Cal was 1 WriteBNO055Calibration now ="));
        Console.print(mySensor.readSystemCalibStatus());
        Console.print(F(" compass ="));
        Console.println(Compass());
        break;
      case 2:
        ValidCompass = true;
        Console.print(F(" readSystemCalibStatus = "));
        Console.print(mySensor.readSystemCalibStatus());
        Console.print(F(" compass ="));
        Console.println(Compass());
        break;
      case 3:
        ValidCompass = true;
        IJustWroteCal = false;
        break;
    }

This rewrites the values only once if the state falls to 1 and reboots if it falls to zero. It does this routinely and sometimes writing the values doesn’t fix it so I reboot it by cycling the BNO055 reset line on the 9-Axis shield. Also if you write the values over and over the device never changes the cal state shown in mySensor.readSystemCalibStatus.

Example operation follows:
readSystemCalibStatus = 2 compass =242.21
readSystemCalibStatus = 2 compass =242.21
readSystemCalibStatus = 2 compass =242.21
readSystemCalibStatus = 2 compass =242.21
readSystemCalibStatus = 2 compass =242.21
readSystemCalibStatus = 2 compass =242.21
readSystemCalibStatus = 2 compass =242.21
Cal was 1 WriteBNO055Calibration now =1 compass =242.21
WiFi= 57 KBs _2017.04.05.16.39-BNO-055 @ 4/6/2017 18:39:00 = 46685 SunDown El=ENA Az=ENA S1 G3 A3 M0 EL-Off East Az 227.90 El 43.81 C 242.15 R 0.22 N 1571 W 5.26 mag 0 {-52,208,-520,0} gyro 0 {7,-4,-8} accel 0 {-24,2,7,1000}
Cal was 0 RebootBNO055 now =0 compass =242.15
WiFi= 50 KBs _2017.04.05.16.39-BNO-055 @ 4/6/2017 18:39:20 = 46686 SunDown El=ENA Az=ENA AzTO ElTO S3 G3 A1 M3 Up Az-Off Az 222.65 El 44.06 C 222.65 R 0.04 N 2496 W 4.98 mag 0 {-52,208,-520,836} gyro 0 {-1,-3,0} accel 0 {-23,2,6,1000}

The string “S1 G3 A3 M0” shows the four calibration states. The code runs once per second.

You have probably noticed that the BNO055 calibration parameters are different for every calibration run.

I found it useful to do a dozen or so calibration runs, after each run copying the 11 resulting offsets or constants into a spreadsheet and then average them.

Upon application startup, I simply copy those to the BNO055. The code is very simple (using a very simple I2C library), and follows.

//  write 11 word calibration table to BNO055

void I2C_WriteCal(unsigned char busAddr, unsigned char deviceRegister, int* vector) {

 unsigned char i,l;
    I2C_Start(busAddr); // send device address
    I2C_Write(deviceRegister); // set register pointer
 for (i=0; i<11; i++) {
     l = (*vector)&0x00ff; // low byte register data
 I2C_Write(l); //write it
 l = (*vector)>>8;
 I2C_Write(l);
 vector++;
 }
    I2C_Stop();
}

/* snip of main() */


//predetermined BNO055 calibration constants

 int precal[]={-6,3,17,-2,395,112,0,1,0,1000,832}; //averages of 9 in situ calibration runs

...

// check for BNO055 presence

 if (0xa0 != (result=I2C_ReadRegister(BNO055_A0,BNO055_WHO_AM_I))) { //check internal device ID
 clear();
 print(" No BNO");
 while(1); //hang here
 }

// reset BNO055, in case of soft reboot
 I2C_WriteRegister(BNO055_A0, BNO055_SYS_TRIGGER, 0x20); //BNO055 system reset
 delay(1000); //required reset delay

// store earlier derived calibration parameters
 I2C_WriteCal(BNO055_A0, BNO055_CAL_DATA, (int *)precal);
 I2C_WriteRegister(BNO055_A0, BNO055_SYS_TRIGGER, 0x80); //use external 32 kHz crystal.

// set to measurement mode

 I2C_WriteRegister(BNO055_A0,BNO055_OPER_MODE,BNO055_OPER_MODE_NDOF);
 delay(10); //minimum 7 ms delay

Hello,

I am using the BNO055 in a project with an Arduino Mega. I am using the modified library from ispybadguys. Thank you for sharing your knowledge!!! Well, I have a sketch for storing the values for a correct calibration in a SD card: with sensor in NDOF mode, I perform the movements for calibrating the sensor and, when all four calibration status values are 3, I change the mode to CONFIG mode, reading the values and storing them in the SD card. It seems to write reasonable values.

With other sketch, I read the previously stored values and load them from the SD card to the NAxisMotion in the setup(). After that, I put the sensor in NDOF mode and I see that accelerometer is calibrated and gyroscope too, but status of magnetometer calibration is 0, and system calibration too.

#include <NAxisMotion.h>
#include <Wire.h>
#include <SD.h>
#include <SPI.h>

#define N_CALIBRACION "Calib.ini"

NAxisMotion mySensor;
unsigned long lastStreamTime = 0;
const int streamPeriod = 20;

File myFile;


void setup()
{    
  Serial.begin(9600);
  I2C.begin();                    
  mySensor.initSensor();
  if (WriteBNO055Calibration() == true) Serial.println("Exit!");
  else Serial.println("Fail!");
  mySensor.setOperationMode(OPERATION_MODE_NDOF);
  mySensor.setUpdateMode(AUTO);
}

void loop()
{
  unsigned int estadoAccel;
  unsigned int estadoGyro;
  unsigned int estadoMag;
  unsigned int estado;
  
  if ((millis() - lastStreamTime) >= streamPeriod)
  {
    lastStreamTime = millis();
    mySensor.updateEuler();
    Serial.print(" H: ");
    Serial.print(mySensor.readEulerHeading());
    Serial.print(" grados");

    mySensor.updateCalibStatus();

    Serial.print("Acelerometro: ");
    estadoAccel = mySensor.readAccelCalibStatus();
    Serial.println(estadoAccel);
	
    Serial.print("Magnetometro: ");
    estadoMag = mySensor.readMagCalibStatus();
    Serial.println(estadoMag);
	
    Serial.print("Giroscopio: ");
    estadoGyro = mySensor.readGyroCalibStatus();
    Serial.println(estadoGyro);
	
    estado = mySensor.readSystemCalibStatus();
    Serial.print("Dispositivo: ");
    Serial.println(estado);
  }
}


bool WriteBNO055Calibration() {

  struct bno055_mag_offset_t saved_mag_offset;
  struct bno055_accel_offset_t saved_accel_offset;
  struct bno055_gyro_offset_t saved_gyro_offset;

  if (!SD.begin()) return false;
  else {
    myFile = SD.open(N_CALIBRACION, FILE_READ);
    if (myFile) {
      saved_accel_offset.x = LeeOffsetSD();
      saved_accel_offset.y = LeeOffsetSD();
      saved_accel_offset.z = LeeOffsetSD();
      saved_accel_offset.r = LeeOffsetSD();
      mySensor.write_accel_offset(&saved_accel_offset);
      saved_gyro_offset.x = LeeOffsetSD();
      saved_gyro_offset.y = LeeOffsetSD();
      saved_gyro_offset.z = LeeOffsetSD();
      mySensor.write_gyro_offset(&saved_gyro_offset);
      saved_mag_offset.x = LeeOffsetSD();
      saved_mag_offset.y = LeeOffsetSD();
      saved_mag_offset.z = LeeOffsetSD();
      saved_mag_offset.r = LeeOffsetSD();
      mySensor.write_mag_offset(&saved_mag_offset);
      return true;
    }
  else return false;
  }
}  


int LeeOffsetSD() {
  
 const int MAX_LEN = 6; 
 
 int i = 0;
 char digito = 0;
 char valor[MAX_LEN + 1];
 
  while (myFile.available()) {
    digito = myFile.read();
    if ((digito == 10) || (digito == ';')) break;
    else if ((digito == ' ') || (digito == 13));
    else if (i == MAX_LEN) break;
    else valor[i++] = digito;
  }
  
  valor[i++] = 0;
  return atoi(valor);
}

Of course, Monitor Serial shows “Exit!” after sensor calibration.

Does anynoe have any idea of what it is happening?