Sunday, October 16, 2016

Featherboard M0 and BNO055

Featherboard M0 and BNO055

BMI055 is the 9 axis MEMS inertial (3 axis Accelerometer + 3 axis Gyroscope + 3 axis Magnetometer) that is used for motion detection, gesture recognition, pedometer, orientation tracking, and so on. It has analog-to--digital converter inside and supports digital interface (SPI or I2C) to obtain the inertial data of gravity and/or acceleration as G and angular velocity as DPS (degree per second). In addition, there is a DSP that runs sensor fusion and you can obtain the calculated results as either Quaternion or Euler Angle (Pitch, Roll, and Yaw).
The driver code is available from here.
The breakout board is available from here.

Code

Since I am just using Arduino IDE for this test using Adafruit feather M0 board, I downloaded the driver code from the above link and added extracted files under the libraries folder of Arduino IDE.
I would recommend to try one of the example code that Adafruit BNO055 library provides which is "restore_offsets".
This is very nice sample code to calibrate the sensor offsets before starting the coding deeply. This code actually give you the steps to calibrate the sensor, store the data to flash memory, and load the values to sensor to compensate the raw sensor outputs.
The calibration procedure is not so complicated but it took a little bit of time especially for accelerometer calibration. Gyro is easy and you just keep the device still. Rotating the device as you figure 8 will calibrate the magnetometer. Each sensor has the calibration level, 1, 2, and 3, and you will see all 3 when it is fully calibrated. You can refer to the setup function for this process as listed below.
    else
    {
        Serial.println("Please Calibrate Sensor: ");
        while (!bno.isFullyCalibrated())
        {
            bno.getEvent(&event);
            Serial.print("X: ");
            Serial.print(event.orientation.x, 4);
            Serial.print("\tY: ");
            Serial.print(event.orientation.y, 4);
            Serial.print("\tZ: ");
            Serial.print(event.orientation.z, 4);
            /* Optional: Display calibration status */
            displayCalStatus();
            /* New line for the next sample */
            Serial.println("");
            /* Wait the specified delay before requesting new data */
            delay(BNO055_SAMPLERATE_DELAY_MS);
        }
    }
Once the calibration is completed, this program stores the data to memory. Next time when you run the program, it will load the stored values from the memory and write the values to the offset registers of the sensor. You can refer to the beginning of the setup function in "restore_offsets.ino" as listed below.
    /*
    *  Look for the sensor's unique ID at the beginning of EEPROM.
    *  This isn't foolproof, but it's better than nothing.
    */
    bno.getSensor(&sensor);
    if (bnoID != sensor.sensor_id)
    {
        Serial.println("\nNo Calibration Data for this sensor exists in EEPROM");
        delay(500);
    }
    else
    {
        Serial.println("\nFound Calibration for this sensor in EEPROM.");
        eeAddress += sizeof(long);
        EEPROM.get(eeAddress, calibrationData);
        displaySensorOffsets(calibrationData);
        Serial.println("\n\nRestoring Calibration data to the BNO055...");
        bno.setSensorOffsets(calibrationData);
        Serial.println("\n\nCalibration data loaded into BNO055");
        foundCalib = true;
    }

Next is the loop function and it is very simple.
void loop() {
    /* Get a new sensor event */
    sensors_event_t event;
    bno.getEvent(&event);
    /* Display the floating point data */
    Serial.print("X: ");
    Serial.print(event.orientation.x, 4);
    Serial.print("\tY: ");
    Serial.print(event.orientation.y, 4);
    Serial.print("\tZ: ");
    Serial.print(event.orientation.z, 4);
    /* Optional: Display calibration status */
    displayCalStatus();
    /* Optional: Display sensor status (debug only) */
    //displaySensorStatus();
    /* New line for the next sample */
    Serial.println("");
    /* Wait the specified delay before requesting new data */
    delay(BNO055_SAMPLERATE_DELAY_MS);
}
It is basically calling the "getEvent" function in the Adafruit library and this gives you the Euler angle which is pitch, roll, and yaw as you can refer to the function below.
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;
}
"displayCalStatus" is just getting the calibration level (1-3) and printing out to the terminal.
This is useful to see if the calibration level is always good when you are acquiring sensor data.
You can change the definition of "BNO055_SAMPLERATE_DELAY_MS" which decides the sampling rate of the event loop. The default number is 100 which is 10Hz.

If you want to get the quaternion instead, you can call the "getQuat" function as listed below.
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;
}
You can add something like the following code in your loop function.
  // Quaternion data
  imu::Quaternion quat = bno.getQuat();
  Serial.print(quat.w(), 4);
  Serial.print(" ");
  Serial.print(quat.y(), 4);
  Serial.print(" ");
  Serial.print(quat.x(), 4);
  Serial.print(" ");
  Serial.println(quat.z(), 4);
Some people might want to convert from Quaternion to Euler and you can refer to here for the quick reference.


                 

1 comment: