Go Down

Topic: FIFO Now Working with the CurieIMU and Visualizer (Read 87 times) previous topic - next topic

Merlin513

Mar 15, 2017, 02:32 am Last Edit: Mar 15, 2017, 03:25 pm by Merlin513
For a long time I wanted to try to implement the FIFO capabilities of the Curie IMU but for various reasons had it on the backburner until now.  I found an implementation of FIFO from the Boshtec API for the 160 which hanyazou implemented for the BMI160 library used with the Curie IMU.  I contacted him on this GITHUB page with some questions and he graciously gave me an example of using the BMI160 FIFO.  You can see the discussion here: Without his assistance I would spent longer than I would care to mention on this.  All the credit goes to hanyazou.  Strongly suggest that you read the BMI160 spec if you want to get into this in detail.

You will have to edit the BMI160.h file and move the statements that currently in the
"private " section up into the "public" section of the .h file.

I will post the code in the next two messages in this thread.  


Merlin513

#1
Mar 15, 2017, 02:33 am Last Edit: Mar 15, 2017, 02:35 am by Merlin513
Example code for the BMI160 FIFO:

Code: [Select]

#include "CurieIMU.h"
#include "BMI160.h"

  int fifo_cnt;
  uint8_t data[1024];
  int16_t ax, ay, az;         // accelerometer values
  int16_t gx, gy, gz;         // gyrometer values

void bmi160_intr(void)
{
  fifo_cnt = CurieIMU.getFIFOCount();
  fifo_cnt += 4;
  CurieIMU.getFIFOBytes(data, fifo_cnt);
  decode();
  //show_fifo_frames();
}

void decode(){
  gx = (((int16_t)data[13])  << 8) | data[12];
  ax = (((int16_t)data[19])  << 8) | data[18];

  gy = (((int16_t)data[15])  << 8) | data[14];
  ay = (((int16_t)data[21])  << 8) | data[20];
  
  gz = (((int16_t)data[17])  << 8) | data[16];
  az = (((int16_t)data[23])  << 8) | data[22];
  // display tab-separated accel/gyro x/y/z values
  Serial.print("a/g:\t");
  Serial.print(ax);  Serial.print("\t"); Serial.print(ay); Serial.print("\t");
  Serial.print(az);  Serial.print("\t");
  Serial.print(gx);  Serial.print("\t"); Serial.print(gy); Serial.print("\t");
  Serial.println(gz);
}

void show_fifo_frames(){
  //Serial.println(fifo_cnt);
  for(int i = 0; i < fifo_cnt; i++){
    Serial.print(data[i]); Serial.print(", ");
  }
  Serial.println("----------------------");
}


void setup() {
// initialize device
  Serial.begin(9600); // initialize Serial communication
  while (!Serial);    // wait for the serial port to open

  // initialize device
  Serial.println("Initializing IMU device...");
  CurieIMU.begin();

  // verify connection
  Serial.println("Testing device connections...");
  if (CurieIMU.begin()) {
    Serial.println("CurieIMU connection successful");
  } else {
    Serial.println("CurieIMU connection failed");
  }

  //CurieIMU.setGyroRate(25);
  CurieIMU.setAccelerometerRange(2);
  CurieIMU.setAccelRate(BMI160_ACCEL_RATE_1600HZ);
  CurieIMU.setGyroRange(250);
  CurieIMU.setGyroRate(BMI160_GYRO_RATE_1600HZ);

    Serial.println("About to calibrate. Make sure your board is stable and upright");
    delay(5000);

    // The board must be resting in a horizontal position for
    // the following calibration procedure to work correctly!
    Serial.print("Starting Gyroscope calibration and enabling offset compensation...");
    CurieIMU.autoCalibrateGyroOffset();
    Serial.println(" Done");

    Serial.print("Starting Acceleration calibration and enabling offset compensation...");
    CurieIMU.autoCalibrateAccelerometerOffset(X_AXIS, 0);
    CurieIMU.autoCalibrateAccelerometerOffset(Y_AXIS, 0);
    CurieIMU.autoCalibrateAccelerometerOffset(Z_AXIS, 1);
    Serial.println(" Done");

    Serial.println("Internal sensor offsets AFTER calibration...");
    Serial.print(CurieIMU.getAccelerometerOffset(X_AXIS));
    Serial.print("\t"); // -76
    Serial.print(CurieIMU.getAccelerometerOffset(Y_AXIS));
    Serial.print("\t"); // -2359
    Serial.print(CurieIMU.getAccelerometerOffset(Z_AXIS));
    Serial.print("\t"); // 1688
    Serial.print(CurieIMU.getGyroOffset(X_AXIS));
    Serial.print("\t"); // 0
    Serial.print(CurieIMU.getGyroOffset(Y_AXIS));
    Serial.print("\t"); // 0
    Serial.println(CurieIMU.getGyroOffset(Z_AXIS));
  
  CurieIMU.setFIFOHeaderModeEnabled(false);   // Use Headerless Mode
  CurieIMU.setAccelFIFOEnabled(true);         // Enable the accelerometer FIFO
  CurieIMU.setGyroFIFOEnabled(true);          // Enable the gyro FIFO

  #define BMI160_FIFO_TIME_EN_BIT 1           // Returns a sensor time after last valid frame
  CurieIMU.reg_write_bits(BMI160_RA_FIFO_CONFIG_1, 0x1, BMI160_FIFO_TIME_EN_BIT, 1);

  CurieIMU.attachInterrupt(bmi160_intr);      // Attach interrupt for when FIFO data greater than
                                              // watermark level
  //CurieIMU.reg_write(BMI160_RA_FIFO_CONFIG_0, 0b00000101); // set FIFO water mark level
  CurieIMU.reg_write(BMI160_RA_FIFO_CONFIG_0, 0b00001010); // set FIFO water mark level
  #define BMI160_FWM_INT_BIT 6                // Sets watermark interrupt
  CurieIMU.reg_write_bits(BMI160_RA_INT_EN_1, 0x1, BMI160_FWM_INT_BIT, 1);
  CurieIMU.resetFIFO();

  // Some debug info to verify register writes
  //Serial.println(CurieIMU.reg_read(BMI160_RA_FIFO_CONFIG_0), BIN);
  //Serial.println(CurieIMU.reg_read(BMI160_RA_FIFO_CONFIG_1), BIN);

}

