Yaw, pitch and roll - BNO080

[GianmarcoMicrotech]

Hi, I have a new problem, with this buffer 'cargo' i obtain this yaw,pitch and roll value: 0x17 0x80 0x03 0x9B 0xFB 0x17 0x00 0x00 0x00 0x05 0xB2 0x03 0x00 0xA8 0x16 0x21 0xF1 0xF6 0xDF 0x53 0x30 0xCF 0x01

a=0.755065 b=0.354003 c=-0.23236 d=-0.50061 argument=-0.00354 yaw=336.9 pitch=-0.2 roll=50.1

But the pitch value can't be about equal to 0, why the sensor is positioned like this:

                  \
                    \  -> BNO080
                      \
         ---------------------------- -> DESK

The pitch should take the value about 70-80. I posted my code:

/ VARIABLES /

float Q0 , Q1 , Q2 , Q3 ;

float H_est ; // heading accurracy estimation uint8t stat ; // Status (0-3)

int len = 0 ; uint8_t next_data_seqNum = 0x0A ; // reading next data sequence number start with 10 to catch up earlier uint8_t seqNum_2 = 0 ; // writing sequence number channel 2 uint8_t cargo [ 300 ] = "" ;

const uint8_t quat_report = 0x05 ; // defines kind of rotation vector (0x05), geomagnetic (0x09), AR/VR (0x28), // without magnetometer : game rotation vector (0x08), AR/VR Game (0x29)

define reporting_frequency 100 // reporting frequency in Hz

// report interval need to be longer than loop duration not to miss any report ! --> code checks for incrementing seqNum
// note that serial output strongly reduces data rate

uint8_t B0_rate = 1000000 / reporting_frequency ; // Calculate LSB (byte 0) uint8_t B1_rate = ( 1000000 / reporting_frequency ) >> 8 ; // Calculate byte 1

void get_QUAT ( void ) {
memset ( cargo , 0 , sizeof ( cargo ) ) ;

nrf_drv_twi_rx ( &m_twi , BNO_ADDRESS , cargo , 4 ) ;   

// Verifico se ho ricevuto il messaggio

if ( check_cargo_buffer_isEmpty ( cargo , 4 ) == false ) {
memset ( cargo , 0 , sizeof ( cargo ) ) ; nrf_drv_twi_rx ( &m_twi , BNO_ADDRESS , cargo , 23 ) ;

// to reduce I2C bus and computing --> read header first              
    if ( ( cargo [ 9 ] == quat_report ) /*&& ( ( cargo [ 10 ] ) == next_data_seqNum )*/ )
    {      
        int16_t     q0 , q1 , q2 , q3 , h_est ;             
        float       a , b , c , d , norm ;

        // Check for report and incrementing data seqNum
        next_data_seqNum = ++ cargo [ 10 ] ;    // Predict next data seqNum    
        stat_ = cargo [ 11 ] & 0x03 ;       // bits 1:0 contain the status  

        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

        // Apply Q point (quats are already unity vector)
        a = q0 * QP(14) ; b = q1 * QP(14) ; c = q2 * QP(14) ; d = q3 * QP(14) ; 
        norm = sqrtf ( a * a + b * b + c * c + d * d ) ;

        // Skip faulty quats; margin is empirically determined
        if ( abs ( norm - 1.0f ) > 0.0015 ) return ;    

        // Update quaternions
        Q0 = a ; Q1 = b; Q2 = c; Q3 = d ;   

        // bits 1:0 contain the status (0,1,2,3)
        stat_ = cargo [ 11 ] & 0x03 ;        

        // Heading accurracy only in some reports available
        if ( ( quat_report == 0x05 ) || ( quat_report == 0x09 ) || ( quat_report == 0x28 ) )    
  {                 
            // Heading accurracy estimation 
            h_est = ( ( ( int16_t ) cargo [ 22 ] << 8 ) | cargo [ 21 ] ) ;   

            // Apply Q point 
            H_est = h_est * QP(12) ;    

            // Convert to degrees 
            H_est *= radtodeg ; 

  }

        // Calculate euler angles                
        yaw   =  atan2f ( Q1 * Q2 + Q0 * Q3, Q0 * Q0 + Q1 * Q1 - 0.5f ) ;

        // May be > 1 due to rounding errors --> undefined
        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 ) ;

        // Correction of the y axis direction
        yaw += M_PI * 0.5f ; 

        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 ;

        // Ci sono nuovi dati, li invio via BLE
        update_data_BNO080 = true ;

    }

}

}

