Hi, could anyone help me with getting reasonably accurate fused orientation data from the 9DoF LSM9DS1 on the Arduino Nano BLE 33 sense chip?
I would have thought that this is a common requirement of this sensor, but most instructions and library examples I've found only cover extracting the raw data, or else use very rough and drifty approximations for true orientation. Ideally I'd like to be within a degree or 2 on all 3 axes, and stable at rest.
Here is the code I've tried using the Madgwick filter (which I understand very little about.) Perhaps unsurprisingly my output numbers bear no resemblance to what they should. I'm not even sure the units I'm using are correct, or how I would find that out. Can anyone tell me how to fix this, or point me to a working equivalent for this sensor?
Huge thanks for any help.
#include <Arduino_LSM9DS1.h>
#include "Streaming.h" // needed for the Serial output https://github.com/geneReeves/ArduinoStreaming
#include "SensorFusion.h"
SF fusion;
float g_x, g_y, g_z, a_x, a_y, a_z, m_x, m_y, m_z;
float pitch, roll, yaw;
float deltat;
void setup() {
// serial to display data
Serial.begin(115200);
while (!Serial) {}
// start communication with IMU
while (!Serial);
Serial.println("Started");
if (!IMU.begin()) {
Serial.println("Failed to initialize IMU!");
while (1);
}
}
void loop() {
IMU.readAcceleration(a_x, a_y, a_z);
IMU.readGyroscope(g_x, g_y, g_z);
IMU.readMagneticField(m_x, m_y, m_z);
g_x = g_x * DEG_TO_RAD;
g_y = g_y * DEG_TO_RAD;
g_z = g_z * DEG_TO_RAD;
deltat = fusion.deltatUpdate();
//fusion.MahonyUpdate(g_x, g_y, g_z, a_x, a_y, a_z, m_x, m_y, m_z, deltat); //mahony is suggested if there isn't the mag
fusion.MadgwickUpdate(g_x, g_y, g_z, a_x, a_y, a_z, m_x, m_y, m_z, deltat); //else use the magwick
roll = fusion.getRoll();
pitch = fusion.getPitch();
yaw = fusion.getYaw();
Serial.print(pitch);
Serial.print(",");
Serial.print(roll);
Serial.print(",");
Serial.println(yaw);
delay(50);
}