Info progetto IMU + NRF24l01

salve a tutti,

Voglio intraprendere un progetto che consiste nel poter inserire in una piccola scatolina una piattaforma imu (GY 80) collegata ad un Arduino Nano e ad un modulo wireless 2.4 ghz che mi permetta di trasmettere i dati ad un altro Arduino connesso via usb al pc per poterli leggere. Volevo in pratica poter consentire al sensore di effettuare tutti i movimenti del caso senza essere vincolato da nessun "cavetto".

Ho già implementato il codice per elaborare i dati della imu e stamparli in seriale per poter essere accessibili (per ora) collegando il cavo usb e aprendo il monitor seriale dall IDE di Arduino.

Il passo successivo sarebbe appunto implementare il modulo NRF24l01 a 2.4ghz per poter svincolarmi dal cavetto usb, potrebbe essere fattibile fare una cosa del genere? Premetto che non so ancora molto di questi modulino e non li ho mai utilizzati prima

Ciao Giacomo,

Naturalmente i modulini radio servono per trasmettere senza fili, il fattibile è quindi presto fatto anche se... Come tutte le cose dipende dalle prestazioni che vorrai raggiungere.

Ciao! Grazie per la risposta prima di tutto ;D In pratica la frequenza con cui lavora la mia imu è 50 Hz.. Quindi 20 ms.. Ho visto questo video, quindi in teoria posso farcela ma non so da dove iniziare ahah che dite?

https://m.youtube.com/watch?v=OC5KFWCgRdM

E 20ms caratterizzerebbe tutte le tue specifiche? :smiling_imp:
Beato te…

Si in effetti ho detto una cavolata :confused: .. comunque ho provato a cercare in rete e ho trovato un paio di sketch con cui ho provato ad armeggiare.. il codice con cui leggo i dati e li trasmetto è questo, ho aggiunto la parte di codice dell'NRF24l01, mentre nel post successivo metterò il codice dell'arduino in ricezione.. ovviamente non funziona, è solo un tentativo, ma visto che a logica dovrebbe andare, non capisco per quale arcano motivo sul monitor seriale dell'arduino in ricezione mi appare che non è disponeibile nessuna connessione radio, eppure se provo a fare uno sketch in ricezione inviando via radio 2 semplici valori tutto sembra funzionare, ma quando provo a implementarlo (come sotto) nel codice completo per inviare i 3 angoli di eulero niente è come se non arrivasse nessun segnale :confused:

[code]
#define ACCEL_X_MIN (-278.00f)
#define ACCEL_X_MAX (270.00f)
#define ACCEL_Y_MIN (-254.0f)
#define ACCEL_Y_MAX (284.0f)
#define ACCEL_Z_MIN (-294.0f)
#define ACCEL_Z_MAX (235.0f)


#define MAGN_X_MIN (-511.0f)
#define MAGN_X_MAX (581.0f)
#define MAGN_Y_MIN (-516.0f)
#define MAGN_Y_MAX (568.0f)
#define MAGN_Z_MIN (-489.0f)
#define MAGN_Z_MAX ( 486.0f)


#define GYRO_X_OFFSET (-34.82f)
#define GYRO_Y_OFFSET (100.41f)
#define GYRO_Z_OFFSET (-16.38f)


// Altymeter
#define ALT_SEA_LEVEL_PRESSURE 102133


#include <Wire.h>

// Sensor calibration scale and offset values
#define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f)
#define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f)
#define ACCEL_Z_OFFSET ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f)
#define ACCEL_X_SCALE (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET))
#define ACCEL_Y_SCALE (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET))
#define ACCEL_Z_SCALE (GRAVITY / (ACCEL_Z_MAX - ACCEL_Z_OFFSET))

#define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f)
#define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f)
#define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f)
#define MAGN_X_SCALE (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET))
#define MAGN_Y_SCALE (100.0f / (MAGN_Y_MAX - MAGN_Y_OFFSET))
#define MAGN_Z_SCALE (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET))

// Gain for gyroscope
#define GYRO_GAIN_X (0.06957f)
#define GYRO_GAIN_Y (0.06957f)
#define GYRO_GAIN_Z (0.06957f)

#define GYRO_X_SCALE (TO_RAD(GYRO_GAIN_X))
#define GYRO_Y_SCALE (TO_RAD(GYRO_GAIN_Y))
#define GYRO_Z_SCALE (TO_RAD(GYRO_GAIN_Z))