Where is the problem ? It's very urgent !

Thanks you,

Regards

[jps2000]

Hi when you put your numbers for a,b,c,d in the formula for argument you do get a very different value? do you have not enough free memory ?

[GianmarcoMicrotech]

I have many memory free.

I get these values printing them with the debugging, while the sensor is working: a=0.755065 b=0.354003 c=-0.23236 d=-0.50061 argument=-0.00354 yaw=336.9 pitch=-0.2 roll=50.1

These value ( a,b,c,d, argument, yaw, pitch and roll ) are obtained from this buffer: 0x17 0x80 0x03 0x9B 0xFB 0x17 0x00 0x00 0x00 0x05 0xB2 0x03 0x00 0xA8 0x16 0x21 0xF1 0xF6 0xDF 0x53 0x30 0xCF 0x01

Why the algorithm calculate a pitch value so low, after the sensor is a oblique position of 70 degrees ?

Thanks you,

Regards

[jps2000]

Ok my post above is not correct because I missed one minus sign. Only thing I can argue is that the cal is still poor despite it is showing 3. At high angles eulers fail For example having the sensor perpendicular than there is almost no roll / yaw. What happens at smaller tilt angles? is then everything ok? or are the angles somewhere not correct. Have you applied tare somewhere? this can modify your sensor even permanently. so check first if yaw is correct . than apply pitch +/-45 and then roll +/-45. is that ok?

[GianmarcoMicrotech]

"my post above is not correct because I missed one minus sign." Where is the error in the code ?

"What happens at smaller tilt angles? is then everything ok? or are the angles somewhere not correct. Have you applied tare somewhere? this can modify your sensor even permanently. so check first if yaw is correct . than apply pitch +/-45 and then roll +/-45. is that ok?"
Can you explain better at me ?

Thanks you,

[GianmarcoMicrotech]

I update the informations

If the sensor is resting with the board long side on the desk in a vertical position, i obtain these results: q1=0x008F q2=0x0A78 q3=0xFF54 q0=0x3F22

a= 0.98 b=0.008 c=0.163 d=-0.010

yaw=271 pitch=90 (value fixed, strange) roll=0.8

If the sensor is positioned horizontally on the desk, i obtain these results: q1=0xEC33 q2=0xD366 q3=0xF221 q0=0x2704

a=0.609 b=-0.309 c=-0.696 d=-0.216

yaw=158 pitch=79 roll=-155

If the sensor is resting with the board short side on the desk in a vertical position, i obtain these results: q1=0xD5AD q2=0x11C3 q3=0x0D4C q0=0x2A92

a=0.665 b=-0.661 c=0.277 d=0.207

yaw=276 pitch=-40 roll=-92

These results are corrects?

I have added function TARE, after initialization: void TARE ( void ) { // Rotation vector used to tare defined in quat_report uint8_t RV ;

if ( quat_report == 0x05 ) RV = 0x00 ; if ( quat_report == 0x08 ) RV = 0x01 ; if ( quat_report == 0x09 ) RV = 0x02 ;

// 0x07 means all axes 0x04 = Z axis only; based on rotation vector

uint8_t tare_now [ 16 ] = { 16,0,2,0,0xF2,0,0x03,0,0x07,RV,0,0,0,0,0,0};
uint8_t tare_persist [ 16 ] = {16,0,2,0,0xF2,0,0x03,0x01,RV,0,0,0,0,0,0,0};

// register adress is the LSB of the header 

// I2c.write(BNO_ADDRESS,16,tare_now,15);

nrf_drv_twi_tx ( &m_twi , BNO_ADDRESS , tare_now , 16 , false ) ;   

++seqNum_2 ;

// register adress is the LSB of the header

// I2c.write(BNO_ADDRESS,16,tare_persist,15);

nrf_drv_twi_tx ( &m_twi , BNO_ADDRESS , tare_persist , 16 , false ) ;   

++seqNum_2 ;

}

