velocity example keeps resetting on teensy 4.0

[jindrichus]

Hello, the velocity example seems to reset the teensy on the accIntegral fusion; constructor. I use the vector datatype library in the version 1.3.1 and basicMPU6050 in the version 0.3.0, both installed via Arduino Library management. Any suggestion is appreciated. Thank you.

[RCmags]

Hi! Were you able to get the heading estimate (pitch, yaw, roll) to work?

It might be a variable is overflowing and the Teensy resets as a consequence.

Is the accelerometer calibrated? It MUST read (0,0,1) when perfectly level.

[jindrichus]

Hello, actually, no. It resets immediately on the constructor, which is at the very beginning of the program.

[RCmags]

So you cannot get the code to even run? Do you have any error messages? Any debug information?

Please post your code. Thanks.

[jindrichus]

I tried exactly your velocity example. When I could not get it work, I just added some LED output to the setup() and loop() as the only way I know to debug arduino code. When the first LED lit up, I knew, that setup() even did not start. So I moved the accIntegral fusion; after this LED light up and the LED at least worked. Then teensy rebooted. Sorry for that bad debug output. I am analog electronic designer and I am very bad at programming. If you suggest how to get some debug output from arduino IDE, I will gladly try.

[RCmags]

I see.

Well, we will need to use the Serial.print() function to output values to the console. See this link: https://docs.arduino.cc/software/ide-v2/tutorials/ide-v2-serial-plotter/

Second, what IMU sensor are you using? If you're using the MPU6050, I suggest you try out my library since I can help you calibrate your sensor. See: https://github.com/RCmags/basicMPU6050

It is essential the sensor is calibrated for the acceleration to be integrated properly. This must be done manually with help of the serial console. If possible, please run this example: https://github.com/RCmags/basicMPU6050/blob/main/examples/output/output.ino

Through the output it generates, you will know whether your sensor is calibrated or not. Place the accelerometer perfectly level and read the outputs of each axis. Any deviation from (0,0,1) will represent a bias that you must feedback into the sensor to calibrate it.

This video provides an example of the procedure you need to perform: https://www.youtube.com/watch?v=5xLHZEl0h10

[jindrichus]

Hi, I am using the MPU6500, which should be the same as MPU6050, according the datasheet (only with 6500 having also SPI). And I am using your basicMPU6050 library. The output of the output.ino shows approximately (4,0,1) for acceleration and (0,0,0) from gyroscope. edit: But it ends immediately on the accIntegral fusion;

[RCmags]

The accelerometer is in dire need of calibration. Please run the "raw_output" example from BasicMPU6050: https://github.com/RCmags/basicMPU6050/blob/main/examples/raw_output/raw_output.ino

Place the sensor as level as possible, and read the accelerometer values from the serial console. Store these numbers.

Now run the "parameters" example: https://github.com/RCmags/basicMPU6050/blob/main/examples/parameters/parameters.ino

Reset all offsets as such: AX_OFFSET = AY_OFFSET = AZ_OFFSET = 0 AX_SCALE = AY_SCALE = AZ_SCALE = 1 GX_SCALE = GY_SCALE = GZ_SCALE = 1

Now replace AX_OFFSET and AY_OFFSET with the accelerometer values taken from the "raw_output" example. You will need to adjust all accelerometer offsets so the calibrated output has magnitude = 1 indifferent of how the sensor is rotated. You should read (as close as possible) the vectors (0,0+-1), (+-1,0,0) and (0,+-1,0) accordingly.

Afterwords, replace the aforementioned offsets in the "output" example from ImuFilter: https://github.com/RCmags/imuFilter/blob/main/examples/output/output.ino

Check the serial console. You should be able to clearly measure the pitch, yaw, and roll angles.

[jindrichus]

Well, from the "raw_output", I can see the value overflowing, in the x-axis. It fluctuates from "something close to zero" to "something close to 65536" The device is laying with the +z facing up. It should not, because the most sensitive range of the accelerometer is, according to the datasheet, +-2 g. ...Actually, reading from any axis overflows, when tangential to the earth surface.

[RCmags]

The sensor won't output values in decimals/G-units. It outputs large integer values that need to be scaled depending the chosen sensitivity. If there is any overflow present (since the signal are signed integers), you should see the output suddenly change sign and wrap-around.

If all you see are large values that scale smoothly from zero to some maximum/minimum when you rotate the sensor, then I'd say everything is working fine.

