Make more information calculated by the NXP Fusion algorithm available to the user

[ToMe25]

I deleted all the template stuff, because i don't think it applies to suggestions.

The NXP Sensor Fusion algorithm calculates a bunch of interesting values, that don't seem to be accessible in any way. I suggest changing this, by adding getters for them. The ones that seem most interesting to me are:

• Linear Acceleration(aSeMi, aSePl)
• Gravitational Acceleration(gSeGyMi)
• And maybe the Geomagnetic Vector(mSeGyMi)

I suggest adding methods like this: void getLinearAcceleration(float *x, float *y, float *z) const; and I would be willing to PR this, if this that has a chance of getting merged. However I would need to know which of the linear acceleration(g, sensor frame) arrays to use, which i currently do not.

[ladyada]

sure, well documented getters would be accepted if they pass CI :)

[ToMe25]

Wow, that was a fast response. I will try to document them well enough. Starting to work on the PR them :)

[ToMe25]

I have started working on this, and while the Gravity Vector(gSeGyMi) returns valid results, and i have no idea what the Geomagnetic Vector(mSeGyMi) should be(googling it didn't help) so i just asume its right, the linear acceleration fields(aSeMi, aSePl, aGlPl) all return strange things.

I used this program and a LSM9DS1 to test this:

Source(Click to show)

```cpp #include #include #include #include #include Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1(); Adafruit_NXPSensorFusion filter; void setup() { Serial.begin(115200); if (!lsm.begin()) { while (1) { Serial.println( "Failed to initialize the LSM9DS1. Check your wiring!"); delay(1000); } } lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_2G); lsm.setupMag(lsm.LSM9DS1_MAGGAIN_4GAUSS); lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_245DPS); filter.begin(20); } void loop() { struct timeval tv_now; gettimeofday(&tv_now, NULL); uint64_t start_us = ((uint64_t) tv_now.tv_sec * 1000000L + (uint) tv_now.tv_usec); lsm.read(); sensors_event_t a, m, g, t; lsm.getEvent(&a, &m, &g, &t); filter.update(g.gyro.x, g.gyro.y, g.gyro.z, a.acceleration.x, a.acceleration.y, a.acceleration.z, m.magnetic.x, m.magnetic.y, m.magnetic.z); Serial.println("--------------------"); Serial.print("Gyro X: "); Serial.println(g.gyro.x); Serial.print("Gyro Y: "); Serial.println(g.gyro.y); Serial.print("Gyro Z: "); Serial.println(g.gyro.z); Serial.print("Acceleration X: "); Serial.println(a.acceleration.x); Serial.print("Acceleration Y: "); Serial.println(a.acceleration.y); Serial.print("Acceleration Z: "); Serial.println(a.acceleration.z); Serial.print("Magnetic X: "); Serial.println(m.magnetic.x); Serial.print("Magnetic Y: "); Serial.println(m.magnetic.y); Serial.print("Magnetic Z: "); Serial.println(m.magnetic.z); float linear_x, linear_y, linear_z; filter.getLinearAcceleration(&linear_x, &linear_y, &linear_z); Serial.print("Linear Acceleration X: "); Serial.println(linear_x); Serial.print("Linear Acceleration Y: "); Serial.println(linear_y); Serial.print("Linear Acceleration Z: "); Serial.println(linear_z); float gravity_x, gravity_y, gravity_z; filter.getGravityVector(&gravity_x, &gravity_y, &gravity_z); Serial.print("Gravity Vector X: "); Serial.println(gravity_x); Serial.print("Gravity Vector Y: "); Serial.println(gravity_y); Serial.print("Gravity Vector Z: "); Serial.println(gravity_z); float magnetic_x, magnetic_y, magnetic_z; filter.getGeomagneticVector(&magnetic_x, &magnetic_y, &magnetic_z); Serial.print("Geomagnetic Vector X: "); Serial.println(magnetic_x); Serial.print("Geomagnetic Vector Y: "); Serial.println(magnetic_y); Serial.print("Geomagnetic Vector Z: "); Serial.println(magnetic_z); gettimeofday(&tv_now, NULL); uint64_t end_us = ((uint64_t) tv_now.tv_sec * 1000000L + (uint) tv_now.tv_usec); Serial.print("Measuring Time: "); Serial.print(uint(end_us - start_us)); Serial.println("µs"); delayMicroseconds(50000 - min(uint64_t(50000), (end_us - start_us))); } ```

And this is some sample output:

Log(Click to show)

``` -------------------- Gyro X: 0.03 Gyro Y: 0.02 Gyro Z: -0.05 Acceleration X: 5.04 Acceleration Y: 0.29 Acceleration Z: -8.40 Magnetic X: 53.29 Magnetic Y: 12.12 Magnetic Z: 25.21 Linear Acceleration X: -3.12 Linear Acceleration Y: -0.18 Linear Acceleration Z: 5.20 Gravity Vector X: 0.50 Gravity Vector Y: 0.03 Gravity Vector Z: -0.87 Geomagnetic Vector X: 34.87 Geomagnetic Vector Y: 13.57 Geomagnetic Vector Z: 33.16 Measuring Time: 36665µs -------------------- Gyro X: 0.02 Gyro Y: 0.02 Gyro Z: -0.06 Acceleration X: 5.03 Acceleration Y: 0.28 Acceleration Z: -8.36 Magnetic X: 53.27 Magnetic Y: 12.03 Magnetic Z: 24.52 Linear Acceleration X: -3.12 Linear Acceleration Y: -0.17 Linear Acceleration Z: 5.17 Gravity Vector X: 0.50 Gravity Vector Y: 0.03 Gravity Vector Z: -0.86 Geomagnetic Vector X: 34.82 Geomagnetic Vector Y: 13.63 Geomagnetic Vector Z: 33.19 Measuring Time: 36682µs -------------------- Gyro X: 0.01 Gyro Y: 0.02 Gyro Z: -0.05 Acceleration X: 5.03 Acceleration Y: 0.31 Acceleration Z: -8.47 Magnetic X: 53.07 Magnetic Y: 12.06 Magnetic Z: 24.70 Linear Acceleration X: -3.12 Linear Acceleration Y: -0.19 Linear Acceleration Z: 5.23 Gravity Vector X: 0.50 Gravity Vector Y: 0.03 Gravity Vector Z: -0.86 Geomagnetic Vector X: 34.77 Geomagnetic Vector Y: 13.69 Geomagnetic Vector Z: 33.22 Measuring Time: 36665µs -------------------- Gyro X: 0.02 Gyro Y: 0.02 Gyro Z: -0.05 Acceleration X: 5.01 Acceleration Y: 0.30 Acceleration Z: -8.41 Magnetic X: 53.36 Magnetic Y: 12.72 Magnetic Z: 24.35 Linear Acceleration X: -3.11 Linear Acceleration Y: -0.19 Linear Acceleration Z: 5.21 Gravity Vector X: 0.50 Gravity Vector Y: 0.03 Gravity Vector Z: -0.86 Geomagnetic Vector X: 34.78 Geomagnetic Vector Y: 13.75 Geomagnetic Vector Z: 33.19 Measuring Time: 36669µs -------------------- Gyro X: 0.02 Gyro Y: 0.02 Gyro Z: -0.05 Acceleration X: 5.04 Acceleration Y: 0.28 Acceleration Z: -8.51 Magnetic X: 53.70 Magnetic Y: 12.15 Magnetic Z: 24.54 Linear Acceleration X: -3.12 Linear Acceleration Y: -0.18 Linear Acceleration Z: 5.26 Gravity Vector X: 0.50 Gravity Vector Y: 0.03 Gravity Vector Z: -0.86 Geomagnetic Vector X: 34.76 Geomagnetic Vector Y: 13.81 Geomagnetic Vector Z: 33.18 Measuring Time: 36665µs ```

For this test i used aSePl as Linear Acceleration, which should be linear acceleration (g, sensor frame), and the sensor didn't move much.

[ToMe25]

The Linear Acceleration issue was seemingly just me being stupid, and probably one of the examples being wrong. Not sure about the second one, but if its true i will PR that later.

[ToMe25]

BTW the example is not wrong, the thing i didn't notice is that for the roll/pitch/yaw calculation it doesn't matter whether the input acceleration is m/s² or g, while for the linear acceleration it does. And as the example uses a fusion algorithm that doesn't calculate the linear acceleration it doesn't matter.