Prop shield Heading Drift

Status
Not open for further replies.
After installing and testing the prop shield I tried:
A: Calibration.ino and than Orientation.ino and found that the Heading drift is terrible.

Trying some other example's: MahonyIMU.ino gives error: 'class Mahony' has no member named 'begin'
So this example is out of date. The Arduino Mahoney library does not have class begin anymore.

After reading the forum about heading drift .

https://forum.pjrc.com/threads/45173-Prop-Shield-large-drift

https://forum.pjrc.com/threads/33902-Prop-Shield-NXPSensorFusion-observations

https://forum.pjrc.com/threads/53202-Propshield-Orientation-Rotation-Backwards

B: I tried Kris Winer example and found that the Heading is not drifting, but has a deviation of +/- 10 degrees and the altitude has +110 meters offset.

C: Than I tried https://github.com/mjs513/PRJC-PropShield-Practice-Sketches psIMU_serial.ino as advised by mjs513 in this forum.
But there was a library missing "iCompass.h" .

Does anybody know or have a good example to start with or what am I doing wrong ?

Thank's Tom Eilers
 
As to your questions I can try to answer:

1. The version of the MadgwickAHRS you need is the Teensy version that you can download from GitHub: https://github.com/PaulStoffregen/MadgwickAHRS
2. Issue with altitude is probably because you need to adjust the sea level pressure for your current location. I usually grab it from: https://aviationweather.gov/metar. You just need the closet airport identifier but make sure you select "decoded". The ADDS station table can help you get your airport code.
3. As for psIMU. Here is the missing library:
View attachment iCompass.zip
 
Thank you in the first place.

I unzipped the ICompass file and edited the include part see: code

Now I have an error: C:\Users\pope\AppData\Local\Temp\arduino_build_205428\sketch\iCompass.cpp:1:22: fatal error: iCompass.h: No such file or directory

I uploaded the a picture of my directory and Arduino screen, there you can see that all includes are loaded, but still this error ? Why, this is new for me.


Arduino tabs.jpg

I also did run the Kris Winer example with ADDS station table you adviced me.
I changed the line:
const int station_elevation_m = -1; // Accurate for the roof on my house; convert from feet to meters
to -1 meter (Netherland below see level).
Not changed the pressure is good according https://aviationweather.gov/metar. EHAM for Amsterdam Schiphol
pressure is 103601.50 Pa, altitude is -185.75 m, temperature is 28.63 C
 

Attachments

  • Directory.png
    Directory.png
    48.4 KB · Views: 97
Last edited:
A couple more things.
1. Pressure probably should be input as 1036.02.
2. I just uploaded a updated version that I verified on a Teensy 4. What teensy are you using by the way you didn't mention or I missed it. What IDE version and TD version are you using, may help.

https://github.com/mjs513/PSImu

As for directory structure. PSImu and iCompass directories should be put in the Arduino libraries folder and the sketches run the examples list.

As for directory struture
 
I have a Teensy 3.3 Arduino 1.8.10 Teensyduino 1.48 Windows 10 ver 1909

OK I placed the psIMU, Icompass, calibrattion in the Arduino libraries directory.

Now it compiles but with a million warnings.

psIMU_serial: In function 'float invSqrt(float)':
psIMU_serial:370: warning: dereferencing type-punned pointer will break strict-aliasing rules
uint32_t i = 0x5F1F1412 - (*(uint32_t*)&x >> 1);

^

psIMU_serial:371: warning: dereferencing type-punned pointer will break strict-aliasing rules
float tmp = *(float*)&i;

^

MadgwickQuantFileter: In function 'void MadgwickQuaternionUpdate(float, float, float, float, float, float, float, float, float)':
MadgwickQuantFileter:15: warning: variable '_2q0q2' set but not used
float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;

^

MadgwickQuantFileter:15: warning: variable '_2q2q3' set but not used
float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;

^

MahoneyQuantFilter: In function 'void MahonyQuaternionUpdate(float, float, float, float, float, float, float, float, float)':
MahoneyQuantFilter:48: warning: 'halfex' may be used uninitialized in this function
halfex += (my * halfwz - mz * halfwy);

^

MahoneyQuantFilter:49: warning: 'halfey' may be used uninitialized in this function
halfey += (mz * halfwx - mx * halfwz);

^

MahoneyQuantFilter:50: warning: 'halfez' may be used uninitialized in this function
halfez += (mx * halfwy - my * halfwx);

