Closed HyFrow closed 1 year ago
Hi HyFrow,
I suspect the problem lies in how you're initializing or updating the imu values. Mainly, the gyroscope and accelerometer measurements are not initialized to a particular value. Rather, they're probably taking on random values, as this clause at the start of the setup()
or loop()
function
if(IMU.accelerationAvailable() && IMU.gyroscopeAvailable()){ IMU.readAcceleration(acceleration[0],acceleration[1],acceleration[2]); IMU.readGyroscope(gyrom[0], gyrom[1], gyrom[2]);
Might not be executed immediately after the arduino turns on. Try modifying the variable declarations as such:
float acceleration[3] = {0,0,1}; float gyrom[3] = {0,0,0};
Alternatively, try calling the filter update function without the gyroscope data:
fusion.setup()
Let me know if this fixed the problem.
Hello RCmags,
I tried your idea but the problem does not seem to come from here. I tried to find exactly where in the code the problem happens, and from my experimentations I think the issue comes from the updateVel function
Indeed, by adding these prints to the accIntegral.cpp :
I am getting this.
That's why I think the problem might come from the rotate function from quaternion_type.cpp file.
As I said, even when i try to send a constant value for the acceleration, like {0,0,0}, this is still giving me "nan" for the vel_local.x value after the Remove centrifugal step in accIntegral.cpp.
If the quaternion is messing up, it may be the heading is not being updated properly via the fusion.update()
function. It expects the angular velocity to be in radians/second, and the acceleration to be normalized with respect to 1g (earth gravity).
Is the output of the acceleterometer normalized? ( (0,0,1) output at rest ) What units does the gyroscope output have?
That being said, try checking the output of the heading using the "heading" example sketch. If its working, you should be able to read the pitch/yaw/roll angles of the sensor. If it fails, then we're closer to identifying the problem.
Any updates on this issue? How did it go?
Sorry, I didn't check your answer since Friday.
Indeed, I wasn't sending radian/second but degree/second. I changed that. For the acceleration, I am sending in g, but it can go up to +-4g, I don't know if this may cause a problem ? According to your advices, I have modified the heading example with my inputs and it goes like this :
/*
This sketch shows to rotate the heading (yaw angle) of the estimated orientation.
*/
#include <Arduino_LSM9DS1.h>
#include <imuFilter.h>
float acceleration[3] = {0,0,1};
float gyrom[3] = {0,0,0};
imuFilter fusion;
void setup() {
Serial.begin(38400);
if(!IMU.begin())
while(1);
if(IMU.accelerationAvailable()){
IMU.readAcceleration(acceleration[0],acceleration[1],acceleration[2]);
Serial.print("acceleration : ");
Serial.println(acceleration[0]);
}
// Initialize filter:
fusion.setup( acceleration[0], acceleration[1], acceleration[2] );
// Rotate heading:
float angle = 45 * DEG_TO_RAD; // angle in radians to rotate heading about z-axis
fusion.rotateHeading( angle, LARGE_ANGLE ); // Can choose LARGE_ANGLE or SMALL_ANGLE approximation
}
void loop() {
// Update filter:
if(IMU.accelerationAvailable() && IMU.gyroscopeAvailable()){
IMU.readAcceleration(acceleration[0],acceleration[1],acceleration[2]);
IMU.readGyroscope(gyrom[0], gyrom[1], gyrom[2]);
vec3_t gyro;
gyrom[0] *= DEG_TO_RAD;
gyrom[1] *= DEG_TO_RAD;
gyrom[2] *= DEG_TO_RAD;
// 2. update heading
fusion.update( gyrom[0], gyrom[1], gyrom[2], acceleration[0],acceleration[1],acceleration[2] );
// Display angles:
Serial.print( fusion.pitch() );
Serial.print( " " );
Serial.print( fusion.yaw() );
Serial.print( " " );
Serial.print( fusion.roll() );
Serial.println();
}
}
However, I am still getting nan for the 3 angles...
Hi HyFrow,
Sorry for the delay in response. Unfortunately, I don't have an answer for your problem. Have you installed the latest version of "vector_datatype"? That's the only thing, code-wise, that I can think of that would cause the quaterion to fail.
Nonetheless, I was able to replicate your (nan,nan,nan) error using the Arduino IDE v1.8.19.
It seems the problem lay while uploading the sketch to the board. Try only opening an example, and check your board connection. Maybe this will resolve the problem?
Besides this, I've updated the library to make the "accIntegral" class simpler. It explicitly mentions the required gyroscope and accelerometer scales. Try upgrading the library. It may help.
Hello, I'm having a problem.
I'm trying to use the velocity example that you provide with the data from the LSM9DS1 IMU. I am gathering the accelerometer and gyroscope data thanks to the Arduino_LSM9DS1 Library, as it can be seen on the script below. According to the documentation and my previous experimentations, the functions readAccelerometer and readGyroscope return float values for each axis, just like the imu data that you can get from the basicMPU6050.
I'm kind of disappointed, because the only thing I changed compared to your example is to put my values instead of yours in fusion.setup, fusion.update and fusion.updateVel functions, and I'm getting "nan nan nan" in the Serial Console for vel.x, vel.y and vel.z.
Moreover, I tried to send static values (i.e send {2.0,2.0,2.0} for gyro and {1.0,0.5,0.5} for accel), and it still gives the same result.
Do you have any ideas where the problem comes from ?