Problem with this library to obtain yaw, pitch and roll

[GianmarcoMicrotech]

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,

[jps2000]

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

[GianmarcoMicrotech]

Hi,

Thanks you for your response. I have modify the code such as you have indicate me.

define QP12 0.000244141f

define QP14 0.0000610352f

define radtodeg 57.2958f

define M_PI 3.14159265358979323846264338327950288

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,

[jps2000]

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

define QP12 0.000244141f

define QP14 0.0000610352f

by this:

define QP(n) (1.0f / (1 << n)) // 1 << n == 2^-n

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

[GianmarcoMicrotech]

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.

[jps2000]

Great that it works now.