^

C:\Users\pope\Documents\Arduino\libraries\psIMU\psIMU..cpp: In member function 'void psIMU::FXOS8700CQMagOffset()':

C:\Users\pope\Documents\Arduino\libraries\psIMU\psIMU..cpp:365:9: warning: variable 'dest1' set but not used [-Wunused-but-set-variable]

float dest1[3] = {0, 0, 0}, dest2[3] = {0, 0, 0};

^

C:\Users\pope\Documents\Arduino\libraries\psIMU\psIMU..cpp:365:31: warning: variable 'dest2' set but not used [-Wunused-but-set-variable]

float dest1[3] = {0, 0, 0}, dest2[3] = {0, 0, 0};

^

C:\Users\pope\Documents\Arduino\libraries\psIMU\psIMU..cpp: In member function 'void psIMU::initFIFOMPL3115A2()':

C:\Users\pope\Documents\Arduino\libraries\psIMU\psIMU..cpp:963:11: warning: variable 'temp' set but not used [-Wunused-but-set-variable]

uint8_t temp;

^

C:\Users\pope\Documents\Arduino\libraries\psIMU\psIMU..cpp: In member function 'void psIMU::initRealTimeMPL3115A2()':

C:\Users\pope\Documents\Arduino\libraries\psIMU\psIMU..cpp:1000:11: warning: variable 'temp' set but not used [-Wunused-but-set-variable]

uint8_t temp;

^

C:\Users\pope\Documents\Arduino\libraries\ICompass\iCompass.cpp:5:0: warning: "M_PI" redefined

#define M_PI 3.14159265359

^

In file included from C:\Program Files (x86)\Arduino\hardware\teensy\avr\cores\teensy3/WProgram.h:36:0,

from C:\Users\pope\AppData\Local\Temp\arduino_build_987866/pch/Arduino.h:6,

from C:\Users\pope\Documents\Arduino\libraries\ICompass/iCompass.h:4,

from C:\Users\pope\Documents\Arduino\libraries\ICompass\iCompass.cpp:1:

c:\program files (x86)\arduino\hardware\tools\arm\arm-none-eabi\include\math.h:640:0: note: this is the location of the previous definition

#define M_PI 3.14159265358979323846

^

C:\Users\pope\Documents\Arduino\libraries\ICompass\iCompass.cpp: In constructor 'iCompass::iCompass()':

C:\Users\pope\Documents\Arduino\libraries\ICompass\iCompass.cpp:10:84: warning: unused variable 'test' [-Wunused-variable]

iCompass::iCompass(void) : myRA(1) { declinationAngle = 0; maxSamples = 500; float test = 1.0f; samples = 0; myRA.clear(); }

^

C:\Users\pope\Documents\Arduino\libraries\ICompass\iCompass.cpp: In constructor 'iCompass::iCompass(float)':

C:\Users\pope\Documents\Arduino\libraries\ICompass\iCompass.cpp:11:97: warning: unused variable 'test' [-Wunused-variable]

iCompass::iCompass(float dAngle) : myRA(1) { declinationAngle = dAngle; maxSamples = 500; float test = 1.0f; samples = 0; myRA.clear(); }

^

C:\Users\pope\Documents\Arduino\libraries\ICompass\iCompass.cpp: In constructor 'iCompass::iCompass(float, unsigned int)':

C:\Users\pope\Documents\Arduino\libraries\ICompass\iCompass.cpp:12:127: warning: unused variable 'test' [-Wunused-variable]

iCompass::iCompass(float dAngle, unsigned int windSize) : myRA(windSize) { declinationAngle = dAngle; maxSamples = 500; float test = 1.0f; samples = 0; myRA.clear(); }

^

C:\Users\pope\Documents\Arduino\libraries\ICompass\iCompass.cpp: In constructor 'iCompass::iCompass(float, unsigned int, unsigned int)':

C:\Users\pope\Documents\Arduino\libraries\ICompass\iCompass.cpp:13:147: warning: unused variable 'test' [-Wunused-variable]

iCompass::iCompass(float dAngle, unsigned int windSize, unsigned int maxS) : myRA(windSize) { declinationAngle = dAngle; maxSamples = maxS; float test = 1.0f; samples = 0; myRA.clear(); }

^
But OK I will test it, and see what happens.

Thank you very much.

My wife is waiting me now, so testing will have to wait.
 
