Hi all,
I am currently developing software for my own home built quad copter. It is powered by an Arduino Mega 2560, and has a Chinese IMU (GY-85) wired via I2C using the arduino "Wire" library. Furthermore the motors are controlled via the "Servo" Arduino library. The orientation of the IMU is calculated by the Sebastian Madgwick AHRS function. (code below)
So far it has gone well, but now I have stumbled upon a (for me) perplexing problem. I am hoping someone will be able to give me new insights about how to tackle this problem.
The problem:
Succinctly put the AHRS algotithm returns a quaternion of which all 4 values are NAN (Not A Number).
The weird part is that I can fix this (i.e. returning usable values) by adding the line:
delayMicroseconds(1);
In a specific part of the algorithm:
Quaternion IMU_MADGWICK::MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
}
// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-quaternion.q1 * gx - quaternion.q2 * gy - quaternion.q3 * gz);
qDot2 = 0.5f * (quaternion.q0 * gx + quaternion.q2 * gz - quaternion.q3 * gy);
qDot3 = 0.5f * (quaternion.q0 * gy - quaternion.q1 * gz + quaternion.q3 * gx);
qDot4 = 0.5f * (quaternion.q0 * gz + quaternion.q1 * gy - quaternion.q2 * gx);
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrt2(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Normalise magnetometer measurement
recipNorm = invSqrt2(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0mx = 2.0f * quaternion.q0 * mx;
_2q0my = 2.0f * quaternion.q0 * my;
_2q0mz = 2.0f * quaternion.q0 * mz;
_2q1mx = 2.0f * quaternion.q1 * mx;
_2q0 = 2.0f * quaternion.q0;
_2q1 = 2.0f * quaternion.q1;
_2q2 = 2.0f * quaternion.q2;
_2q3 = 2.0f * quaternion.q3;
_2q0q2 = 2.0f * quaternion.q0 * quaternion.q2;
_2q2q3 = 2.0f * quaternion.q2 * quaternion.q3;
q0q0 = quaternion.q0 * quaternion.q0;
q0q1 = quaternion.q0 * quaternion.q1;
q0q2 = quaternion.q0 * quaternion.q2;
q0q3 = quaternion.q0 * quaternion.q3;
q1q1 = quaternion.q1 * quaternion.q1;
q1q2 = quaternion.q1 * quaternion.q2;
q1q3 = quaternion.q1 * quaternion.q3;
q2q2 = quaternion.q2 * quaternion.q2;
q2q3 = quaternion.q2 * quaternion.q3;
q3q3 = quaternion.q3 * quaternion.q3;
==================>> //delayMicroseconds(1); //<==========================================================================
// Reference direction of Earth's magnetic field
hx = mx * q0q0 - _2q0my * quaternion.q3 + _2q0mz * quaternion.q2 + mx * q1q1 + _2q1 * my * quaternion.q2 + _2q1 * mz * quaternion.q3 - mx * q2q2 - mx * q3q3;
hy = _2q0mx * quaternion.q3 + my * q0q0 - _2q0mz * quaternion.q1 + _2q1mx * quaternion.q2 - my * q1q1 + my * q2q2 + _2q2 * mz * quaternion.q3 - my * q3q3;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q0mx * quaternion.q2 + _2q0my * quaternion.q1 + mz * q0q0 + _2q1mx * quaternion.q3 - mz * q1q1 + _2q2 * my * quaternion.q3 - mz * q2q2 + mz * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
// Gradient decent algorithm corrective step
s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * quaternion.q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * quaternion.q3 + _2bz * quaternion.q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * quaternion.q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * quaternion.q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * quaternion.q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx *quaternion.q2 + _2bz * quaternion.q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * quaternion.q3 - _4bz * quaternion.q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f *quaternion.q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx *quaternion.q2 - _2bz * quaternion.q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * quaternion.q1 + _2bz * quaternion.q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * quaternion.q0 - _4bz *quaternion.q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx *quaternion.q3 + _2bz * quaternion.q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * quaternion.q0 + _2bz *quaternion.q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * quaternion.q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
recipNorm = invSqrt2(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
quaternion.q0 += qDot1 * (1.0f / sampleFrequency);
quaternion.q1 += qDot2 * (1.0f / sampleFrequency);
quaternion.q2 += qDot3 * (1.0f / sampleFrequency);
quaternion.q3 += qDot4 * (1.0f / sampleFrequency);
// Normalise quaternion
recipNorm = invSqrt2(quaternion.q0 * quaternion.q0 + quaternion.q1 * quaternion.q1 + quaternion.q2 * quaternion.q2 + quaternion.q3 * quaternion.q3);
quaternion.q0 *= recipNorm;
quaternion.q1 *= recipNorm;
quaternion.q2 *= recipNorm;
quaternion.q3 *= recipNorm;
return quaternion;
}
To make it even weirder, replacing the delay line by:
if(isnan(quaternion.q0)
{
}
Will also result in the algorithm working without NAN values.
Some additional information about how the method is called:
The algorithm calculates an orientation that is returned as a quaternion (struct that consists of floats q0,q1,q2,q3)
This algorithm is called 100 times per second.
This is done via hardware timer 2 to generate an interrupt:
/*Interrupt service routines (ISR)*/
ISR(TIMER2_COMPB_vect)
{
TCNT2 = 0;
isr_timer_fired = true;
}
This interrupt sets a flag upon which a task will be executed:
if(isr_timer_fired)
{
isr_timer_fired = false;
Process100HzTask();
}
This task then updates its orientation via the Madwick algotitm:
void Process100HzTask(void)
{
{....} Filter incoming sensor data noise
if(isnan(sensorData.compassX) || isnan(sensorData.compassY) || isnan(sensorData.compassZ) || isnan(sensorData.accelX) || isnan(sensorData.accelY) || isnan(sensorData.accelZ) || isnan(sensorData.gyroX) || isnan(sensorData.gyroY) || isnan(sensorData.gyroZ))
{
Serial.println("found error\n\n\n\n\n\n");
}
orientationQuat = imu.MadgwickAHRSupdate(sensorData.gyroX,sensorData.gyroY,sensorData.gyroZ,
-sensorData.accelX,-sensorData.accelY,sensorData.accelZ,
sensorData.compassX,-sensorData.compassY,sensorData.compassZ);
if(isnan(orientationQuat.q0) && isnan(orientationQuat.q1) && isnan(orientationQuat.q2) && isnan(orientationQuat.q3))
{
Serial.println("q0 t/m q3 are nan");
}
{...} stabilize quad copter
}
End part 1.
MadgwickAHRS.cpp (10.7 KB)
MadgwickAHRS.h (1.51 KB)
