I bought one of these IMU breakboard boards: https://www.sparkfun.com/products/15335
I’ve got it talking to an Arduion Uno over I2C and reading data using this library: https://github.com/sparkfun/SparkFun_IC … inoLibrary
Unfortunately I cannot access the DMP on the chip to do sensor fusion computations (due to memory limits), so I need to do those computations on the Arduino. I tried applying MadgwickAHRS to the data read from the IMU, but I’m not having good results with orientation (e.g. lots of drift, I rotate it 90 deg, but it only updates about 45 deg in my visualization application). Are there some other techniques I can apply to get an acceptable orientation computation?
Here is my code thus far:
// This is meant to be nothing more than a sketch that dumps data on the serial port
// for the python application to show/visualize.
// Communication protocol: This print strings on the Serial port, at the rate of 115200 baud; it never reads data
// It might print out status messages like "waiting for data"
// Actual orientation data will follow the format:
//
// HPR <heading/yaw deg> <pitch deg> <roll deg>
//
// For example:
//
// HPR 5.5 -10.07773 0.22000023
//
#include <Arduino.h>
#include <Wire.h>
#include <ICM_20948.h>
#include "MadgwickAHRS.h"
//#include "MahonyAHRS.h"
#define SERIAL_BAUD_RATE 115200
#define I2C_RATE 400000
#define IMU_TIMEOUT_MS 500 // When there was a failure in talking to the IMU (or no data ready), how many milliseconds should we wait?
#define IMU_DATA_READING_WAIT_MS 5 // How many milliseconds to wait before attempting the next data reading
#define IMU_FUSION_FREQUENCY (1000.0f / float(IMU_DATA_READING_WAIT_MS)) // The sensor fusion algorithm needs to know how often the IMU is being read from
#define SERIAL_PRECISION 12 // How many digits of the float to print to Serial
#define SERIAL_NUM_UPDATES_PER_SECOND 20 // How many times per second to send an update
// TODO research this better
#define AD0_VAL 1 // The value of the last bit of the I2C address.
// On the SparkFun 9DoF IMU breakout the default is 1, and when
// the ADR jumper is closed the value becomes 0
// Using I2C to connect to the imu
ICM_20948_I2C imu;
// For orientation calculation
Madgwick filter;
//Mahony filter;
float heading, pitch, roll; // The orientation
// For sending orientation data to the computer
#define SERIAL_UPDATE_DELTA_MS (1000 / SERIAL_NUM_UPDATES_PER_SECOND) // How many milliseoncds to wait until to update the orientation on the serial connection
unsigned long prev_serial_update_ms = 0; // Previous time (in milliseconds) and update was sent
void setup() {
// Setup communications
Serial.begin(SERIAL_BAUD_RATE); // To computer
Wire.begin(); // To IMU
Wire.setClock(I2C_RATE);
// imu.enableDebugging();
// Try to enable the IMU
bool connected = false;
while (!connected) {
imu.begin(Wire, AD0_VAL);
// Print status to serial
Serial.print("Init connection to ICM 20948: ");
Serial.println(imu.statusString());
// If it didn't work, try again shortly
if (imu.status != ICM_20948_Stat_Ok) {
Serial.println("Try again in 500 ms");
delay(IMU_TIMEOUT_MS);
} else
connected = true;
}
Serial.println("Connected to ICM 20948");
// Setup filter
filter.begin(IMU_FUSION_FREQUENCY);
}
void loop() {
unsigned long current_millis = millis();
// If enough time has passed, send an update of the orientation over the serial connection
if (current_millis >= (prev_serial_update_ms + SERIAL_UPDATE_DELTA_MS)) {
// Mark the time we send the data over
prev_serial_update_ms = current_millis;
// Send Orientation data to the PC
Serial.print("HPR "); // HPR = Heading, Pitch, Roll
Serial.print(heading, SERIAL_PRECISION);
Serial.print(" ");
Serial.print(pitch, SERIAL_PRECISION);
Serial.print(" ");
Serial.print(roll, SERIAL_PRECISION);
Serial.println(""); // End the line reading
}
if (imu.dataReady()) {
// Some data is ready
imu.getAGMT(); // Retrieves data
// Update the sensor fusion state
filter.update(
imu.gyrX(), imu.gyrY(), imu.gyrZ(), // These units might be in degrees and not radiasn
imu.accX(), imu.accY(), imu.accZ(),
imu.magX(), imu.magY(), imu.magZ()
);
// Read data
heading = filter.getYaw();
pitch = filter.getPitch();
roll = filter.getRoll();
// Wait a little before the next reading
delay(IMU_DATA_READING_WAIT_MS);
} else {
// Nothing, pause for a moment
// Serial.println("Waiting for data...");
delay(IMU_TIMEOUT_MS);
}
}
Here is the visitation app that runs on my laptop. You can see there is some constant drifting involved. At the end I’m not touching the board no longer
https://i.imgur.com/sBwHDco.gif
I’m not quite sure if the axis of the breakout board and the MadgwickAHRS code are different, I had to swap the X & Y axis when updating the filter. If I didn’t swap them, when trying to rotate about the X axis (pitch), the on screen model would rotate about the Y axis (roll), and vice-versa