void loop() {

  delay(1);
}

Merlin513

Visualizer modification for use with FIFO buffer:

Code: [Select]

#include <CurieIMU.h>
#include "BMI160.h"
#include <MadgwickAHRS.h>

Madgwick filter;
unsigned long microsPerReading, microsPrevious;
float accelScale, gyroScale;

  int fifo_cnt;
  uint8_t data[1024];
  int16_t aix, aiy, aiz;
  int16_t gix, giy, giz;
  float ax, ay, az;
  float gx, gy, gz;
  float roll, pitch, heading;
  unsigned long microsNow;

void bmi160_intr(void)
{
  fifo_cnt = CurieIMU.getFIFOCount();
  fifo_cnt += 4;
  CurieIMU.getFIFOBytes(data, fifo_cnt);
  decode();
}

void decode(){
  gix = (((int16_t)data[13])  << 8) | data[12];
  aix = (((int16_t)data[19])  << 8) | data[18];

  giy = (((int16_t)data[15])  << 8) | data[14];
  aiy = (((int16_t)data[21])  << 8) | data[20];
  
  giz = (((int16_t)data[17])  << 8) | data[16];
  aiz = (((int16_t)data[23])  << 8) | data[22];

/*
  Serial.print("a/g:\t");
  Serial.print(aix);  Serial.print("\t"); Serial.print(aiy); Serial.print("\t");
  Serial.print(aiz);  Serial.print("\t");
  Serial.print(gix);  Serial.print("\t"); Serial.print(giy); Serial.print("\t");
  Serial.println(giz);
*/

  // check if it's time to read data and update the filter
  microsNow = micros();
  if (microsNow - microsPrevious >= microsPerReading) {

    // convert from raw data to gravity and degrees/second units
    ax = convertRawAcceleration(aix);
    ay = convertRawAcceleration(aiy);
    az = convertRawAcceleration(aiz);
    gx = convertRawGyro(gix);
    gy = convertRawGyro(giy);
    gz = convertRawGyro(giz);

/*
  Serial.print("a/g:\t");
  Serial.print(ax);  Serial.print("\t"); Serial.print(ay); Serial.print("\t");
  Serial.print(az);  Serial.print("\t");
  Serial.print(gx);  Serial.print("\t"); Serial.print(gy); Serial.print("\t");
  Serial.println(gz);
*/
    // update the filter, which computes orientation
    filter.updateIMU(gx, gy, gz, ax, ay, az);

    // print the heading, pitch and roll
    roll = filter.getRoll();
    pitch = filter.getPitch();
    heading = filter.getYaw();
    Serial.print("Orientation: ");
    Serial.print(heading);
    Serial.print(" ");
    Serial.print(pitch);
    Serial.print(" ");
    Serial.println(roll);

    // increment previous time, so we keep proper pace
    microsPrevious = microsPrevious + microsPerReading;
  }
  
}