Thanks you, Regards

[jps2000]

This clarifies your problems. Having the sensor flat on the table there should be yaw xxx (depending on orientation) pitch~0, roll~0 if this is not the case you have done tare or tare persist when the sensor hat an arbitrary (undefined orientation. Better do not use tare at all because then you change yaw pitch roll orientation relative to the BNO housing. You may repair this by clearing the tare setting ( I do not know by heart at the moment how to do that --> look into data sheet) or you can do tare by placing the sensor flat an in direction north on a table after careful calibration. and then do tare persist just once. tare does the following: It sets quaternions to 1,0,0,0 at the actual orientation (any) of the sensor. tare now is not remembered after reset. tare persist is remembered ( as the name says) So you have altered the sensor quaternion output forever.

[GianmarcoMicrotech]

Ok, therefore if i cancel the Tare with this function i resolve my problem ? void clear_TARE ( void ) { // set reorientation uint8_t tare_clear [ 16 ] = {16,0,2,seqNum_2,0xF2,0,0x03,0x02,0,0,0,0,0,0,0,0};

// register adress is the LSB of the header 

nrf_drv_twi_tx ( &m_twi , BNO_ADDRESS , tare_clear , 16 , false ) ; 
++seqNum_2; 

}

The results (yaw, pitch and roll) that i previously posted are correct ?

Thanks you,

[jps2000]

Yes It is the command set reorientation ( Chapter 6.4.4.3 SH-2 reference manual) if you have the BNO flat on the desk and when you get pitch , roll very close to zero it is ok I can not check your results easily but they should be correct.

[GianmarcoMicrotech]

Ok, when the BNO is flat on the desk a sometimes the pitch alternates low values (3.7) a the value fix 90, why ? 90 is not correct. This problem occurs continuously.

Thanks you,

Regards

[jps2000]

I do not understand. Can you post a table showing yaw, pitch, roll, hest and stat (for example every second) when the BNO is flat on the table

[GianmarcoMicrotech]

ok, Results: yaw=69.6 pitch=90 roll=-2.7

Time:2seconds yaw=69.4 pitch=3.7 roll=-2.7

Time:3seconds yaw=69.5 pitch=90 roll=-2.7

Time:3seconds yaw=69.7 pitch=3.6 roll=-2.7

Time:4seconds yaw=70.1 pitch=90 roll=-2.7

Time:5seconds yaw=69.5 pitch=3.6 roll=-2.7

Time:6seconds yaw=69.6 pitch=90 roll=-2.7

Thanks you,

Regards

[jps2000]

that is more than strange. Can you stop bluetooth and look if it is the same?

[GianmarcoMicrotech]

I have execute the test stopped the bluetooth, but the result is the same, the algorithm return wrongly value pitch equal to 90. Why the sensor return this wrong value ?

When the sensor return this wrong value, the algorithm calculate these values: q0=0x0AB8 q1=0xFDA7 q2=0x011E q3=0xC0F5

a=0.167 b=-0.0366 c=0.017 d=-0.0985

norm=1.00000823 stat_=0x03 H_est=4.825

yaw=70.611 pitch=90 roll=-2.681

Thanks you,

Regards

[jps2000]

I have never seen such results. May be you have a problem with the I2C bus: It can be that you do not read correct data from the BNO although it is sending correct data Which controller do you use which BNO board do you use Which pullups do you have? etc First you need 400kHz. Then you are using in your program a combination of older and newer software now. I do not have this ++seqNum_2 etc . Please copy carefully Arduino_BNO080_2.ino or that nrf52 feather_BNO080.ino example without anything else and see if it works

[GianmarcoMicrotech]

OK, i have checked my function are equal to those implemented in the Arduino_BNO080_2.ino example. When the sensor return pitch value equal to 90, this is buffer received to sensor:

0x17 0x80 0x03 0x8F 0xFB 0xA1 0x00 0x00 0x00 0x05 0xE8 0x03 0x6D 0xB5 0xFD 0x10 0x01 0x59 0xC1 0xD0 0x0C 0xF4

[jps2000]

What may happen is that the buffer is not fully rewritten. This you can test by clearing the buffer ( writing zeroes): void get_QUAT() { int16_t q0,q1,q2,q3,h_est; // quaternions q0 = qw 1 = i; 2 = j; 3 = k; float a,b,c,d,norm; for( int i = 0; i<24; i++) cargo[i] =0; // clear cargo just to make sure nothing old is remaining .... With that try again

[jps2000]

IN your example from above you have written: <<<<<< When the sensor return this wrong value, the algorithm calculate these values: q0=0x0AB8 q1=0xFDA7 q2=0x011E q3=0xC0F5

a=0.167 b=-0.0366 c=0.017 d=-0.0985

norm=1.00000823 stat_=0x03 H_est=4.825

yaw=70.611 pitch=90 roll=-2.681

you see that norm is not correctly calculated norm = sqrtf ( a a + b b + c c + d d ) ;

    // Skip faulty quats; margin is empirically determined
    if ( abs ( norm - 1.0f ) > 0.0015 ) return ;    

[GianmarcoMicrotech]

I clear already the buffer cargo with the function memset.

In my results a,b,c,d are corrects ? It's not correct only the calculation of norm value ?

Thanks you, Regards

[GianmarcoMicrotech]

Update : I have commented these two code line and the pitch value remain constant, the pitch value does no longer assumes the value equal to 90, why ? if ( abs ( argument ) > 1.0f ) argument = 1.0f ;

Thanks you, Regards

[jps2000]

Please post your current setup and your current getquat subroutine or - better- your complete code and please answer the questions Which controller do you use? which BNO board do you use Which I2C pullups do you have?

[GianmarcoMicrotech]

Which controller do you use? I use the board nrf52840 from the Nordic. which BNO board do you use? The name is FSM30X. Which I2C pullups do you have? Yes, in the board there is a pullups, but i don't knows give you another informations because i aren't electrical engineer.I'm sorry.

My code :

include

include

include // add

include // add

include

include

include "BNO080.h"

/ DEFINE /

define BNO_ADDRESS 0x4A // Device address when SA0 Pin 17 = GND; 0x4B SA0 Pin 17 = VDD

define REGISTER_ADDRESS 21 // 21 = predicted lenght incl header

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

define radtodeg (180.0f / M_PI)

/ VARIABLES /

float Q0 , Q1 , Q2 , Q3 ;

float H_est ; // heading accurracy estimation uint8t stat ; // Status (0-3)

int len = 0 ; uint8_t next_data_seqNum = 0x0A ; // reading next data sequence number start with 10 to catch up earlier uint8_t seqNum_2 = 0 ; // writing sequence number channel 2 uint8_t cargo [ 300 ] = "" ;

const uint8_t quat_report = 0x05 ; // defines kind of rotation vector (0x05), geomagnetic (0x09), AR/VR (0x28), // without magnetometer : game rotation vector (0x08), AR/VR Game (0x29)

define reporting_frequency 400 // reporting frequency in Hz

// report interval need to be longer than loop duration not to miss any report ! --> code checks for incrementing seqNum
// note that serial output strongly reduces data rate

/ EXTERN / extern const nrf_drv_twi_t m_twi ; extern float yaw ; extern float pitch ; extern float roll ; extern bool update_data_BNO080 ;

// Nota : funzioni riprese dal file 'Arduino_BNO080.ino' e da 'nrf52 feather_BNO080.ino'

/**/ /! Inizilizzo il sensore / /**/ bool BNO080_init ( void ) { uint8_t tmp_buff [ 25 ] = "" ;

nrf_delay_ms ( 1000 ) ;

// Resetto il BNO080

reset_BNO ( ) ;

// Pausa di attesa
nrf_delay_ms ( 1000 ) ;

// Inizalizzo l'IMU
initialize ( ) ;

// Verifico il PID
uint8_t get_PID [ 6 ] = { 6 , 0 , 2 , 0 , 0xF9 , 0 } ;

nrf_drv_twi_tx ( &m_twi , BNO_ADDRESS , get_PID , 6 , false ) ;    

nrf_delay_ms ( 200 ) ;  

nrf_drv_twi_rx ( &m_twi , BNO_ADDRESS , tmp_buff , 25 ) ;

if ( tmp_buff [ 4 ] != 0xF8 )
{   
    uint8_t index_ID ;

    for ( index_ID = 0 ; index_ID < 10 ; index_ID ++ )
    {               
        nrf_delay_ms ( 500 ) ;

        nrf_drv_twi_tx ( &m_twi , BNO_ADDRESS , get_PID , 6 , false ) ;   

        nrf_delay_ms ( 200 ) ;

        memset ( tmp_buff , 0 , sizeof ( tmp_buff ) ) ;

        nrf_drv_twi_rx ( &m_twi , BNO_ADDRESS , tmp_buff , 25 ) ;

        // Risposta corretta
        if ( tmp_buff [ 4 ] == 0xF8 )
            break ;     

    }       

}

if ( tmp_buff [ 4 ] != 0xF8 )
    return KO ;

// Set the required feature report data rate  are generated  at preset report interva
set_feature_cmd_QUAT ( ) ;          

// saves DCD every 5 minutes ( only if cal = 3) 

save_periodic_DCD ( ) ;

// default after reset is (1,0,1,0); switch autocal on @ booting (otherwise gyro is not on) ME_cal( 1 , 1 , 1 , 0 ) ;

return OK ;

}

//**** // COMMANDS //**** / This code activates quaternion output at desired data rate / void set_feature_cmd_QUAT ( void ) { uint8_t tmp_buff [ 25 ] = "" ;

uint32_t rate              = 1000000 / reporting_frequency; 
uint8_t B0_rate            = rate & 0xFF;                       //calculates LSB (byte 0)
uint8_t B1_rate            = (rate >> 8) & 0xFF; 
uint8_t B2_rate            = (rate >> 16) & 0xFF; 
uint8_t B3_rate            = rate >> 24;                        //calculates MSB (byte 3) 

// quat_report determines the kind of quaternions (see data sheets)
//uint8_t quat_setup [ 21 ] = { REGISTER_ADDRESS , 0 , 2 , seqNum_2 , 0xFD , quat_report , 0 , 0 , 0 , B0_rate , B1_rate , 0 , 0 , 0  , 0 , 0 , 0 , 0 , 0 , 0 , 0 } ;   

uint8_t quat_setup[21] = {21,0,2,0,0xFD,quat_report,0,0,0,B0_rate,B1_rate,B2_rate,B3_rate,0,0,0,0,0,0,0,0};

nrf_drv_twi_tx ( &m_twi , BNO_ADDRESS , quat_setup , 21 , false ) ;         

nrf_delay_ms ( 200 ) ;  

nrf_drv_twi_rx ( &m_twi , BNO_ADDRESS , tmp_buff , 25 ) ;

} //void set_feature_cmd_QUAT(){ // quat_report determines the kind of quaternions (see data sheets) // uint8_t quat_setup[21] = {21,0,2,0,0xFD,quat_report,0,0,0,B0_rate,B1_rate,B2_rate,B3_rate,0,0,0,0,0,0,0,0};
// Wire.beginTransmission(BNO_ADDRESS);
// Wire.write(quat_setup, sizeof(quat_setup));
// Wire.endTransmission(); //}

/**** /* This code reads quaternions and looks for new data (incrementing sequence number)

• kind of reporting (quat_report) is defined above */ void get_QUAT ( void ) {
  memset ( cargo , 0 , sizeof ( cargo ) ) ;

  nrf_drv_twi_rx ( &m_twi , BNO_ADDRESS , cargo , 4 ) ;

  // Verifico se ho ricevuto il messaggio if ( check_cargo_buffer_isEmpty ( cargo , 4 ) == false ) {
  memset ( cargo , 0 , sizeof ( cargo ) ) ; nrf_drv_twi_rx ( &m_twi , BNO_ADDRESS , cargo , 23 ) ;

  // to reduce I2C bus and computing --> read header first
  if ( ( cargo [ 9 ] == quat_report ) /&& ( ( cargo [ 10 ] ) == next_data_seqNum )/ ) {
  int16_t q0 , q1 , q2 , q3 , h_est ;
  float a , b , c , d , norm ;

      // Check for report and incrementing data seqNum
      next_data_seqNum = ++ cargo [ 10 ] ;    // Predict next data seqNum    
      stat_ = cargo [ 11 ] & 0x03 ;       // bits 1:0 contain the status  
  
      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
  
      // Apply Q point (quats are already unity vector)
      a = q0 * QP(14) ;       // (1.0f / (1 << n)) 0,03571428571428571428571428571429
      b = q1 * QP(14) ;       // (1.0f / (1 << n))
      c = q2 * QP(14) ;       // (1.0f / (1 << n)) 
      d = q3 * QP(14) ;       // (1.0f / (1 << n))

// a = q0 0.03571428571428571428571428571429 ; // (1.0f / (1 << n)) 0,03571428571428571428571428571429 // b = q1 0.03571428571428571428571428571429 ; // (1.0f / (1 << n)) // c = q2 0.03571428571428571428571428571429 ; // (1.0f / (1 << n)) // d = q3 0.03571428571428571428571428571429 ; // (1.0f / (1 << n))

        norm = sqrtf ( a * a + b * b + c * c + d * d ) ;

        // Skip faulty quats; margin is empirically determined
        if ( abs ( norm - 1.0f ) > 0.0015 ) return ;    

        // Update quaternions
        Q0 = a ; Q1 = b; Q2 = c; Q3 = d ;   

        // bits 1:0 contain the status (0,1,2,3)
        stat_ = cargo [ 11 ] & 0x03 ;        

        // Heading accurracy only in some reports available
        if ( ( quat_report == 0x05 ) || ( quat_report == 0x09 ) || ( quat_report == 0x28 ) )    
  {                 
            // Heading accurracy estimation 
            h_est = ( ( ( int16_t ) cargo [ 22 ] << 8 ) | cargo [ 21 ] ) ;   

            // Apply Q point 
            H_est = h_est * QP(12) ;

            // Convert to degrees 
            H_est *= radtodeg ; 

  }

        // Calculate euler angles                
        yaw   =  atan2f ( Q1 * Q2 + Q0 * Q3 , Q0 * Q0 + Q1 * Q1 - 0.5f ) ;

        // May be > 1 due to rounding errors --> undefined
        float   argument = 2.0f * ( Q1 * Q3 - Q0 * Q2 ) ;   

        // Commentato

// if ( abs ( argument ) > 1.0f ) // argument = 1.0f ;

        pitch =  asinf ( argument ) ; 

        roll  =  atan2f ( Q0 * Q1 + Q2 * Q3, Q0 * Q0 + Q3 * Q3 - 0.5f ) ;

        // Correction of the y axis direction
        yaw += M_PI * 0.5f ; 

        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 ;

// if ( pitch == 90 ) // { // uint8_t w ; //
// w= 9; //
// } // else
// { // uint8_t w ; //
// w= 9; //
// }

        // Ci sono nuovi dati, li invio via BLE
        update_data_BNO080 = true ;

    }

}

}

