complementary.ino
#include "CurieImu.h"
int16_t ax, ay, az; // accelerometer values
int16_t gx, gy, gz; // gyrometer values
//#define ACCELEROMETER_SENSITIVITY 8192.0
#define GYROSCOPE_SENSITIVITY 65.536
#define M_PI 3.14159265359
#define dt 0.01 // 10 ms sample rate!
uint32_t _prev = millis();
void ComplementaryFilter(short accData[3], short gyrData[3], float *pitch, float *roll)
{
float pitchAcc, rollAcc;
// Integrate the gyroscope data -> int(angularSpeed) = angle
*pitch += ((float)gyrData[0] / GYROSCOPE_SENSITIVITY) * dt; // Angle around the X-axis
*roll -= ((float)gyrData[1] / GYROSCOPE_SENSITIVITY) * dt; // Angle around the Y-axis
// Compensate for drift with accelerometer data if !bullshit
// Sensitivity = -2 to 2 G at 16Bit -> 2G = 32768 && 0.5G = 8192
int forceMagnitudeApprox = abs(accData[0]) + abs(accData[1]) + abs(accData[2]);
if (forceMagnitudeApprox > 8192 && forceMagnitudeApprox < 32768)
{
// Turning around the X axis results in a vector on the Y-axis
pitchAcc = atan2f((float)accData[1], (float)accData[2]) * 180 / M_PI;
*pitch = *pitch * 0.98 + pitchAcc * 0.02;
// Turning around the Y axis results in a vector on the X-axis
rollAcc = atan2f((float)accData[0], (float)accData[2]) * 180 / M_PI;
*roll = *roll * 0.98 + rollAcc * 0.02;
}
}
void setup() {
Serial.begin(9600); // initialize Serial communication
while (!Serial); // wait for the serial port to open
// initialize device
Serial.println("Initializing IMU device...");
CurieImu.initialize();
CurieImu.setFullScaleAccelRange(BMI160_ACCEL_RANGE_2G);
CurieImu.setFullScaleGyroRange(BMI160_GYRO_RANGE_2000);
CurieImu.setGyroRate(BMI160_GYRO_RATE_3200HZ);
Serial.print("full scale accel \t");
Serial.print(CurieImu.getFullScaleAccelRange());
Serial.print("full scale gyro \t");
Serial.println(CurieImu.getFullScaleGyroRange());
// verify connection
Serial.println("Testing device connections...");
if (CurieImu.testConnection()) {
Serial.println("CurieImu connection successful");
} else {
Serial.println("CurieImu connection failed");
}
// use the code below to calibrate accel/gyro offset values
Serial.println("Internal sensor offsets BEFORE calibration...");
Serial.print(CurieImu.getXAccelOffset());
Serial.print("\t"); // -76
Serial.print(CurieImu.getYAccelOffset());
Serial.print("\t"); // -235
Serial.print(CurieImu.getZAccelOffset());
Serial.print("\t"); // 168
Serial.print(CurieImu.getXGyroOffset());
Serial.print("\t"); // 0
Serial.print(CurieImu.getYGyroOffset());
Serial.print("\t"); // 0
Serial.println(CurieImu.getZGyroOffset());
// To manually configure offset compensation values,
// use the following methods instead of the autoCalibrate...() methods below
// CurieImu.setXGyroOffset(220);
// CurieImu.setYGyroOffset(76);
// CurieImu.setZGyroOffset(-85);
// CurieImu.setXAccelOffset(-76);
// CurieImu.setYAccelOffset(-235);
// CurieImu.setZAccelOffset(168);
Serial.println("About to calibrate. Make sure your board is stable and upright");
delay(500);
// The board must be resting in a horizontal position for
// the following calibration procedure to work correctly!
Serial.print("Starting Gyroscope calibration...");
CurieImu.autoCalibrateGyroOffset();
Serial.println(" Done");
Serial.print("Starting Acceleration calibration...");
CurieImu.autoCalibrateXAccelOffset(0);
CurieImu.autoCalibrateYAccelOffset(0);
CurieImu.autoCalibrateZAccelOffset(1);
Serial.println(" Done");
Serial.println("Internal sensor offsets AFTER calibration...");
Serial.print(CurieImu.getXAccelOffset());
Serial.print("\t"); // -76
Serial.print(CurieImu.getYAccelOffset());
Serial.print("\t"); // -2359
Serial.print(CurieImu.getZAccelOffset());
Serial.print("\t"); // 1688
Serial.print(CurieImu.getXGyroOffset());
Serial.print("\t"); // 0
Serial.print(CurieImu.getYGyroOffset());
Serial.print("\t"); // 0
Serial.println(CurieImu.getZGyroOffset());
Serial.println("Enabling Gyroscope/Acceleration offset compensation");
CurieImu.setGyroOffsetEnabled(true);
CurieImu.setAccelOffsetEnabled(true);
_prev = millis();
}
void loop() {
static float roll;
static float pitch;
if ( millis() - _prev > dt * 1000) {
_prev = millis();
// put your main code here, to run repeatedly:
short accData[3];
short gyrData[3];
CurieImu.getMotion6(&accData[0], &accData[1], &accData[2], &gyrData[0], &gyrData[1], &gyrData[2]);
ComplementaryFilter(accData, gyrData, &pitch, &roll);
Serial.print(pitch);
Serial.print("\t");
Serial.println(roll);
}
}