Unexpected logic in NXPSensorFusion::update()

Description

Describe your problem. Line 224 in SensorFusion.cpp file shows the following: if (fabsf(mx) >= 20.0f && fabsf(mx) >= 20.0f && fabsf(mx) >= 20.0f) {

Three checks for fabsf(mx) >= 20.0f seems redundant. Looks like a copy&paste error. Recommend the second mx be changed to my, and the third to mz.

This hasn't caused any noticeable problems in behavior with the code, but my testing hasn't been all that rigorous to date. I just noticed this while reviewing the code.

Hardware & Software

Board Teensy 3.6 Shields / modules used PropShield Arduino IDE version 1.8.9 on MacBook Teensyduino version (if using Teensy) 1.48 Version info & package name (from Tools > Boards > Board Manager) Operating system & version OS X 10.11.6 Any other software or hardware?

Arduino Sketch

// Change the code below by your sketch (please try to give the smallest code which demonstrates the problem)
#include <Arduino.h>

// libraries: give links/details so anyone can compile your code for the same result

void setup() {
}

void loop() {
}

Errors or Incorrect Output

If you see any errors or incorrect output, please show it here. Please use copy & paste to give an exact copy of the message. Details matter, so please show (not merely describe) the actual message or error exactly as it appears.

Hi, I've stumbled upon the same line of code today if (fabsf(mx) >= 20.0f && fabsf(mx) >= 20.0f && fabsf(mx) >= 20.0f) I also believe, that it is some kind of copy and paste error.

However, this line has a huge effect. Almost certainly ValidMagCal will be zero and the first orientation lock is not performed. That does not matter, if you expect yaw to be zero initially. But the Kalman filter can give you absolute heading, i.e. the compass orientation where heading = 0 means you head towards magnetic north.

I changed my code to

// *********************************************************************************
  // initial orientation lock to accelerometer and magnetometer eCompass
  // orientation
  // *********************************************************************************
  //if (fabsf(mx) >= 20.0f && fabsf(mx) >= 20.0f && fabsf(mx) >= 20.0f) {
  //  ValidMagCal = 1;
  //} else {
  //  ValidMagCal = 0;
  //}

  ValidMagCal = 1;

  // do a once-only orientation lock after the first valid magnetic calibration
  if (ValidMagCal && !FirstOrientationLock) {
    // get the 6DOF orientation matrix and initial inclination angle
    feCompassNED(RPl, &DeltaPl, Mag, Accel);

    // get the orientation quaternion from the orientation matrix
    fQuaternionFromRotationMatrix(RPl, &qPl);

    // set the orientation lock flag so this initial alignment is only performed
    // once
    FirstOrientationLock = 1;
    printf("Performed initial orientation lock\n");
  }

Now I obtain absolute heading, instead of relative yaw with respect to the initial orientation. I still experience some drift, but I believe that this is a matter of magnetometer calibration and timing (I am using a Raspberry Pi 4).

Description

Describe your problem. Line 224 in SensorFusion.cpp file shows the following: if (fabsf(mx) >= 20.0f && fabsf(mx) >= 20.0f && fabsf(mx) >= 20.0f) {

Three checks for fabsf(mx) >= 20.0f seems redundant. Looks like a copy&paste error. Recommend the second mx be changed to my, and the third to mz.

This hasn't caused any noticeable problems in behavior with the code, but my testing hasn't been all that rigorous to date. I just noticed this while reviewing the code.

Hardware & Software

Board Teensy 3.6 Shields / modules used PropShield Arduino IDE version 1.8.9 on MacBook Teensyduino version (if using Teensy) 1.48 Version info & package name (from Tools > Boards > Board Manager) Operating system & version OS X 10.11.6 Any other software or hardware?

Arduino Sketch
// Change the code below by your sketch (please try to give the smallest code which demonstrates the problem)
#include <Arduino.h>

// libraries: give links/details so anyone can compile your code for the same result

void setup() {
}

void loop() {
}
Errors or Incorrect Output

If you see any errors or incorrect output, please show it here. Please use copy & paste to give an exact copy of the message. Details matter, so please show (not merely describe) the actual message or error exactly as it appears.

Note that the above recommendation second mx be changed to my, and the third to mz is not a proper fix, because with a calibrated magnetometer mx, my, and mz are between -50 and 50 the area where the abs() of all three is greater than 20 is just a random portion of the sphere with radius 50.

See https://de.mathworks.com/help/nav/ug/magnetometer-calibration.html https://www.pjrc.com/store/prop_shield.html

PaulStoffregen / NXPMotionSense