// DCM parameters
#define Kp_ROLLPITCH (0.02f)
#define Ki_ROLLPITCH (0.00002f)
#define Kp_YAW (1.2f)
#define Ki_YAW (0.00002f)

// Stuff
#define GRAVITY (256.0f) // "1G reference" used for DCM filter and accelerometer calibration
#define TO_RAD(x) (x * 0.01745329252)  // *pi/180
#define TO_DEG(x) (x * 57.2957795131)  // *180/pi

// RAW sensor data
float accel[3];  // Actually stores the NEGATED acceleration (equals gravity, if board not moving).
//float accel_min[3];
//float accel_max[3];

float magnetom[3];
//float magnetom_min[3];
//float magnetom_max[3];

float gyro[3];
//float gyro_average[3];
//int gyro_num_samples = 0;

float temperature;
float pressure;
float altitude;

// DCM variables
float MAG_Heading;
float Magn_Vector[3]= {0, 0, 0}; // Store the magnetometer turn rate in a vector
float Accel_Vector[3]= {0, 0, 0}; // Store the acceleration in a vector
float Gyro_Vector[3]= {0, 0, 0}; // Store the gyros turn rate in a vector
float Omega_Vector[3]= {0, 0, 0}; // Corrected Gyro_Vector data
float Omega_P[3]= {0, 0, 0}; // Omega Proportional correction
float Omega_I[3]= {0, 0, 0}; // Omega Integrator
float Omega[3]= {0, 0, 0};
float errorRollPitch[3] = {0, 0, 0};
float errorYaw[3] = {0, 0, 0};
float DCM_Matrix[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
float Update_Matrix[3][3] = {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}};
float Temporary_Matrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};

// Euler angles
float yaw, pitch, roll;

// DCM timing in the main loop
long timestamp;
long timestamp_old;
float G_Dt; // Integration time for DCM algorithm

// More output-state variables
int num_accel_errors = 0;
int num_magn_errors = 0;
int num_gyro_errors = 0;



//PARTE DI TRASMISSIONE MODULO NRF24L01




 IMPORT LIBRERIE DEDICATE 
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>

/*-----( DICHIARO I PIN )-----*/
#define CE_PIN   9
#define CSN_PIN 10

// NOTE: the "LL" at the end of the constant is "LongLong" type
const uint64_t pipe = 0xE8E8F0F0E1LL; // Define the transmit pipe


/*-----( Declare objects )-----*/

RF24 radio(CE_PIN, CSN_PIN); // Create a Radio

/*-----( Declare Variables )-----*/

float eulero[3];  // 3 element array 






void ReadSensors() {
  Read_Pressure();
  Read_Gyro(); // Read gyroscope
  Read_Accel(); // Read accelerometer
  Read_Magn(); // Read magnetometer
  ApplySensorMapping();  
}


void reset_sensor_fusion()
{
  float temp1[3];
  float temp2[3];
  float xAxis[] = {1.0f, 0.0f, 0.0f};

  ReadSensors();
  
  timestamp = millis();
  
  // GET PITCH
  // Using y-z-plane-component/x-component of gravity vector
  pitch = -atan2(Accel_Vector[0], sqrt(Accel_Vector[1] * Accel_Vector[1] + Accel_Vector[2] * Accel_Vector[2]));
 
 
  Vector_Cross_Product(temp1, Accel_Vector, xAxis);
  Vector_Cross_Product(temp2, xAxis, temp1);
 
  roll = atan2(temp2[1], temp2[2]);
  
  // GET YAW
  Compass_Heading();
  yaw = MAG_Heading;
  
  // Init rotation matrix
  init_rotation_matrix(DCM_Matrix, yaw, pitch, roll);
}