void setup() {
  Serial.begin(9600);

  // start the IMU and filter
  filter.begin(25);

  // initialize device
  Serial.println("Initializing IMU device...");
  CurieIMU.begin();

  // verify connection
  Serial.println("Testing device connections...");
  if (CurieIMU.begin()) {
    Serial.println("CurieIMU connection successful");
  } else {
    Serial.println("CurieIMU connection failed");
  }

  //CurieIMU.setGyroRate(25);
  CurieIMU.setAccelerometerRange(2);
  CurieIMU.setAccelRate(BMI160_ACCEL_RATE_1600HZ);
  CurieIMU.setGyroRange(250);
  CurieIMU.setGyroRate(BMI160_GYRO_RATE_1600HZ);

    Serial.println("About to calibrate. Make sure your board is stable and upright");
    delay(5000);

    // The board must be resting in a horizontal position for
    // the following calibration procedure to work correctly!
    Serial.print("Starting Gyroscope calibration and enabling offset compensation...");
    CurieIMU.autoCalibrateGyroOffset();
    Serial.println(" Done");

    Serial.print("Starting Acceleration calibration and enabling offset compensation...");
    CurieIMU.autoCalibrateAccelerometerOffset(X_AXIS, 0);
    CurieIMU.autoCalibrateAccelerometerOffset(Y_AXIS, 0);
    CurieIMU.autoCalibrateAccelerometerOffset(Z_AXIS, 1);
    Serial.println(" Done");

    Serial.println("Internal sensor offsets AFTER calibration...");
    Serial.print(CurieIMU.getAccelerometerOffset(X_AXIS));
    Serial.print("\t"); // -76
    Serial.print(CurieIMU.getAccelerometerOffset(Y_AXIS));
    Serial.print("\t"); // -2359
    Serial.print(CurieIMU.getAccelerometerOffset(Z_AXIS));
    Serial.print("\t"); // 1688
    Serial.print(CurieIMU.getGyroOffset(X_AXIS));
    Serial.print("\t"); // 0
    Serial.print(CurieIMU.getGyroOffset(Y_AXIS));
    Serial.print("\t"); // 0
    Serial.println(CurieIMU.getGyroOffset(Z_AXIS));
  
  CurieIMU.setFIFOHeaderModeEnabled(false);   // Use Headerless Mode
  CurieIMU.setAccelFIFOEnabled(true);         // Enable the accelerometer FIFO
  CurieIMU.setGyroFIFOEnabled(true);          // Enable the gyro FIFO

  #define BMI160_FIFO_TIME_EN_BIT 1           // Returns a sensor time after last valid frame
  CurieIMU.reg_write_bits(BMI160_RA_FIFO_CONFIG_1, 0x1, BMI160_FIFO_TIME_EN_BIT, 1);

  CurieIMU.attachInterrupt(bmi160_intr);      // Attach interrupt for when FIFO data greater than
                                              // watermark level
  //CurieIMU.reg_write(BMI160_RA_FIFO_CONFIG_0, 0b00000101); // set FIFO water mark level
  CurieIMU.reg_write(BMI160_RA_FIFO_CONFIG_0, 0b00001010); // set FIFO water mark level
  #define BMI160_FWM_INT_BIT 6                // Sets watermark interrupt
  CurieIMU.reg_write_bits(BMI160_RA_INT_EN_1, 0x1, BMI160_FWM_INT_BIT, 1);
  CurieIMU.resetFIFO();


  // initialize variables to pace updates to correct rate
  microsPerReading = 1000000 / 25;
  microsPrevious = micros();
}

void loop() {

}

float convertRawAcceleration(int aRaw) {
  // since we are using 2G range
  // -2g maps to a raw value of -32768
  // +2g maps to a raw value of 32767
  
  float a = (aRaw * 2.0) / 32768.0;
  return a;
}

float convertRawGyro(int gRaw) {
  // since we are using 250 degrees/seconds range
  // -250 maps to a raw value of -32768
  // +250 maps to a raw value of 32767
  
  float g = (gRaw * 250.0) / 32768.0;
  return g;
}

Go Up
 


Please enter a valid email to subscribe

Confirm your email address

We need to confirm your email address.
To complete the subscription, please click the link in the email we just sent you.

Thank you for subscribing!

Arduino
via Egeo 16
Torino, 10131
Italy