Closed GianmarcoMicrotech closed 4 years ago
Hi
this quats are not ok because magnitude (sqrt(q0^2+q1^2+q2^2+q3^2) should be one. So your quats can not give correct eulers
This is the code from the nfr52 example that works for sure. It checks for errors
if((cargo[9] == quat_report) )
{
q1 = (((int16_t)cargo[14] << 8) | cargo[13] );
q2 = (((int16_t)cargo[16] << 8) | cargo[15] );
q3 = (((int16_t)cargo[18] << 8) | cargo[17] );
q0 = (((int16_t)cargo[20] << 8) | cargo[19] );
// patch for rarely occurring wrong data from BNO of unknown reasons. --> Check if q(0,1,2,3) is not unity vector
a = q0 * QP(14); b = q1 * QP(14); c = q2 * QP(14); d = q3 * QP(14); // apply Q point (quats are already unity vector)
norm = sqrtf(a * a + b * b + c * c + d * d);
if(abs(norm - 1.0f) > 0.0015) return; // skip faulty quats; margin is empirically determined
Q0 = a; Q1 = b; Q2 = c; Q3 = d; // update quaternions
stat_ = cargo[11] & 0x03; // bits 1:0 contain the status (0,1,2,3)
if (quat_report == 0x05 || quat_report == 0x09 || quat_report == 0x28 ) // heading accurracy only in some reports available
{
h_est = (((int16_t)cargo[22] << 8) | cargo[21] ); // heading accurracy estimation
H_est = h_est * QP(12); // apply Q point
H_est *= radtodeg; // convert to degrees
}
// calculate euler angles
yaw = atan2f(Q1 * Q2 + Q0 * Q3, Q0 * Q0 + Q1 * Q1 - 0.5f);
float argument = 2.0f * (Q1 * Q3 - Q0 * Q2); // may be > 1 due to rounding errors --> undefined
if(abs(argument) > 1.0f) argument = 1.0f;
pitch = asinf(argument);
roll = atan2f(Q0 * Q1 + Q2 * Q3, Q0 * Q0 + Q3 * Q3 - 0.5f);
yaw += PI * 0.5f; // correction of the y axis direction
if(yaw < 0) yaw += 2.0f * PI;
if(yaw > 2.0f *PI) yaw -= 2.0f * PI;
yaw *= radtodeg;
yaw = 360.0f - yaw;
pitch *= radtodeg;
roll *= radtodeg;
hope that helps By the way: calibration is also zero in your data
Hi,
Thanks you for your response. I have modify the code such as you have indicate me.
if ( ( cargo [ 9 ] == quat_report ) && ( ( cargo [ 10 ] ) == next_data_seqNum ) )
{
float q0 , q1 , q2 , q3 , a , b , c , d , norm ;
next_data_seqNum = ++ cargo [ 10 ] ;
stat = cargo [ 11 ] & 0x03 ;
q1 = ( ( ( int16_t ) cargo [ 14 ] << 8 ) | cargo [ 13 ] ) ;
q2 = ( ( ( int16_t ) cargo [ 16 ] << 8 ) | cargo [ 15 ] ) ;
q3 = ( ( ( int16_t ) cargo [ 18 ] << 8 ) | cargo [ 17 ] ) ;
q0 = ( ( ( int16_t ) cargo [ 20 ] << 8 ) | cargo [ 19 ] ) ;
a = q0 * QP14 ; b = q1 * QP14 ; c = q2 * QP14 ; d = q3 * QP14 ;
norm = sqrtf ( a * a + b * b + c * c + d * d ) ;
if ( abs ( norm - 1.0f ) > 0.0015 ) return;
Q0 = a ; Q1 = b; Q2 = c; Q3 = d;
stat = cargo [ 11 ] & 0x03;
if ( ( quat_report == 0x05 ) || ( quat_report == 0x09 ) || ( quat_report == 0x28 ) )
{
float tmp_h_est ;
tmp_h_est = ( ( ( int16_t ) cargo [ 22 ] << 8 ) | cargo [ 21 ] );
H_est = tmp_h_est * QP12 ;
H_est *= radtodeg ;
}
yaw = atan2f(Q1 * Q2 + Q0 * Q3, Q0 * Q0 + Q1 * Q1 - 0.5f ) ;
float argument = 2.0f * (Q1 * Q3 - Q0 * Q2);
if(abs(argument) > 1.0f) argument = 1.0f;
pitch = asinf(argument);
roll = atan2f(Q0 * Q1 + Q2 * Q3, Q0 * Q0 + Q3 * Q3 - 0.5f ) ;
yaw += M_PI * 0.5f; // correction of the y axis direction
if(yaw < 0) yaw += 2.0f * M_PI ;
if(yaw > 2.0f *M_PI) yaw -= 2.0f * M_PI ;
yaw *= radtodeg;
yaw = 360.0f - yaw;
pitch *= radtodeg;
roll *= radtodeg;
}
I have obtain these results with this buffer 'cargo' :
0x17 0x80 0x03 0xB1 0xFB 0x1D 0x00 0x00 0x00 0x05 0x0C 0x00 0x00 0xD7 0xFE 0x5D 0x01 0xDB 0x3A 0x13 0x19 0x44 0x32
Q0 = 0.3917 Q1 = 3.9818 Q2 = 0.2130 Q3 = 0.9196
H_est = 180.0000
yaw = 268 pitch = 90 roll 72
Yaw, pitch ad roll are not corrects because the BNO080 sensor is positioned horizontally above the desk. Can you help me ?
Thanks you,
There are still wrong Quaternions (all values should be between +/-1, never ever 3.9) and they should for a unity vector ( lenght 1). Replace those two lines
by this:
and in the code change this line a = q0 QP(14); b = q1 QP(14); c = q2 QP(14); d = q3 QP(14); The QP number needs to be in brackets. This definition works then for all QP.
Then (more important) replace this line: float q0 , q1 , q2 , q3 , a , b , c , d , norm ; by these two: int16_t q0,q1,q2,q3,h_est; // quaternions q0 = qw 1 = i; 2 = j; 3 = k; float a,b,c,d,norm;
What is not working in your code is the conversion from integer to float q0,1,2,3 needs to be int16_t format. Then convert with the QP and then it is a float. --> Just copy carefully the nrf52 feather_BNO080.ino example
Hi, I have resolved my problem, now with BNO080 sensor stationary positioned horizontally above the desk, i received this results: yaw=95 pitch 0.23 roll = -1.23
thanks you.
Great that it works now.
Hi,
I have un problem with this library to obtain yaw, pitch and roll.
With this buffer 'cargo': 0x17 0x80 0x03 0xAF 0xFB 0x19 0x00 0x00 0x00 0x05 0x0B 0x00 0x00 0xC6 0xFE 0xB0 0x01 0x29 0x3A 0xA2 0x1A 0x44 0x32
I have obtain this results: q1 = ( ( ( int16_t ) cargo [ 14 ] << 8 ) | cargo [ 13 ] ) ; q2 = ( ( ( int16_t ) cargo [ 16 ] << 8 ) | cargo [ 15 ] ) ; q3 = ( ( ( int16_t ) cargo [ 18 ] << 8 ) | cargo [ 17 ] ) ; q0 = ( ( ( int16_t ) cargo [ 20 ] << 8 ) | cargo [ 19 ] ) ;
q0 = QP14 ; q1 = QP14 ; q2 = QP14 ; q3 = QP14 ;
q1 = 3.9808 q2 = 0.0263 q3 = 0.9087 q0 = 0.4161
To obtain yaw.pitch and roll i have apply this conversions : tmp_yaw = atan2 ( ( q1 q2 + q0 q3 ) , ( ( q0 q0 + q1 q1 ) - 0.5f ) ) ;
tmp_pitch = -asin ( 2.0f ( q1 q3 - q0 q2 ) ) ; tmp_roll = atan2 ( ( q0 q1 + q2 q3 ) , ( ( q0 q0 + q3 * q3 ) - 0.5f ) ) ;
tmp_yaw = 0.0311 tmp_pitch = 0 tmp_roll = 1.2821
I have convert this variables in degrees: pitch = tmp_pitch ( 180.0 / M_PI ) ; yaw = tmp_yaw ( 180.0 / M_PI ) ; roll = tmp_roll * ( 180.0 / M_PI ) ;
pitch = 0 yaw = 1.78 roll = 73.46
This results in degrees, it are not real because the BNO080 sensor is stopped. Where is the problem ? It's urgent !
Thanks you!
Regards,