The sensor is integrated with an accelerometer (ADXL345), a gyroscope (ITG3200), a magnetometer (VCM5883L) and a temperature and pressure sensor (BMP280).
I was successfully able to acquire the linear acceleration values after having installed the library when using the sensor with an UNO, however I was getting errors when using with an Arduino Nano 33 IoT. After having modified the sensor libraries, so as to be apt with the SAMD 21 processor that the Nano 33 IoT uses, I am just getting static values.
Here is the code I am running to get the sensor readings:
#include <FreeSixIMU.h>
#include <FIMU_ADXL345.h>
#include <FIMU_ITG3200.h>
#include <Wire.h>
float angles[3]; // yaw pitch roll
// Set the FreeSixIMU object
FreeSixIMU sixDOF = FreeSixIMU();
void setup() {
Serial.begin(9600);
Wire.begin();
delay(5);
sixDOF.init(); //begin the IMU
delay(5);
}
void loop() {
sixDOF.getEuler(angles);
Serial.print(angles[0]);
Serial.print(" | ");
Serial.print(angles[1]);
Serial.print(" | ");
Serial.println(angles[2]);
delay(100);
}
And here are the modified library files:
- FreeSixIMU.cpp:
/*
FreeSixIMU.cpp - A libre and easy-to-use orientation sensing library for Arduino
Copyright (C) 2011 Fabio Varesano <fabio at varesano dot net>
This program is free software: you can redistribute it and/or modify
it under the terms of the version 3 GNU General Public License as
published by the Free Software Foundation.
*/
#include <inttypes.h>
#include <Wire.h>
#include "FreeSixIMU.h"
FreeSixIMU::FreeSixIMU() {
acc = ADXL345();
gyro = ITG3200();
// Initialize quaternion
q0 = 1.0f;
q1 = 0.0f;
q2 = 0.0f;
q3 = 0.0f;
exInt = 0.0;
eyInt = 0.0;
ezInt = 0.0;
twoKp = twoKpDef;
twoKi = twoKiDef;
integralFBx = 0.0f;
integralFBy = 0.0f;
integralFBz = 0.0f;
lastUpdate = 0;
now = 0;
}
void FreeSixIMU::init() {
init(FIMU_ACC_ADDR, FIMU_ITG3200_DEF_ADDR, false);
}
void FreeSixIMU::init(bool fastmode) {
init(FIMU_ACC_ADDR, FIMU_ITG3200_DEF_ADDR, fastmode);
}
void FreeSixIMU::init(int acc_addr, int gyro_addr, bool fastmode) {
delay(5);
// Disable internal pull-ups for SAMD21
pinMode(PIN_WIRE_SDA, INPUT);
pinMode(PIN_WIRE_SCL, INPUT);
// If fastmode, set I2C to 400kHz
if (fastmode) {
Wire.begin();
Wire.setClock(400000); // Set I2C clock speed to 400kHz
}
// Initialize the accelerometer (ADXL345)
acc.init(acc_addr);
// Initialize the gyroscope (ITG3200)
gyro.init(gyro_addr);
delay(1000);
// Calibrate the gyroscope
gyro.zeroCalibrate(128, 5);
}
void FreeSixIMU::getRawValues(int * raw_values) {
acc.readAccel(&raw_values[0], &raw_values[1], &raw_values[2]);
gyro.readGyroRaw(&raw_values[3], &raw_values[4], &raw_values[5]);
}
void FreeSixIMU::getValues(float * values) {
int accval[3];
acc.readAccel(&accval[0], &accval[1], &accval[2]);
values[0] = ((float) accval[0]);
values[1] = ((float) accval[1]);
values[2] = ((float) accval[2]);
gyro.readGyro(&values[3]);
}
// Quaternion implementation of the 'DCM filter' by Mayhony et al.
void FreeSixIMU::AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float recipNorm;
float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
float qa, qb, qc;
// Auxiliary variables to avoid repeated arithmetic
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
// Compute feedback only if accelerometer measurement is valid
if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) {
float halfvx, halfvy, halfvz;
// Normalize accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Estimated direction of gravity
halfvx = q1q3 - q0q2;
halfvy = q0q1 + q2q3;
halfvz = q0q0 - 0.5f + q3q3;
// Error is sum of cross product between estimated direction and measured direction of field vectors
halfex += (ay * halfvz - az * halfvy);
halfey += (az * halfvx - ax * halfvz);
halfez += (ax * halfvy - ay * halfvx);
}
// Apply feedback only when valid data has been gathered
if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
if(twoKi > 0.0f) {
integralFBx += twoKi * halfex * (1.0f / sampleFreq);
integralFBy += twoKi * halfey * (1.0f / sampleFreq);
integralFBz += twoKi * halfez * (1.0f / sampleFreq);
gx += integralFBx;
gy += integralFBy;
gz += integralFBz;
} else {
integralFBx = 0.0f;
integralFBy = 0.0f;
integralFBz = 0.0f;
}
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
}
gx *= (0.5f * (1.0f / sampleFreq));
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy - qc * gx);
// Normalize quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
void FreeSixIMU::getQ(float * q) {
float val[9];
getValues(val);
now = micros();
sampleFreq = 1.0 / ((now - lastUpdate) / 1000000.0);
lastUpdate = now;
AHRSupdate(val[3] * M_PI / 180, val[4] * M_PI / 180, val[5] * M_PI / 180, val[0], val[1], val[2], 0, 0, 0);
q[0] = q0;
q[1] = q1;
q[2] = q2;
q[3] = q3;
}
void FreeSixIMU::getEuler(float * angles) {
float q[4];
getQ(q);
angles[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0] * q[0] + 2 * q[1] * q[1] - 1) * 180 / M_PI;
angles[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]) * 180 / M_PI;
angles[2] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1) * 180 / M_PI;
}
void FreeSixIMU::getAngles(float * angles) {
float a[3];
getEuler(a);
angles[0] = a[0];
angles[1] = a[1];
angles[2] = a[2];
}
// Fast inverse square root algorithm
float FreeSixIMU::invSqrt(float number) {
volatile long i;
volatile float x2, y;
const float threehalfs = 1.5F;
x2 = number * 0.5F;
y = number;
i = * ( long * ) &y;
i = 0x5f3759df - ( i >> 1 );
y = * ( float * ) &i;
y = y * ( threehalfs - ( x2 * y * y ) );
return y;
}
Rest all library files are the same as in the git repo
All I am getting is static values. Where am I possibly going wrong?