//***** /* This code request a feature response of quats (6.5.3 --> 6.5.5

• it responds with data: FC,quat_report,0,0,0,interval,interval,0,0,0,0,0,0,0,0,0,0, */

void get_feature_request_quat ( void ) { // uint8_t gfr_quat [ 5 ] = { 0 , 2 , seqNum_2 , 0xFE , quat_report } ; uint8_t gfr_quat [ 6 ] = { 6 , 0 , 2 , seqNum_2 , 0xFE , quat_report } ;

nrf_drv_twi_tx ( &m_twi , BNO_ADDRESS , gfr_quat , 6 , false ) ;    

// I2c.write ( BNO_ADDRESS , 6 , gfr_quat , 5 ) ; // 7 is the LSB of the header 6 bytes to follow ++ seqNum_2 ;

}

//**** /* This code reads the header and fetches available data, The I2C routine can read max 32 bytes at once (or max 256).

• As sometimes more bytes are waiting multiple reads has to be performed

• */ void get_message ( void ) { int i = 0 ; int j = 0 ;

  //*** header** memset ( cargo , 0 , sizeof ( cargo ) ) ; nrf_drv_twi_rx ( &m_twi , BNO_ADDRESS , cargo , 4 ) ;

  // MSB 14:0 remaining cargo length need to block Bit 15. (Bit 15 HIGH means continued) len = ( ( ( uint16_t ) ( cargo [ 1 ] & 0x7F ) << 8 ) | cargo [ 0 ] ) ;

  //*** read channels ** if ( len != 0 ) { memset ( cargo , 0 , sizeof ( cargo ) ) ; nrf_drv_twi_rx ( &m_twi , BNO_ADDRESS , cargo , len ) ; // dump part of the cargo incl header (max 32, may be also > 256)

      if ( check_cargo_buffer_isEmpty ( cargo , len ) == false )      // every read generates one header (length + 4)             
          j += len ;

  }

}