// Apply calibration to raw sensor readings
void ApplySensorMapping()
{
    // Magnetometer axis mapping
    Magn_Vector[1] = -magnetom[0];
    Magn_Vector[0] = -magnetom[1];
    Magn_Vector[2] = -magnetom[2];

    // Magnetometer values mapping
    Magn_Vector[0] -= MAGN_X_OFFSET;
    Magn_Vector[0] *= MAGN_X_SCALE;
    Magn_Vector[1] -= MAGN_Y_OFFSET;
    Magn_Vector[1] *= MAGN_Y_SCALE;
    Magn_Vector[2] -= MAGN_Z_OFFSET;
    Magn_Vector[2] *= MAGN_Z_SCALE;
  
    // Accelerometer axis mapping
    Accel_Vector[1] = accel[0];
    Accel_Vector[0] = accel[1];
    Accel_Vector[2] = accel[2];

    // Accelerometer values mapping
    Accel_Vector[0] -= ACCEL_X_OFFSET;
    Accel_Vector[0] *= ACCEL_X_SCALE;
    Accel_Vector[1] -= ACCEL_Y_OFFSET;
    Accel_Vector[1] *= ACCEL_Y_SCALE;
    Accel_Vector[2] -= ACCEL_Z_OFFSET;
    Accel_Vector[2] *= ACCEL_Z_SCALE;
    
    // Gyroscope axis mapping
    Gyro_Vector[1] = -gyro[0];
    Gyro_Vector[0] = -gyro[1];
    Gyro_Vector[2] = -gyro[2];

    // Gyroscope values mapping
    Gyro_Vector[0] -= GYRO_X_OFFSET;
    Gyro_Vector[0] *= GYRO_X_SCALE;
    Gyro_Vector[1] -= GYRO_Y_OFFSET;
    Gyro_Vector[1] *= GYRO_Y_SCALE;
    Gyro_Vector[2] -= GYRO_Z_OFFSET;
    Gyro_Vector[2] *= GYRO_Z_SCALE;
}


void setup()
{
  // Init serial output
  Serial.begin(OUTPUT_BAUD_RATE);
  
  // MODULO NRF24l01
  radio.begin();
  radio.openWritingPipe(pipe);
  
  // Init sensors
  delay(50);  // Give sensors enough time to start
  I2C_Init();
  Accel_Init();
  Magn_Init();
  Gyro_Init();
  Pressure_Init();
  
  // Read sensors, init DCM algorithm
  delay(20);  // Give sensors enough time to collect data
  reset_sensor_fusion();
}

// Main loop
void loop()
{
  // Time to read the sensors again?
  if ((millis() - timestamp) >= OUTPUT_DATA_INTERVAL) {
    timestamp_old = timestamp;
    timestamp = millis();
    if (timestamp > timestamp_old)
      G_Dt = (float) (timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
    else
      G_Dt = 0;

    ReadSensors();
    
    // Run DCM algorithm
    Compass_Heading(); // Calculate magnetic heading

    Matrix_update();
    Normalize();
    Drift_correction();
    Euler_angles();
    
    
    Serial.print(TO_DEG(yaw));    Serial.print(";");
    Serial.print(TO_DEG(pitch));  Serial.print(";");
    Serial.print(TO_DEG(roll));   Serial.print(";");
    Serial.print(temperature);    Serial.print(";");
    Serial.print(pressure);       Serial.print(";");
    Serial.print(altitude);       Serial.println();
    
  //Trasmetto via radio i 3 angoli di eulero 
    
  eulero[0] = TO_DEG(yaw);
  eulero[1] = TO_DEG(pitch);
  eulero[2] = TO_DEG(pitch);
  
  radio.write( eulero, sizeof(eulero) );
    
    
  }
}

[/code]

codice in Ricezione

/*-----( Import needed libraries )-----*/
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
/*-----( Declare Constants and Pin Numbers )-----*/
#define CE_PIN   9
#define CSN_PIN 10

// NOTE: the "LL" at the end of the constant is "LongLong" type
const uint64_t pipe = 0xE8E8F0F0E1LL; // Define the transmit pipe


/*-----( Declare objects )-----*/
RF24 radio(CE_PIN, CSN_PIN); // Create a Radio
/*-----( Declare Variables )-----*/
float eulero[3];  // 2 element array holding Joystick readings

void setup()   /****** SETUP: RUNS ONCE ******/
{
  Serial.begin(9600);
  delay(1000);
  Serial.println("Nrf24L01 Receiver Starting");
  radio.begin();
  radio.openReadingPipe(1,pipe);
  radio.startListening();;
}//--(end setup )---


void loop()   /****** LOOP: RUNS CONSTANTLY ******/
{
  if ( radio.available() )
  {
    // Read the data payload until we've received everything
    bool done = false;
    while (!done)
    {
      // Fetch the data payload
      done = radio.read( eulero, sizeof(eulero) );
      Serial.print("X = ");
      Serial.print(eulero[0]);
      Serial.print(" Y = ");      
      Serial.println(eulero[1]);
      Serial.print(" Z = ");      
      Serial.println(eulero[2]);
    }
  }
  else
  {    
      Serial.println("No radio available");
  }

}

In pratica non ho fatto altro che provare a trasmettere roll pitch e yaw messi all interno di un array cercando di implementare il codice Che ho trovato qua https://arduino-info.wikispaces.com/Nrf24L01-2.4GHz-HowTo

Controlla se la dimensione dei pacchetti corrisponde col payload radio.

Ad ogni modo come minimo: - range di funzionamento - autonomia - banda

ti servono per capire se i modulini radio fanno al caso tuo.

Ok graziee ;D

Per quanto riguarda il payload ho provato ad inviare lo stesso array float di 3 elementi (che ho testato essere trasmissibile) del codice sketch di esempio dell ultimo link che ho messo…

Buon giorno! Ho notato che se nello sketch di prova, in trasmissione, metto un delay maggiore di 5 ms prima di scrivere radio.write( eulero, sizeof(eulero) ); mi da il solito errore "no radio available" penso che sia quindi ne codice del programma un problema di tempi, come potrei risolvere?

Nessuno riesce a spiegarmi il perché? :'(

Quant'è la tua payloadsize e quanti byte cerchi di inviare?

Ho 32 Bytes di payload disponibile con il modulo NRF24l01, vorrei trasmettere ameno un array di 3 float che contengono i 3 angoli di eulero (nel programma eulero[3] )

Il payload è di massimo 32 char, non è questo il tuo problema, sappi che puoi trasmettere e ricevere solo caratteri ascii, quindi regolati di conseguenza. A me quei modulini funzionano perfettamente.

un float è 32 bit, devi trasmettere esattamente 32 byte quindi vedi tu..

triac60: Il payload è di massimo 32 char, non è questo il tuo problema, sappi che puoi trasmettere e ricevere solo caratteri ascii, quindi regolati di conseguenza. A me quei modulini funzionano perfettamente.

no puo' trasmettere qualsiasi valore anche float purche' i byte "totali" non superino i 32

leggi un po come ho risolto per i payload

in questo esempio ho trasmesso 3 variabili float quindi 12 byte, di spazio ne rimane ;)

