Hi,
I'm in the process of teaching myself about IMUs. I've got a Nano BLE 33 Sense with an lsm9ds1 sensor, and I'm working through different measures of orientation. So far I've got this from the accelerometer using trigonometry, and now I'm trying to dead reckon the gyro position with the formula (gyro output in degrees/second)*(time from millis since last reading in seconds.)
I'm expecting to get a fair amount of drift from the gyro but at least the angle ranges should be in the right ballpark. However with both roll and pitch I'm turning the sensor right through 180 degrees and consistently only seeing about 150 degrees of variance.
I don't know if this might be to do with my data types, imprecise implementation of the timer, or something else entirely. I could just multiply the final output such that the 150 I'm seeing shows as the 180 I'm moving through, but as a learning exercise I'd like to fix it properly if I can.
Thanks. Here is my code:
#include <Arduino_LSM9DS1.h>
float dt = 0;
float millisOld = 0;
float thetaM = 0;
float phiM = 0;
float thetaFold=0;
float thetaFnew = 0;
float phiFold = 0;
float phiFnew = 0;
float thetaG = 0;
float phiG = 0;
void setup() {
Serial.begin(9600);
while (!Serial);
Serial.println("Started");
if (!IMU.begin()) {
Serial.println("Failed to initialize IMU!");
while (1);
}
Serial.print("Accelerometer sample rate = ");
Serial.print(IMU.accelerationSampleRate());
Serial.println(" Hz");
Serial.print("Gyroscope sample rate = ");
Serial.print(IMU.gyroscopeSampleRate());
Serial.println(" Hz");
Serial.print("Magnetic field sample rate = ");
Serial.print(IMU.magneticFieldSampleRate());
Serial.println(" uT");
Serial.println("Acceleration in G's");
Serial.println("Gyroscope in degrees/second");
Serial.println("Magnetic Field in uT");
}
void loop() {
float a_x, a_y, a_z, g_x, g_y, g_z, m_x, m_y, m_z;
//get raw values from each sensor
if (IMU.accelerationAvailable() || IMU.gyroscopeAvailable() || IMU.magneticFieldAvailable()) {
IMU.readAcceleration(a_x, a_y, a_z);
IMU.readGyroscope(g_x, g_y, g_z);
IMU.readMagneticField(m_x, m_y, m_z);
//Update clock
dt = (millis() - millisOld)/1000;
millisOld = millis();
//compute accelerometer Theta (inverse tan of x acc over y acc, divided by 57.3 into degrees from radians) and phi (same in other plane)
thetaM = atan2(a_x,a_z)*57.295779513;
phiM = atan2(a_y,a_z)*57.295779513;
//Filter accelerometer theta and phi to remove noise
thetaFnew = 0.9*thetaFold + 0.1*thetaM ;
phiFnew = 0.9*phiFold + 0.1*phiM ;
//compute gyro theta and phi
thetaG = thetaG + (g_y * dt);
phiG = phiG + (g_x * dt);
//Print raw values from each sensor
Serial.print(a_x);
Serial.print(',');
Serial.print(a_y);
Serial.print(',');
Serial.print(a_z);
Serial.print(',');
Serial.print(g_x);
Serial.print(',');
Serial.print(g_y);
Serial.print(',');
Serial.print(g_z);
Serial.print(',');
Serial.print(m_x);
Serial.print(',');
Serial.print(m_y);
Serial.print(',');
Serial.print(m_z);
//Print accelerometer angles raw
Serial.print(',');
Serial.print(thetaM);
Serial.print(',');
Serial.print(phiM);
//Print accelerometer angles filtered
Serial.print(',');
Serial.print(thetaFnew);
Serial.print(',');
Serial.print(phiFnew);
//Print gyro angles raw
Serial.print(',');
Serial.print(thetaG);
Serial.print(',');
Serial.print(phiG);
//Debugging
Serial.print(',');
Serial.println(dt*100000);
//Update filter memory
thetaFold = thetaFnew;
phiFold = phiFnew;
}
}