//**** // Verify if the buffer is empty. bool check_cargo_buffer_isEmpty ( uint8_t * tmp_gen_buff , uint8_t tmp_len ) { uint8_t index_gen_buff = 0 ;

for ( index_gen_buff = 0 ; index_gen_buff < tmp_len ; index_gen_buff ++ )
{
    if ( tmp_gen_buff [ index_gen_buff ] != 0 )
        return false ;  

}

return true ;   

}

//**** // This code resets the BNO080 or put it into sleep void reset_BNO ( void ) { // 1 = reset, 2 = on; 3 = sleep // uint8_t action = 1 ;
//
// // uint8_t reset_BNO [ 4 ] = { 0 , 1 , 0 , action } ; // uint8_t buff_reset_BNO [ 5 ] = { 5 , 0 , 1 , 0 , action } ; //
// // I2c.write ( BNO_ADDRESS , 5 , reset_BNO , 4 ) ; //
// nrf_drv_twi_tx ( &m_twi , BNO_ADDRESS , buff_reset_BNO , 5 , false ) ;
//
// nrf_delay_ms ( 1000 ) ;

////////////////////////////// 

// 1 = reset, 2 = on; 3 = sleep
uint8_t     action = 2 ;   

// uint8_t reset_BNO [ 4 ] = { 0 , 1 , 0 , action } ;
uint8_t buff_reset_BNO_2 [ 5 ] = { 5 , 0 , 1 , 0 , action } ;