http://forum.arduino.cc/index.php?topic=313642.0

Grazie1000 ora però ho un probema strano.. Arduino trasmette solo i primi 3 float dopo il reset, poi più nulla fino a che non premo il pulsante di reset ancora e la situazione si ripete

giacomobonatti: Grazie1000 ora però ho un probema strano.. Arduino trasmette solo i primi 3 float dopo il reset, poi più nulla fino a che non premo il pulsante di reset ancora e la situazione si ripete

allega il codice

Allego la parte della trasmissione

#define OUTPUT_BAUD_RATE 115200


#define OUTPUT_DATA_INTERVAL 20  // in milliseconds

#define ACCEL_X_MIN (-250.0f)
#define ACCEL_X_MAX (270.00f)
#define ACCEL_Y_MIN (-254.0f)
#define ACCEL_Y_MAX (284.0f)
#define ACCEL_Z_MIN (-294.0f)
#define ACCEL_Z_MAX (235.0f)


#define MAGN_X_MIN (-600.0f)
#define MAGN_X_MAX (600.0f)
#define MAGN_Y_MIN (-600.0f)
#define MAGN_Y_MAX (600.0f)
#define MAGN_Z_MIN (-600.0f)
#define MAGN_Z_MAX (600.0f)

// Gyroscope
// "gyro x,y,z (current/average) = .../OFFSET_X  .../OFFSET_Y  .../OFFSET_Z
#define GYRO_X_OFFSET (0.0f)
#define GYRO_Y_OFFSET (0.0f)
#define GYRO_Z_OFFSET (0.0f)


// Altymeter
#define ALT_SEA_LEVEL_PRESSURE 102133  //IN PASCAL


/*‐‐‐‐‐( Import needed libraries )‐‐‐‐‐*/
#include <Wire.h>
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <RF24_config.h>