[jindrichus]

No, it is the case when it switches between max and zero. Not smoothly and with only minimum movement, when I try it level. The 65536 is the maximum of 16bit number, which is the resolution of the accelerometer I am aware, that the sensor won't output directly in m/s^2. But the +-2 G sensitivity should not output more than one quarter of the 16 bit maximum without any movement, which would be something around 16000. Other sensitivities settings show even less. In other words, I am surprised to see more than 1 G, while the device is only sitting on the table.

[RCmags]

That is very odd behavior. Do you have any loose wires? Are the SCL and SDA pins connected properly? It could be the sensor is bad. If they are exposed to too much heat when soldering the pins, some of the axes can stop working.

Alternatively, try out a different MPU library and see if it can output smooth values for the accelerometer: https://github.com/ElectronicCats/mpu6050

Unfortunately, this filter library will be useless if you cannot get the sensor to work as intended.

[jindrichus]

The library from Electronic Cats gives pretty reasonable accelerometer ouput. It is well in the +- quarter of 16 bit boundary (+- 16384) and reacts smoothly to changes of positon.

[RCmags]

Great! At this point you will have to perform the calibration procedure mentioned above.

• You need to subtract any bias from the accelerometer when it is level.
• You may need multiply the accelerometer by a scaling factor to ensure it reads 1g indifferent of its orientation.

Note that the gyroscope also needs to be calibrated. It MUST read (0,0,0) when not rotating and its output must be in radians / second.

Finally, return to the "output" example from ImuFilter and replace all mentions of:

• imu.ax(), imu.ay(), imu.az() with your accelerometer output.
• imu.gx(), imu.gy(), imu.gz() with your gyroscope output.

[jindrichus]

Oh, I see now, what problem I encounter with the 65000 value. Because the sensor output is two's complement, the values close to the 65536 are actualy something negative close to 0. It can also explain, why I obtain values up to 4 g in your "ouptut" example and none negative value at all.

[jindrichus]

I changed the declaration in your basicMPU6050 library from int to int16_t and now I am able to calibrate the "parameters" example and it outputs reasonable values. But with the "velocity" teensy still reboots on the accIntegral fusion; constructor.

[RCmags]

Excellent detail. I will update BasicMPU6050 to include that modification. Thank you for identifying and fixing that bug.

That aside, since the sensor now works, I still need to know whether you have calibrated the accelerometer. It is essential that it reads 1g while at rest as that is the reference the sensor fusion uses to determine orientation.

It could be that imuFusion is failing because of another bug like the one you found. Can you get the "output" example to work and measure pitch, yaw, and roll?

[jindrichus]

Yes, I have calibrated the accelerometer, but it drifts a bit during an hour. And yes, the "output" example works fine.

[jindrichus]

...altough I cannot say in what units the pitch is. The others, I would say, are in radians, right? Those are in the range +- pi. But pitch is in half of that range.

[jindrichus]

This: vec3_t vel = 0; seems to be te problem. I changed the initial value to {0,0,0} and now the example runs.

[RCmags]

...altough I cannot say in what units the pitch is. The others, I would say, are in radians, right? Those are in the range +- pi. But pitch is in half of that range.

Yes all heading angles are in radians. Pitch only goes up to +-90 degrees if I remember as it is calculated slightly differently than yaw or roll.

This: vec3_t vel = 0; seems to be te problem. I changed the initial value to {0,0,0} and now the example runs.

That would definitely cause an error. Vel is defined as a vector so it must be initialized with three components.

If you need very precise position estimates, I recommend you use a different sensor in place of the MPU-6500. Its accelerometer is very noisy and causes a lot of drift as you have noticed. The GY-BNO055 is much better alternative as this video demonstrates:

https://www.youtube.com/watch?v=3-IBOJ5FQvI&t=344s

At this point you should have everything working. Please let me know if you have any questions adjusting the sensor, etc. The default parameters should work well though.

[RCmags]

I changed the declaration in your basicMPU6050 library from int to int16_t and now I am able to calibrate the "parameters" example and it outputs reasonable values. But with the "velocity" teensy still reboots on the accIntegral fusion; constructor.

What file did you modify and at what line?

I'm guessing its "basicMPU6050.tpp" @ line 49 ?

[jindrichus]

Files basicMPU6050.h lines 97 to 107 and in basicMPU6050.tpp in "raw measurements" section 80 to 124 (so the functions returns int16_t as declared in the *.h after the change). I am not sure if it is necessary that way. As I look at the 49th line, it seem to make more sense. Unfortunately, I cannot try It right now, as I am away from the hardware till monday.