// I2c.write ( BNO_ADDRESS , 5 , reset_BNO , 4 ) ;

nrf_drv_twi_tx ( &m_twi , BNO_ADDRESS , buff_reset_BNO_2 , 5 , false ) ;    

uint8_t tmp_buff [ 100 ] = "" ;

// Verifico se il ci sono dati disponibili
nrf_drv_twi_rx ( &m_twi , BNO_ADDRESS , tmp_buff , 100 ) ;

}

//**** // This code initializes the BNO080 void initialize ( void ) { // uint8_t init [ 15 ] = { 0 , 2 , seqNum_2 , 0xF2 , 0 , 0x04 , 1 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 } ; uint8_t init [ 16 ] = { 16 , 0 , 2 , seqNum_2 , 0xF2 , 0 , 0x04 , 1 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 } ;

// I2c.write(BNO_ADDRESS,16,init,15); 

nrf_drv_twi_tx ( &m_twi , BNO_ADDRESS , init , 16 , false ) ;

// "******** initialize **********"   
nrf_delay_ms ( 500 ) ;
nrf_delay_ms ( 500 ) ;
nrf_delay_ms ( 500 ) ;
nrf_delay_ms ( 500 ) ;

// wait until response is available to report response                                                                   
get_message ( ) ;
get_message ( ) ;
get_message ( ) ; 

}