/*‐‐‐‐‐( Declare Constants and Pin Numbers )‐‐‐‐‐*/
#define CE_PIN 9
#define CSN_PIN 10
// NOTE: the "LL" at the end of the constant is "LongLong" type
const uint64_t pipe = 0xE8E8F0F0E1LL; // Define the transmit pipe
int reading = 0;
int val=0;
float aa;
/*‐‐‐‐‐( Declare objects )‐‐‐‐‐*/
RF24 radio(CE_PIN, CSN_PIN); // Create a Radio

// Sensor calibration scale and offset values
#define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f) 
#define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f)
#define ACCEL_Z_OFFSET ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f)
#define ACCEL_X_SCALE (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET)) 
#define ACCEL_Y_SCALE (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET))
#define ACCEL_Z_SCALE (GRAVITY / (ACCEL_Z_MAX - ACCEL_Z_OFFSET))

#define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f)
#define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f)
#define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f)
#define MAGN_X_SCALE (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET))
#define MAGN_Y_SCALE (100.0f / (MAGN_Y_MAX - MAGN_Y_OFFSET))
#define MAGN_Z_SCALE (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET))

// Gain for gyroscope
#define GYRO_GAIN_X (0.06957f)
#define GYRO_GAIN_Y (0.06957f)
#define GYRO_GAIN_Z (0.06957f)

#define GYRO_X_SCALE (TO_RAD(GYRO_GAIN_X))
#define GYRO_Y_SCALE (TO_RAD(GYRO_GAIN_Y))
#define GYRO_Z_SCALE (TO_RAD(GYRO_GAIN_Z))

// DCM parameters
#define Kp_ROLLPITCH (0.02f)
#define Ki_ROLLPITCH (0.00002f)
#define Kp_YAW (1.2f)
#define Ki_YAW (0.00002f)

// Conversion Stuff 
#define GRAVITY (256.0f) // "1G reference" used for DCM filter and accelerometer calibration
#define TO_RAD(x) (x * 0.01745329252)  // *pi/180
#define TO_DEG(x) (x * 57.2957795131)  // *180/pi
#define TO_GRAVITY(x) (x/256.0f)          //scala il valore di gravità a 1g

// RAW sensor data
float accel[3];  // Actually stores the NEGATED acceleration (equals gravity, if board not moving).
//float accel_min[3];
//float accel_max[3];

float magnetom[3];
//float magnetom_min[3];
//float magnetom_max[3];

float gyro[3];
//float gyro_average[3];
//int gyro_num_samples = 0;

float temperature;
float pressure;
float altitude;


float Data[3];//Array contenente i dati da trasmettere via Radio

// DCM variables
float MAG_Heading;
float Magn_Vector[3]= {0, 0, 0}; // Store the magnetometer turn rate in a vector
float Accel_Vector[3]= {0, 0, 0}; // Store the acceleration in a vector
float Gyro_Vector[3]= {0, 0, 0}; // Store the gyros turn rate in a vector
float Omega_Vector[3]= {0, 0, 0}; // Corrected Gyro_Vector data
float Omega_P[3]= {0, 0, 0}; // Omega Proportional correction
float Omega_I[3]= {0, 0, 0}; // Omega Integrator
float Omega[3]= {0, 0, 0};
float errorRollPitch[3] = {0, 0, 0};
float errorYaw[3] = {0, 0, 0};
float DCM_Matrix[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
float Update_Matrix[3][3] = {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}};
float Temporary_Matrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};

// Euler angles
float yaw, pitch, roll;

// DCM timing in the main loop
long timestamp;
long timestamp_old;
float G_Dt; // Integration time for DCM algorithm

// More output-state variables
int num_accel_errors = 0;
int num_magn_errors = 0;
int num_gyro_errors = 0;

void ReadSensors() {
  Read_Pressure();
  Read_Gyro(); // Read gyroscope
  Read_Accel(); // Read accelerometer
  Read_Magn(); // Read magnetometer
  ApplySensorMapping();  
}

// Read every sensor and record a time stamp
// Init DCM with unfiltered orientation
// TODO re-init global vars?
void reset_sensor_fusion()
{
  float temp1[3];
  float temp2[3];
  float xAxis[] = {1.0f, 0.0f, 0.0f};

  ReadSensors();
  
  timestamp = millis();
  
  // GET PITCH
  // Using y-z-plane-component/x-component of gravity vector
  pitch = -atan2(Accel_Vector[0], sqrt(Accel_Vector[1] * Accel_Vector[1] + Accel_Vector[2] * Accel_Vector[2]));
	
  // GET ROLL
  // Compensate pitch of gravity vector 
  Vector_Cross_Product(temp1, Accel_Vector, xAxis);
  Vector_Cross_Product(temp2, xAxis, temp1);

  roll = atan2(temp2[1], temp2[2]);
  
  // GET YAW
  Compass_Heading();
  yaw = MAG_Heading;
  
  // Init rotation matrix
  init_rotation_matrix(DCM_Matrix, yaw, pitch, roll);
}