Next part: my teensy is 3.2. not 3.3

I tested the code psIMU_serial and the calibration cal_gui.exe

After the calibration reloaded the psIMU_serial and entered the y command, the orientation was three time Nan (not a number) , here it stops for me.
Here are the Calibration results:

Calibration.png

Calibration.h


/**
* psIMU calibration header. Automatically generated by psIMU_GUI.
* Do not edit manually unless you know what you are doing.
*/


#define CALIBRATION_H

accel_bias[0] = -266;
accel_bias[1] = -491;
accel_bias[2] = 223;
accel_scale[0] = 2955.995118;
accel_scale[1] = 3601.994610;
accel_scale[2] = 3860.518617;

mag_bias[0] = -9;
mag_bias[1] = 46;
mag_bias[2] = 477;
mag_scale[0] = 307.527700;
mag_scale[0] = 395.592978;
mag_scale[0] = 528.218481;


The calibration results a here
 
Hi
Yeah a couple of things. Looks like I have a minor bug when the Gui prints out the calibration file. For the mag_scale:
Code:
mag_scale[0] = 307.527700;
mag_scale[0] = 395.592978;
mag_scale[0] = 528.218481;
should read
Code:
mag_scale[0] = 307.527700;
mag_scale[1] = 395.592978;
mag_scale[2] = 528.218481;
the indices didn't increment.

Also just looking at that partial image for calibration it just doesn't look quite right. When collecting data you should see nice spherical data especially if you look at the 3d tab. Here is a video of how someone used that Gui: https://www.youtube.com/watch?v=sf_8jcDyO0Q. Notice that they rotated about all axes fairly slowly to collect the data.

Also here is a link to what that data should look like: http://www.varesano.net/blog/fabio/freeimu-gui-now-making-nice-3d-spheres

Just for reference note this line in the example serial sketch:
Code:
  //Uncomment next line to use a calibration file.
  myIMU.setFileCal();
. If uncommented it will bypass the calibration file and use the calibration method that Kris Winer uses in his sketches for the propshield.
 
Yes, thank you very very much, this did the trick.

The best numbers i have seen with the prop shield.

Now why does PJRC publish after 3 years a very bad example Orientation.ino and a very nice Calibaration.ino and GUI ?


Better a difficult calibration than a bad result.

And the bug is not your's I believe, but varesano.net.
 

Attachments

  • Calibration.png
    Calibration.png
    28.2 KB · Views: 69
Well understand that the orientation.ino sketch is meant to be used with the associated processing sketch. The calibration GUI with the propshield is only set up to do Magnetometer never got updated to do Accelerometer so that is kind of a know issue. However, with that said it seems that folks have gotten some stable results using just the magnetomer.

What I did was take a mishmash of Kris Winer's and PJRC libraries to add a few more options to config the sensors and try and do a little bit better calibration setup. If you do a search on the forum you will see a lot of questions all related heading or yaw drift which is a problem with most of these sensors it seems.

As for the bug being with Varesano Gui, nope that was me. I modified it for use with other sensors and converted to a Windows exe from python scripts. :)

Now, as for your calibration it actually could be better - next time I have a IMU hooked up I will run the GUI and show some good screen shots. In the meantime have fun.
 
Ok.
I will do a better calibration, but I was in a hurry to see some results.
That will not give me Nan and not drifting and no random yaw or heading deviation of +/- 10 degrees.
And that is what your code and answers did.

Only one question, do you how often the calibration need to be done ?

Again thank you very much.
Now I can make my own mistakes, by changing the code to something I can use for my sensor.
a: enter my inclination.
b: make the pressure transducer work.
c: etc.
 
Well believe it or not if you look close at the calls inputting a declination angle and getting pressure data is already in there.

1. Declination:
in the sketch you will see:
Code:
maghead = iCompass(0, 1, 0);
breaking it down:
Code:
iCompass::iCompass(float dAngle, unsigned int windSize, unsigned int maxS) : myRA(windSize) { [COLOR="#FF0000"]declinationAngle = dAngle[/COLOR]; maxSamples = maxS; float test = 1.0f; samples = 0; myRA.clear(); }
So your local declination would be the first parameter that you pass into iCompass.