//*** /* This code tares and stores results in flash after correct positioning and calibration (follow Tare procedure Hillcrest BNO080 Tare function Usage Guide 1000-4045)

• Make sure to run rotation vector (0x05) and watch h_est and stat (should be << 10 deg and 3 respectively)

• Note that tare and calibration are different things */ void TARE ( void ) { // Rotation vector used to tare defined in quat_report uint8_t RV ;

  if ( quat_report == 0x05 ) RV = 0x00 ; if ( quat_report == 0x08 ) RV = 0x01 ; if ( quat_report == 0x09 ) RV = 0x02 ;

  // 0x07 means all axes 0x04 = Z axis only; based on rotation vector uint8_t tare_now [ 16 ] = { 16,0,2,0,0xF2,0,0x03,0,0x07,RV,0,0,0,0,0,0};
  uint8_t tare_persist [ 16 ] = {16,0,2,0,0xF2,0,0x03,0x01,RV,0,0,0,0,0,0,0};

  // register adress is the LSB of the header // I2c.write(BNO_ADDRESS,16,tare_now,15);

  nrf_drv_twi_tx ( &m_twi , BNO_ADDRESS , tare_now , 16 , false ) ;

  ++seqNum_2 ;

  // register adress is the LSB of the header // I2c.write(BNO_ADDRESS,16,tare_persist,15);

  nrf_drv_twi_tx ( &m_twi , BNO_ADDRESS , tare_persist , 16 , false ) ;

  ++seqNum_2 ;

}