You are right about the sensor - it is drifting really crazy. I will eventually try the GY-BNO055 thank you for the tip.

Yes, I have everything working now. Thank you very much, it works nice!

My effort here was more or less an evaluation whether the accelerometer is usable as an position encoder. Well, with a drift like that, certainly not. Actually, I doubt now, that it is even possible with any available accelerometer to inertially track the position with, say, a millimeter accuracy. But I might be surprised :)

[jindrichus]

Thank you for all the effort. I appreciate it.

[RCmags]

Files basicMPU6050.h lines 97 to 107 and in basicMPU6050.tpp in "raw measurements" section 80 to 124 (so the functions returns int16_t as declared in the *.h after the change). I am not sure if it is necessary that way. As I look at the 49th line, it seem to make more sense. Unfortunately, I cannot try It right now, as I am away from the hardware till monday.

Thanks! I will perform those edits ASAP and give you credit.

My effort here was more or less an evaluation whether the accelerometer is usable as an position encoder. Well, with a drift like that, certainly not. Actually, I doubt now, that it is even possible with any available accelerometer to inertially track the position with, say, a millimeter accuracy. But I might be surprised :)

These cheap accelerometers work okay for measuring sudden rapid displacements and jerky motion. Its during slow, drawn out motions with low acceleration that they become useless for position tracking. If you can combine the accelerometer with some other position sensor or even a dead reckoning estimate, they provide a good compliment.

See this video starting at 1:00. Its an example of using this same library to enhance odometry for navigation:
https://www.youtube.com/watch?v=VYhLW5owS3A

Thank you for all the effort. I appreciate it.

I'm happy to have helped :)

[RCmags]

Your changes were made to the basicMPU6050 library. See: https://github.com/RCmags/basicMPU6050/releases/tag/0.3.1

[mittlc]

This: vec3_t vel = 0; seems to be te problem. I changed the initial value to {0,0,0} and now the example runs.

@jindrichus can you share in which file did you change this to fix the reset problem? I face a simillar reset error running the Velocity example.

[jindrichus]

It was in accIntegral.h

[mittlc]

Thanks @jindrichus that helped. Now i only need to figure out how i need to apply the accalerometer offset correctly.

The output example is giving me these values: 0.07 -0.01 1.03 0.00 0.00 0.00 41.34 0.07 -0.01 1.03 0.00 0.00 0.00 41.35 0.07 -0.01 1.03 0.00 0.00 -0.00 41.34 0.07 -0.01 1.03 -0.00 0.00 -0.00 41.34

But the parameters example result doesn´t change notably if i supply those offsets (maybe the offsets are already very close to the target of 0,0,1..... Which seems weird because that would mean my mpu9250 is nearly perfectly calibrated already.

[jindrichus]

Do you calibrate the offsets by the raw values of the device?

[RCmags]

Yes. Read the raw accelerometer values with the sensor held level, and then use those as your offsets.

Also, the last value here: 0.07 -0.01 1.03 -0.00 0.00 -0.00 41.34

It's the temperature of sensor, no? Just kind of confused by the number.

[mittlc]

Ah i see - do you take a average across 100 readings to get your calibration offsets? And for which value in the z Axes should i correct by an offset?

The raw output is quite jittery 56 -36 16848 -251 185 -96 1647 -44 88 16760 -249 205 -94 1645 -68 56 16744 -249 200 -93 1644 -100 8 16852 -251 186 -95 1644 116 16 16788 -253 186 -95 1644

Yeah that last column is the temp. sensor - something is very off there apparently :D

[RCmags]

Temp is a bit high, but whatever looool

Just correct all axes with the value measured by the sensor. You can calculate the average values or just make an educated guess of the averages. The offsets only need to get the accelerometer readings close to (0,0,1) as imuFilter will provide further calibration.

Note: Made a new release based on the accIntegral.h corrections mentioned in this thread. https://github.com/RCmags/imuFilter/releases/tag/1.6.3

[mittlc]

Hi @RCmags , i can confirm that after uninstalling and getting the new version of the imuFilter the resetting error does not occur anymore.

I guess you can close thise issue again and i will be of experimenting if this approach may lead me to a solution for my project.

Thanks

[RCmags]

Excellent. Thank you for reporting the changes were successful.