// Apply calibration to raw sensor readings
void ApplySensorMapping()
{
    // Magnetometer axis mapping
    Magn_Vector[1] = -magnetom[0];
    Magn_Vector[0] = -magnetom[1];
    Magn_Vector[2] = -magnetom[2];

    // Magnetometer values mapping
    Magn_Vector[0] -= MAGN_X_OFFSET;
    Magn_Vector[0] *= MAGN_X_SCALE;
    Magn_Vector[1] -= MAGN_Y_OFFSET;
    Magn_Vector[1] *= MAGN_Y_SCALE;
    Magn_Vector[2] -= MAGN_Z_OFFSET;
    Magn_Vector[2] *= MAGN_Z_SCALE;
  
    // Accelerometer axis mapping
    Accel_Vector[1] = accel[0];
    Accel_Vector[0] = accel[1];
    Accel_Vector[2] = accel[2];

    // Accelerometer values mapping
    Accel_Vector[0] -= ACCEL_X_OFFSET;
    Accel_Vector[0] *= ACCEL_X_SCALE;
    Accel_Vector[1] -= ACCEL_Y_OFFSET;
    Accel_Vector[1] *= ACCEL_Y_SCALE;
    Accel_Vector[2] -= ACCEL_Z_OFFSET;
    Accel_Vector[2] *= ACCEL_Z_SCALE;
    
    // Gyroscope axis mapping
    Gyro_Vector[1] = -gyro[0];
    Gyro_Vector[0] = -gyro[1];
    Gyro_Vector[2] = -gyro[2];

    // Gyroscope values mapping
    Gyro_Vector[0] -= GYRO_X_OFFSET;
    Gyro_Vector[0] *= GYRO_X_SCALE;
    Gyro_Vector[1] -= GYRO_Y_OFFSET;
    Gyro_Vector[1] *= GYRO_Y_SCALE;
    Gyro_Vector[2] -= GYRO_Z_OFFSET;
    Gyro_Vector[2] *= GYRO_Z_SCALE;
}


void setup()
{
  // Init serial output
  Serial.begin(OUTPUT_BAUD_RATE);
 // delay(100);
 radio.begin();
 radio.openWritingPipe(pipe);
  
  radio.setPALevel(RF24_PA_HIGH); //Setta la potenza della trasmissione al massimo
  radio.setDataRate(RF24_250KBPS);//  Setta la velocità di trasmissione a tot KBPS
//  radio.setDataRate(RF24_1MBPS);
//  radio.setDataRate(RF24_2MBPS);
  
  // optional
  radio.setRetries(15,15);
 // radio.setPayloadSize(8);
    radio.stopListening();
  // Init sensors
  delay(50);  // Give sensors enough time to start
  I2C_Init();
  Accel_Init();
  Magn_Init();
  Gyro_Init();
  Pressure_Init();
  
  // Read sensors, init DCM algorithm
  delay(20);  // Give sensors enough time to collect data
  reset_sensor_fusion();
}

// Main loop
void loop()
{
  // Time to read the sensors again?
  if ((millis() - timestamp) >= OUTPUT_DATA_INTERVAL) {
    timestamp_old = timestamp;
    timestamp = millis();
    if (timestamp > timestamp_old)
      G_Dt = (float) (timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
    else
      G_Dt = 0;

    ReadSensors();
    
    // Run DCM algorithm
    Compass_Heading(); // Calculate magnetic heading

    Matrix_update();
    Normalize();
    Drift_correction();
    Euler_angles();
    
 
    
     Data[0] = (TO_DEG(yaw));    //Yaw              
     Data[1] = (TO_DEG(pitch));  //Pitch                   
     Data[2] = (TO_DEG(roll));   //Roll         
   
     Serial.print(sizeof(Data));


bool ok = radio.write( &Data, sizeof(Data));

      if (ok)
        Serial.println("ok\n\r");
      else
        Serial.println("failed\n\r");

    
  }
}