//*** / This code deletes sensor reorientation configuration record fro persist tare command / void clear_TARE ( void ) { // set reorientation uint8_t tare_clear [ 16 ] = {16,0,2,seqNum_2,0xF2,0,0x03,0x02,0,0,0,0,0,0,0,0};

// register adress is the LSB of the header 

//I2c.write(BNO_ADDRESS,16,tare_clear,15);

nrf_drv_twi_tx ( &m_twi , BNO_ADDRESS , tare_clear , 16 , false ) ; 
++seqNum_2; 

}

//*** /* This code disables the calibration running in the background of the accelerometer gyro and magnetometer.

• sensors can be set individually on and off (chapter 6.4.7.1)
• P0 = accelerometer, P1 = gyro; P2 =magnetometer; P4 = planar accelerometer (without Z axis)
• 0 = disable; 1 = enable */

void ME_cal ( uint8_t P0 , uint8_t P1 , uint8_t P2 , uint8_t P4 ) { uint8_t me_cal[16] = {16,0,2,0,0xF2,0,0x07,P0,P1,P2,0,P4,0,0,0,0};

nrf_drv_twi_tx ( &m_twi , BNO_ADDRESS , me_cal , 16 , false ) ;

}

//*** // Saves dynamic cal data periodically every 5 minutes (default)

void save_periodic_DCD ( void ) { uint8_t periodic_dcd[16] = {16,0,2,0,0xF2,0,0x09,0,0,0,0,0,0,0,0,0};

nrf_drv_twi_tx ( &m_twi , BNO_ADDRESS , periodic_dcd , 16 , false ) ;  

}

//////////////////////////////////////////////////////////// int main ( void ) { bool erase_bonds ; ret_code_t ret ;

// Initialize.
    // log_init ( ) ;
timers_init ( ) ;
buttons_leds_init ( &erase_bonds ) ;
power_management_init ( ) ;
ble_stack_init ( ) ;
gap_params_init ( ) ;
gatt_init ( ) ;
advertising_init ( ) ;
services_init ( ) ;
sensor_simulator_init ( ) ;
conn_params_init ( ) ;
peer_manager_init ( ) ;     

    // Inizializzazione necessaria per comunicare con la UART
nrf_drv_clock_lfclk_request ( NULL ) ;
ret = app_timer_init ( ) ;
APP_ERROR_CHECK ( ret ) ;       

    // Inizializzol la comuncazione I2C 
    error_system = twi_init ( ) ;

    // Verifico se devo comunicare lo stato di errore.
    // Nota: Controllo solo error_system in questo caso.
    blink_error ( error_system , NRF_SUCCESS ) ;    

    /* INITIALIZE THE INU */
    error_system = BNO080_init ( ) ;

// Verifico se devo comunicare lo stato di errore.
    // Nota: Controllo solo error_system in questo caso.
    blink_error ( error_system , NRF_SUCCESS ) ;    

 // Start execution.
application_timers_start ( ) ;
advertising_start ( erase_bonds ) ;

    // Cancello la Tara
    clear_TARE ( ) ;

// Enter main loop.
for (;;)
{           
            /******************* IMU. ***************************** 
            */                          
            // IMU BNO080
            get_QUAT ( ) ;

}

}

[jps2000]

I am sorry this is a combination of many programs. I can not and do not want to dig into it.