2. using the command 'p' you can put in your local sl pressure as p 101717 and hit return, code snippet
Code:
    else if(cmd=='p'){
      cmd1 = '0';
      //set sea level pressure
      long sea_press = Serial.parseInt();        
      myIMU.setSeaPress(sea_press/100.0);
      //Serial.println(sea_press);

and if you have the MPL3115 on board you can call pressure as:
Code:
   #ifdef MPL3115
      myIMU.getBaro();
      val_array[13] = myIMU.temperature;
      val_array[14] = myIMU.altimeter_setting_pressure_mb;
      val_array[17] = myIMU.altitude;		//in meters
Look close and probably most of what you want is already in there. There are processing sketches that I posted in the GitHub repository that use the serial code.
 
Very nice that everything is in it.
I found that I have to enter P =0 to have my local pressure, what is 1025.0 mb according https://aviationweather.gov/metar.
P=0 is nice. because I live at sea level=0.
But not what the code expects.

else if(cmd=='p'){
cmd1 = '0';
//set sea level pressure
long sea_press = Serial.parseInt();
myIMU.setSeaPress(sea_press/100.0);
Serial.println(sea_press);

At your Github account I found that you tested a lot of IMU's mjs513/FreeIMU-Updates/libraries.
Did you find the best one or is there none ?
 
Sorry for the delay in answering this but there are a few that I like. Think my favorite was the MPU6050/HMC5883 combo, next was the MPU9250 and the next was the Propshield.
 
Just for your information.
I did some logging on the Oriantation.ino example. See graph.

It think, that PJRC didn't compensate for the loop speed, because the heading is integrating forever.
All the data is stable but not the heading.
Or maybe the My, because it has positive spikes, but these are random.

I fixed the IMU with Teensy on a wooden blok in the middle of my room.
Loop speed was 10 msec.
The IMU heated up for one andhalf hour.
Oriantation Data.jpg
 
It think, that PJRC didn't compensate for the loop speed, because the heading is integrating forever.
All the data is stable but not the heading.
Don't think its a PJRC problem. The Madgwick algorithm is a standard algorithm used for AHRS. Not sure why you are seeing spikes. You can also try running the Mahoney algorithm and see if you get the same results.
 
I re-calibrated the Propshield with the PJRC code calibration.ino and Propshield MotionCal.exe

Now both MadgwickIMO.ino and MahonyIMU.ino and Oriantation.ino are drifting downwards.

Does this mean that calibration is the cause of the drift or just for getting the right angle values ?

Your psIMU_serial.ino is OK and stable.

So what is your trick.
 
I did some new experiments:
Cleaned the EEprom.
Fixed the Ax,Ay,Az,Mx,My,Mz values and compensated the Gz,Gy,Gz to 0 the Yaw or Heading is still drifting.
Ran the Oriantation.ino and the MadgwickAHRS.ino examples.
I looks like the examples are Gyro noise integrators.
What do you think ?
Madgwick A and M fixed values.jpg
Oriantation A and M fixed values.jpg
 
New question:
I was always thinking that those complex math filters (nxpfusion, madgwick, mahony) used the magnetometer to compensate for Gyro noise and drift.
Am I wrong ?
 
Hi tom
The magnetometer is suppose to compensate for the drift not noise. In the past I have put a butterworth type filter on the gyro which seemed to help.
 
@WMXZ
Thanks for posting that - used that as reference in the past but couldn't find the link!
 
Thanks @mjs513 for all your answers and the reading URL's you send me.

I finally found your tricks.
The Magnetometer and the Accelerometer use a Butterworth filter.
For the Gyro you are just testing, if there is A or G movement greater or smaller than a value.
If not all must be 0, simply told.

How clever you are.
 
FYI.
Is was reading the BNO080 manual and found the automatic gyro drift calibration method.
If the IMU is stable for x seconds the gyro drift is compensated.
Your method does detect if it is stable.
Now I only have to build in some moving averages and a timer with flag and update the Gyro bias values.
 
I am curious what amount of gyro drift is considered OK. Recently I was playing around with the Lego Mindstorms Gyro Sensor, which connects to their EV3 controller. I don't know what gyro chip they use, but it is rumored to be similar to the Invensense IXZ-2021. I do know something about how it behaves: Lego doesn't give any real specs for it, but in my testing I found it can detect rotation rates down to about 0.18 degrees per second (that is only 2x faster than the minute hand on a clock rotates!) although it is not too accurate below 1 degree per second, which is still a very slow turn (6 minutes per complete rotation).
 
Status
Not open for further replies.
Back
Top