Qt class to use neo sensors

neosensorsregisters.h

#ifndef NEOSENSORSREGISTERS_H
#define NEOSENSORSREGISTERS_H

//This file contains all the registers used by fxos8700cq and fxas2100c"""

#define GYRO                 0x01
#define ACC                  0x02
#define MAG                  0x03

#define I2C_AM_ADDRESS 0x1e	// I2C accelerometer/magnetometer address
#define I2C_G_ADDRESS  0x20  // I2C gyroscope address

// ACCELEROMETER REGISTERS
// Device Configuration
#define A_STATUS			 0x00	// Data ready status or FIFO status configuration regsiter
#define A_TRIG_CFG			 0X0a	// FIFO trigger configuration register
#define A_SYSMOD			 0x0b	// Current device operating mode register
#define A_INT_SOURCE		 0x0c
#define A_WHO_AM_I			 0x0d
#define A_CTRL_REG1			 0x2a 	// standby/active,autowake,output_data_rate,lnoise,fast_read mode accelerometer register
#define A_CTRL_REG2			 0x2b	// st_bit,rst_bit,Accelerometer Sleep mode,OSR mode selection,Auto-sleep mode,Accelerometer Wake mode OSR mode selection
#define A_CTRL_REG3			 0x2c	// Interrupt control register
#define A_CTRL_REG4			 0x2d	// Interrupt enable register
#define A_CTRL_REG5			 0x2e	// Interrupt routing configuration register

// Auto-Sleep trigger
#define A_ASPL_COUNT		 0x29	// ASPL_COUNT register: sets the minimum time period of event flag inactivity required to initiate a change from the current active mode ODR value specified in CTRL_REG1[dr] to the Sleep mode ODR value specified in CTRL_REG1[aslp_rate], provided that CTRL_REG2[slpe]  1

// Temperature
#define A_TEMP 				 0X51

// Accelerometer output data registers
#define A_OUT_X_MSB 		 0x01 	// out_x_msb accelerometer register
#define A_OUT_X_LSB 		 0x02 	// out_x_lsb accelerometer register
#define A_OUT_Y_MSB 		 0x03 	// out_y_msb accelerometer register
#define A_OUT_Y_LSB 		 0x04 	// out_y_lsb accelerometer register
#define A_OUT_Z_MSB 		 0x05 	// out_z_msb accelerometer register
#define A_OUT_Z_LSB 		 0x06 	// out_z_lsb accelerometer register

// Accelerometer FIFO
#define A_F_SETUP			 0x09	// FIFO buffer operating mode, FIFO sample count watermark register configuration

// Accelerometer sensor data configuration
#define A_XYZ_DATA_CFG		 0x0e  // accelerometer full scale range register

// Accelerometer High-Pass filter
#define A_HP_FILTER_CUTOFF	 0x0f  // High-pass filter cutoff frequency settings register

// Portrait/Landscape detection
#define A_PL_STATUS			 0x10
#define A_PL_CFG		     0x11	// Portrait/Landscape configuration register
#define A_PL_COUNT			 0x12	// debounce count settings register
#define A_PL_BF_ZCOMP		 0x13
#define A_PL_THS_REG		 0x14

// Freefall motion detection
#define A_FFMT_CFG			 0x15	// Freefall/motion configuration register
#define A_FFMT_SRC			 0x16
#define A_FFMT_THS 			 0x17
#define A_FFMT_THS_X_MSB	 0x73
#define A_FFMT_THS_X_LSB	 0x74
#define A_FFMT_THS_Y_MSB	 0x75
#define A_FFMT_THS_Y_LSB	 0x76
#define A_FFMT_THS_Z_MSB	 0x77
#define A_FFMT_THS_Z_LSB	 0x78
#define A_FFMT_COUNT		 0x18

// Accelerometer vector-magnitude function
#define A_VECM_CFG			 0x5f	// latch enable mode, set initial reference values, reference values updating mode, accelerometer vector-magnitude function enable/disable
#define A_VECM_THS_MSB		 0x60
#define A_VECM_THS_LSB		 0x61
#define A_VECM_CNT 			 0x62
#define A_VECM_INITX_MSB	 0x63
#define A_VECM_INITX_LSB	 0x64
#define A_VECM_INITY_MSB	 0x65
#define A_VECM_INITY_LSB	 0x66
#define A_VECM_INITZ_MSB	 0x67
#define A_VECM_INITZ_LSB	 0x68


// Transient acceleration detection
#define A_TRANSIENT_CFG		 0x1d	// Transient event flag latch enable, Z-axis transient event flag enable, Y-axis transient event flag enable, X-axis transient event flag enable, Transient function high-pass filter bypass enable
#define A_TRANSIENT_SRC		 0x1e
#define A_TRANSIENT_THS		 0x1f
#define A_TRANSIENT_COUNT	 0x20

// Pulse detection
#define A_PULSE_CFG			 0x21	// pulse event detection function configuration register
#define A_PULSE_SRC			 0x22
#define A_PULSE_THSX		 0x23
#define A_PULSE_THSY		 0x24
#define A_PULSE_THSZ		 0x25
#define A_PULSE_TMLT		 0x26
#define A_PULSE_LTCY		 0x27
#define A_PULSE_WIND		 0x28

// accelerometer offset correction
#define A_OFF_X				 0x2f
#define A_OFF_Y				 0x30
#define A_OFF_Z				 0x31

// GYROSCOPE REGITERS
#define G_STATUS 			 0x00 // easy reading of the relevant status register or the first sample stored in the FIFO
#define G_DR_STATUS			 0x07 // sample data acquisition status, reflects the real-time updates to the OUT registers
#define G_F_STATUS			 0x08 // when FIFO is enabled, indicates the current status of the FIFO
#define G_F_SETUP			 0x09 // FIFO configurations
#define G_F_EVENT			 0x0A // used to monitor the FIFO event status
#define G_INT_SOURCE_FLAGS	 0x0b // provides the event-flag status for the interrupt generating functions within the device
#define G_WHO_AM_I			 0x0c // contains the device identifier which is factory programmed to 0xD7
#define G_CTRL_REG0			 0x0d // used for general control and configuration of the device
#define G_RT_CFG			 0x0e // enables the Rate Threshold interrupt generation
#define G_RT_SRC			 0x0f // indicates the source of the Rate Threshold event
#define G_RT_THS			 0x10 //  sets the threshold limit for the detection of the rate and the debounce counter mode
#define G_RT_COUNT			 0x11 // sets the number of debounce counts
#define G_TEMP 				 0x12 // contains an 8-bit 2's complement temperature value with a range of –128 °C to +127 °C and a scaling of 1 °C/LSB
#define G_CTRL_REG1 		 0x13 // configures the device ODR
#define G_CTRL_REG2   		 0x14 // enables and assigns the output pin(s) and logic polarities for the various interrupt sources
#define G_CTRL_REG3 		 0x15 // enables FSR expansion, ext. power control input, options to modify the auto-increment read address pointer behavior when doing burst reads of the FIFO data
#define G_OUT_X_MSB 		 0x01 // g_out_x_msb gyroscope register
#define G_OUT_X_LSB 		 0x02 // g_out_x_lsb gyroscope register
#define G_OUT_Y_MSB 		 0x03 // g_out_y_msb gyroscope register
#define G_OUT_Y_LSB 		 0x04 // g_out_y_lsb gyroscope register
#define G_OUT_Z_MSB 		 0x05 // g_out_z_msb gyroscope register
#define G_OUT_Z_LSB 		 0x06 // g_out_z_lsb gyroscope register

// Gyroscope mode and configuartion

#define G_ACTIVE             0x02
#define G_DISACTIVE          0x00
#define G_READY              0x01

#define G_800                0x00
#define G_400                0x04
#define G_200                0x08
#define G_100                0x0C
#define G_50                 0x10
#define G_25                 0x14
#define G_12                 0x18

// acc mode and config
#define A_ACTIVE             0x01
#define A_DISACTIVE          0x00
#define A_FAST_MODE          0x02 //in fast mode the resolution is downgrade to 8 bits, normal mode let 0
#define A_NOISE_ON           0x04 //limit the scale of acc to 2 or 4 g --> 8g not possible

//acc odr in hybride mode
#define A_HYB_1              0x38
#define A_HYB_3              0x30
#define A_HYB_6              0x28
#define A_HYB_25             0x20
#define A_HYB_50             0x18
#define A_HYB_100            0x10
#define A_HYB_200            0x08
#define A_HYB_400            0x00


// MAGNETOMETER REGISTERS
// Magnetometer data registers
#define M_DR_STATUS		 	 0x32	// Magnetic data-ready status register
#define M_OUT_X_MSB		 	 0x33
#define M_OUT_X_LSB 	 	 0x34
#define M_OUT_Y_MSB 	 	 0x35
#define M_OUT_Y_LSB 	 	 0x36
#define M_OUT_Z_MSB 	 	 0x37
#define M_OUT_Z_LSB 	 	 0x38
#define M_CMP_X_MSB		 	 0x39
#define M_CMP_X_LSB 	 	 0x3a
#define M_CMP_Y_MSB		 	 0x3b
#define M_CMP_Y_LSB		 	 0x3c
#define M_CMP_Z_MSB		 	 0x3d
#define M_CMP_Z_LSB		 	 0x3e
#define M_MAX_X_MSB		 	 0x45
#define M_MAX_X_LSB		 	 0x46
#define M_MAX_Y_MSB		 	 0x47
#define M_MAX_Y_LSB		 	 0x48
#define M_MAX_Z_MSB		 	 0x49
#define M_MAX_Z_LSB		 	 0x4a
#define M_MIN_X_MSB		 	 0x4b
#define M_MIN_X_LSB		 	 0x4c
#define M_MIN_Y_MSB		 	 0x4d
#define M_MIN_Y_LSB		 	 0x4e
#define M_MIN_Z_MSB		 	 0x4f
#define M_MIN_Z_LSB		 	 0x50

// Magnetometer offset correction
#define M_OFF_X_MSB 	 	 0x3f
#define M_OFF_X_LSB 	 	 0x40
#define M_OFF_Y_MSB 	 	 0x41
#define M_OFF_Y_LSB 	 	 0x42
#define M_OFF_Z_MSB 	 	 0x43
#define M_OFF_Z_LSB 	 	 0x44

// Magnetometer threshold function
#define M_THS_CFG		 	 0x52	// Magnetic-field threshold detection configuration register
#define M_THS_SRC		 	 0x53
#define M_THS_X_MSB 	 	 0x54
#define M_THS_X_LSB 	 	 0x55
#define M_THS_Y_MSB 	 	 0x56
#define M_THS_Y_LSB 	 	 0x57
#define M_THS_Z_MSB 	 	 0x58
#define M_THS_Z_LSB 	 	 0x59
#define M_THS_COUNT 	 	 0x5a

// Magnetometer control registers
#define M_CTRL_REG1 	 	 0x5b	// Auto-Calibration, One-shot reset register
#define M_CTRL_REG2 	 	 0x5c  // min/MAX detection config, sensor reset frequency register
#define M_CTRL_REG3 	 	 0x5d	// measur. RAW mode, OSR in Auto-sleep, self-test configuration register
#define M_INT_SRC		 	 0x5e

// Magnetometer vector-magnitude function
#define M_VECM_CFG		 	 0x69	// vector-magnitude configuration register
#define M_VECM_THS_MSB	 	 0x6a
#define M_VECM_THS_LSB	 	 0x6b
#define M_VECM_CNT		 	 0x6c
#define M_VECM_INITX_MSB 	 0x6d
#define M_VECM_INITX_LSB 	 0x6e
#define M_VECM_INITY_MSB 	 0x6f
#define M_VECM_INITY_LSB 	 0x70
#define M_VECM_INITZ_MSB 	 0x71
#define M_VECM_INITZ_LSB 	 0x72

//math var
#define PI                   3.14159f

#endif // NEOSENSORSREGISTERS_H

qNeoSensors.cpp

#include "qNeoSensors.h"
#include <linux/i2c-dev-user.h> //for i2c
#include <sys/ioctl.h> //to use in/out connections
#include <fcntl.h> //for files manipulations
#include <unistd.h> //for sleep fonction
#include <QDebug>

//pragma to remove  warning
//#pragma GCC diagnostic ignored "-Wwrite-strings"

QNeoSenors::QNeoSenors(QObject *parent) : QObject(parent)
{
    time=new QTime;
    time->start();

    if ((fileSensor = open(i2cDev.toLocal8Bit(), O_RDWR)) < 0) {
        /* ERROR HANDLING: you can check errno to see what went wrong */
        perror("Failed to open the i2c bus");

    }

}


bool QNeoSenors::initGyroI2c(const int scaleRange, const int fsDouble, const int odr)
{
    gyroScale=scaleRange;
    gyroRange=fsDouble;

    uint8_t data=0;

//we need to kill the gyro i2c driver before to use it ----------------------------
    QProcess sh;
    sh.start("sh");
    sh.write("lsmod | grep fxas2100x");
    sh.closeWriteChannel();
    sh.waitForFinished();
    QByteArray output = sh.readAll();
    qDebug(output);
    sh.close();

    if(output.contains("fxas2100x")) //is driver on
    {
        qDebug("driver gyro on");

        sh.start("sh");
        sh.write("rmmod fxas2100x");
        sh.closeWriteChannel();
        sh.waitForFinished();
        sh.close();

        sh.start("sh");
        sh.write("lsmod | grep fxas2100x");
        sh.closeWriteChannel();
        sh.waitForFinished();
        output = sh.readAll();
        qDebug(output);
        sh.close();

        if(!output.contains("fxas2100x"))
            qDebug("driver gyro killed");
        else
        {
            qDebug("killing gyro driver error");
            return false;
        }
    }

    if (ioctl(fileSensor, I2C_SLAVE, I2C_G_ADDRESS) < 0) {
        fprintf( stderr, "Failed to set slave address: %m\n" );
        /* ERROR HANDLING; you can check errno to see what went wrong */
        return false;
    }

//now we went to configure the gyro -----------------------------------------------

    if(gyroRange==0) //fsDouble: enables (with value 1) or disables (with value 0) the fsdouble function to double the scale range of the gyroscope
        data=0x00;
    else
        data=0x01;

    if( i2c_smbus_write_byte_data(fileSensor, G_CTRL_REG3,data ) < 0 )
    {
        fprintf( stderr, "Failed to write full scale conf to I2C device: %m\n" );
        return false;
    }

    //ScaleRange : represents the range of the gyroscope. It can assume the following values: 250,500,1000,2000 dps.
    switch (gyroScale) {
    case 250:
        data=0x03;
        break;
    case 500:
        data=0x02;
        break;
    case 1000:
        data=0x01;
        break;
    case 2000:
        data=0x00;
        break;
    default:
        data=0x02;
        break;
    }

    if( i2c_smbus_write_byte_data(fileSensor, G_CTRL_REG0,data ) < 0 )
    {
        fprintf( stderr, "Failed to write the scale conf to I2C device: %m\n" );
        return false;
    }

    //configuration of ODR

    switch (odr) {
    case 12:
        data=G_12;
        break;
    case 25:
        data=G_25;
        break;
    case 50:
        data=G_50;
        break;
    case 100:
        data=G_100;
        break;
    case 200:
        data=G_200;
        break;
    case 400:
        data=G_400;
        break;
    case 800:
        data=G_800;
        break;
    default:
        data=G_200;
        break;
    }

    if( i2c_smbus_write_byte_data(fileSensor, G_CTRL_REG1,data ) < 0 )
    {
        fprintf( stderr, "Failed to write the odr conf to I2C device: %m\n" );
        return false;
    }

    //activation of FIFO circular buffer to allow a simple reading with i2c
    data=0x01; //0x01 for circular buffer, 0x10 for stop buffer and 0x00 disable fifo

    if( i2c_smbus_write_byte_data(fileSensor, G_F_SETUP,data ) < 0 )
    {
        fprintf( stderr, "Failed to write the gyro FIFO conf to I2C device: %m\n" );
        return false;
    }

    //we active gyro now

    setGyroActive(true);

    return true;
}

bool QNeoSenors::initAcc(const int scaleRange, const bool noise, const bool hybrid, const int odr)
{
    uint8_t data=0,regNoise=0;
    accScale=scaleRange;
//we need to kill the acc i2c driver before to use it ----------------------------
    QProcess sh;
    sh.start("sh");
    sh.write("lsmod | grep fxos8700");
    sh.closeWriteChannel();
    sh.waitForFinished();
    QByteArray output = sh.readAll();
    qDebug(output);
    sh.close();

    if(output.contains("fxos8700")) //is driver on
    {
        qDebug("driver acc on");

        sh.start("sh");
        sh.write("rmmod fxos8700");
        sh.closeWriteChannel();
        sh.waitForFinished();
        sh.close();

        sh.start("sh");
        sh.write("lsmod | grep fxos8700");
        sh.closeWriteChannel();
        sh.waitForFinished();
        output = sh.readAll();
        qDebug(output);
        sh.close();

        if(!output.contains("fxos8700"))
            qDebug("driver acc killed");
        else
        {
            qDebug("killing acc driver error");
            return false;
        }
    }

    if (ioctl(fileSensor, I2C_SLAVE, I2C_AM_ADDRESS) < 0) {
        fprintf( stderr, "Failed to set slave address: %m\n" );
        /* ERROR HANDLING; you can check errno to see what went wrong */
        return false;
    }

//now we went to configure the acc -----------------------------------------------
    // write 0000 0000 = 0x00 to accelerometer control register 1 to place FXOS8700CQ into standby

    if( i2c_smbus_write_byte_data(fileSensor, A_CTRL_REG1,0 ) < 0 )
    {
        fprintf( stderr, "Failed to write the odr conf to I2C device: %m\n" );
        return false;
    }

    //ScaleRange : represents the range of the accelerometer. In can assume 2 for +/- 2g, 4 for +/- 4g or 8 for +/- 8g
    switch (accScale) {
    case 2:
        data=0x00;
        break;
    case 4:
        data=0x01;
        break;
    case 8:
        data=0x02;
        break;
    default:
        data=0x00;
        break;
    }

    if( i2c_smbus_write_byte_data(fileSensor, A_XYZ_DATA_CFG,data ) < 0 )
    {
        fprintf( stderr, "Failed to write the scale conf to I2C device: %m\n" );
        return false;
    }

    if(noise)
        regNoise=A_NOISE_ON;
    else
        regNoise=0x00;

    //configuration of ODR

    switch (odr) {
    case 1:
        data=A_HYB_1;
        break;
    case 3:
        data=A_HYB_3;
        break;
    case 6:
        data=A_HYB_6;
        break;
    case 25:
        data=A_HYB_25;
        break;
    case 50:
        data=A_HYB_50;
        break;
    case 100:
        data=A_HYB_100;
        break;
    case 200:
        data=A_HYB_200;
        break;
    case 400:
        data=A_HYB_400;
        break;
    default:
        data=A_HYB_200;
        break;
    }

    if( i2c_smbus_write_byte_data(fileSensor, A_CTRL_REG1,data | regNoise ) < 0 )
    {
        fprintf( stderr, "Failed to write the odr conf to I2C device: %m\n" );
        return false;
    }

    if(hybrid)
    {
        // write 0001 1111 = 0x1F to magnetometer control register 1
        // [7]: m_acal=0: auto calibration disabled
        // [6]: m_rst=0: no one-shot magnetic reset
        // [5]: m_ost=0: no one-shot magnetic measurement
        // [4-2]: m_os=111=7: 8x oversampling (for 200Hz) to reduce magnetometer noise
        // [1-0]: m_hms=11=3: select hybrid mode with accel and magnetometer active

        data=0x1F;// enable both accelerometer and magnetometer sensors
    }
    else
        data=0x1C;

    if( i2c_smbus_write_byte_data(fileSensor, M_CTRL_REG1,data ) < 0 )
    {
        fprintf( stderr, "Failed to write the odr conf to I2C device: %m\n" );
        return false;
    }

    // write 0010 0000 = 0x20 to magnetometer control register 2
    // [7]: reserved
    // [6]: reserved
    // [5]: hyb_autoinc_mode=1 to map the magnetometer registers to follow the
    // accelerometer registers
    // [4]: m_maxmin_dis=0 to retain default min/max latching even though not used
    // [3]: m_maxmin_dis_ths=0
    // [2]: m_maxmin_rst=0
    // [1-0]: m_rst_cnt=00 to enable magnetic reset each cycle

    data=0x20;

    if( i2c_smbus_write_byte_data(fileSensor, M_CTRL_REG2,data ) < 0 )
    {
        fprintf( stderr, "Failed to write the odr conf to I2C device: %m\n" );
        return false;
    }

    //we active acc now

    setAccActive(true);

    return true;
}

bool QNeoSenors::setAccActive(const bool active)
{
    if (ioctl(fileSensor, I2C_SLAVE, I2C_AM_ADDRESS) < 0) {
        fprintf( stderr, "Failed to set slave address: %m\n" );
        /* ERROR HANDLING; you can check errno to see what went wrong */
        return false;
    }

    uint8_t currentReg=0;
    //get the current register conf
    currentReg = i2c_smbus_read_byte_data(fileSensor,A_CTRL_REG1);
    qDebug()<<"current reg acc : "<<currentReg;
    currentReg = currentReg >> 1;
    currentReg = currentReg << 1;
    qDebug()<<"current reg acc : "<<currentReg;
    //set acc to active mode
    if(active)
    {
        if( i2c_smbus_write_byte_data(fileSensor, A_CTRL_REG1,currentReg | A_ACTIVE) < 0 )
        {
            fprintf( stderr, "Failed to write the activation of Gyro to I2C device: %m\n" );
            return false;
        }
        usleep(300000); //sleep 300 ms
    }
    else
    {
        if( i2c_smbus_write_byte_data(fileSensor, A_CTRL_REG1,currentReg | A_DISACTIVE) < 0 )
        {
            fprintf( stderr, "Failed to write the activation of Gyro to I2C device: %m\n" );
            return false;
        }
        usleep(300000); //sleep 300 ms
    }


    return true;
}

bool QNeoSenors::setGyroActive(const bool active)
{
    if (ioctl(fileSensor, I2C_SLAVE, I2C_G_ADDRESS) < 0) {
        fprintf( stderr, "Failed to set slave address: %m\n" );
        /* ERROR HANDLING; you can check errno to see what went wrong */
        return false;
    }
    uint8_t currentReg=0;
    //get the current register conf
    currentReg = i2c_smbus_read_byte_data(fileSensor,G_CTRL_REG1);
    qDebug()<<"current reg gyro : "<<currentReg;
    currentReg = currentReg >> 2;
    currentReg = currentReg << 2;
    qDebug()<<"current reg gyro : "<<currentReg;

    //set gyro to active mode
    if(active)
    {
        if( i2c_smbus_write_byte_data(fileSensor, G_CTRL_REG1,currentReg | G_ACTIVE) < 0 )
        {
            fprintf( stderr, "Failed to write the activation of Gyro to I2C device: %m\n" );
            return false;
        }
        usleep(300000); //sleep 300 ms
    }
    else
    {
        if( i2c_smbus_write_byte_data(fileSensor, G_CTRL_REG1,currentReg | G_READY) < 0 )
        {
            fprintf( stderr, "Failed to write the activation of Gyro to I2C device: %m\n" );
            return false;
        }
        usleep(300000); //sleep 300 ms
    }


    return true;
}

void QNeoSenors::calibrateSens(int samples)
{
    calib=false;
    calibTable[0]=0;
    calibTable[1]=0;
    calibTable[2]=0;
    calibTable[3]=0;
    calibTable[4]=0;
    calibTable[5]=0;

    if(!samples)
        samples=1;
    //gyro var
    int gyroSumX=0,gyroSumY=0,gyroSumZ=0,progress=0,gyroXmoy=0,gyroYmoy=0,gyroZmoy=0;

    //acc var
    int accSumX=0,accSumY=0,accSumZ=0,accXmoy=0,accYmoy=0,accZmoy=0;

    float factor = 0;

    if(accScale==2)
        factor=4;
    else if(accScale==4)
        factor=8;
    else
        factor=16;

    for(int i=0;i<samples;i++)
    {
        readDataGyro();
        gyroSumX += gyroRaw[0];
        gyroSumY += gyroRaw[1];
        gyroSumZ += gyroRaw[2];

        readDataAcc();
        accSumX += accRaw[0];
        accSumY += accRaw[1];
        accSumZ += accRaw[2]+ 16384/factor;

        progress = (i*100)/samples;
        /*if(progress != 100)
            qDebug()<<"calibration running : "<<progress<<"%";*/
        emit calibrateState(progress);
    }
    progress++;
    emit calibrateState(progress);
    //qDebug()<<"calibration running : 100% ";

    gyroXmoy = gyroSumX/samples;
    gyroYmoy = gyroSumY/samples;
    gyroZmoy = gyroSumZ/samples;

    accXmoy = accSumX/samples;
    accYmoy = accSumY/samples;
    accZmoy = accSumZ/samples;

    calibTable[0]=gyroXmoy;
    calibTable[1]=gyroYmoy;
    calibTable[2]=gyroZmoy;
    calibTable[3]=accXmoy;
    calibTable[4]=accYmoy;
    calibTable[5]=accZmoy;

    calib=true;

}

float* QNeoSenors::readDataGyro()
{
    if (ioctl(fileSensor, I2C_SLAVE, I2C_G_ADDRESS) < 0) {
        fprintf( stderr, "Failed to set slave address: %m\n" );
        /* ERROR HANDLING; you can check errno to see what went wrong */
    }

    uint8_t Buffer[6]={0};

    i2c_smbus_read_i2c_block_data(fileSensor,G_OUT_X_MSB,6,Buffer);
    gyroRaw[0] = ((Buffer[0] << 8) | Buffer[1])-calibTable[0];
    gyroRaw[1] = ((Buffer[2] << 8) | Buffer[3])-calibTable[1];
    gyroRaw[2] = ((Buffer[4] << 8) | Buffer[5])-calibTable[2];

    if(calib)
    {
        convertData(GYRO);
        filterData();
    }

    for(int i=0;i<3;i++)
    {
        gyroData[i]=gyro[i];
        gyroData[i+3]=gyroFiltered[i];
    }

    return gyroData;
}

float* QNeoSenors::readDataAcc()
{
    if (ioctl(fileSensor, I2C_SLAVE, I2C_AM_ADDRESS) < 0) {
        fprintf( stderr, "Failed to set slave address: %m\n" );
        /* ERROR HANDLING; you can check errno to see what went wrong */
    }

    uint8_t Buffer[13]={0};

    //we read all acc and mag data in a single read

    i2c_smbus_read_i2c_block_data(fileSensor,A_STATUS,13,Buffer);

    //acc data
    accRaw[0] = ((int16_t)(((Buffer[1] << 8) | Buffer[2]))>> 2)-calibTable[3];
    accRaw[1] = ((int16_t)(((Buffer[3] << 8) | Buffer[4]))>> 2)-calibTable[4];
    accRaw[2] = ((int16_t)(((Buffer[5] << 8) | Buffer[6]))>> 2)-calibTable[5];

    //mag data

    magRaw[0] = (Buffer[7] << 8) | Buffer[8];
    magRaw[1] = (Buffer[9] << 8) | Buffer[10];
    magRaw[2] = (Buffer[11] << 8) | Buffer[12];

    if(calib)
    {
        convertData(ACC);
        convertData(MAG);
        filterData();
    }

    for(int i=0;i<3;i++)
    {
        //acc
        accData[i]=acc[i];
        accData[i+3]=accFiltered[i];
    }

    return accData;
}

float * QNeoSenors::getData(const int ms, bool gyro, bool acc)
{
    cycle=ms;
    if(gyro)
        readDataGyro();
    if(acc)
        readDataAcc();
    fillDataTable();
    return data;
}

void QNeoSenors::convertData(const int sensor)
{
    if(sensor==GYRO)
    {
        uint8_t ctrlDouble=0;

        if(gyroRange)
            ctrlDouble = 2;
        else
            ctrlDouble = 1;

        if(!gyroScale) // last two bits are setted to 0b00 (+/- 2000/4000 mode)
            gyroSensivity = (62.5 * ctrlDouble) / 1000;
        else if(gyroScale == 1) // last two bits are setted to 0b01 (+/- 1000/2000 mode)
            gyroSensivity = (31.25 * ctrlDouble) / 1000;
        else if(gyroScale == 2) // last two bits are setted to 0b10 (+/- 500/1000 mode)
            gyroSensivity = (15.625 * ctrlDouble) / 1000;
        else // last two bits are setted to 0b11 (+/- 250/500 mode)
            gyroSensivity = (7.8125 * ctrlDouble) / 1000;

        for (int i=0;i<3;i++)
        gyro[i] = gyroRaw[i]*gyroSensivity;
    }

    if(sensor==ACC)
    {
        uint8_t factor=0;
        float x2=0,y2=0,z2=0;
        //float fNormAcc = 0.0f,fSinRoll = 0.0f,fCosRoll = 0.0f,fSinPitch = 0.0f,fCosPitch = 0.0f, RollAng = 0.0f, PitchAng = 0.0f;

        if(accScale==2) // last two bits are setted to 0b00 (2g mode) sensitivity = 4096
            factor=1;
        else if(accScale==4) // last two bits are setted to 0b01 (4g mode) sensitivity = 2048
            factor=2;
        else  // last two bits are setted to 0b10 (8g mode) sensitivity = 1024
            factor=4;

        accSensivity = (0.244f*factor);

        for (int i=0;i<3;i++)
        {
            acc[i] = ((accRaw[i])*accSensivity)/1000;
        }

        x2= pow(acc[0],2);
        y2= pow(acc[1],2);
        z2= pow(acc[2],2);

        angle[0] = atan(acc[1]/sqrt(x2+z2))*(180/PI);
        angle[1] = atan(acc[0]/sqrt(y2+z2))*(180/PI);
        angle[2] = atan(acc[2]/sqrt(x2+y2))*(180/PI)*(-1);

       /*fNormAcc = sqrt((acc[0]*acc[0])+(acc[1]*acc[1])+(acc[2]*acc[2]));

       fSinRoll = -acc[1]/fNormAcc;
       fCosRoll = sqrt(1.0-(fSinRoll * fSinRoll));
       fSinPitch = acc[0]/fNormAcc;
       fCosPitch = sqrt(1.0-(fSinPitch * fSinPitch));

      if ( fSinRoll >0)
      {
        if (fCosRoll>0)
        {
          RollAng = acos(fCosRoll)*180/PI;
        }
        else
        {
          RollAng = acos(fCosRoll)*180/PI + 180;
        }
      }
      else
      {
        if (fCosRoll>0)
        {
          RollAng = acos(fCosRoll)*180/PI + 360;
        }
        else
        {
          RollAng = acos(fCosRoll)*180/PI + 180;
        }
      }

       if ( fSinPitch >0)
      {
        if (fCosPitch>0)
        {
             PitchAng = acos(fCosPitch)*180/PI;
        }
        else
        {
           PitchAng = acos(fCosPitch)*180/PI + 180;
        }
      }
      else
      {
        if (fCosPitch>0)
        {
             PitchAng = acos(fCosPitch)*180/PI + 360;
        }
        else
        {
           PitchAng = acos(fCosPitch)*180/PI + 180;
        }
      }

       if (RollAng >=360)
       {
         RollAng = RollAng - 360;
       }

       if (PitchAng >=360)
       {
         PitchAng = PitchAng - 360;
       }

       qDebug()<<"x angle : "<<PitchAng;
       qDebug()<<"y angle : "<<RollAng;

       fTiltedX = MagBuffer[0]*fCosPitch+MagBuffer[2]*fSinPitch;
       fTiltedY = MagBuffer[0]*fSinRoll*fSinPitch+MagBuffer[1]*fCosRoll-MagBuffer[1]*fSinRoll*fCosPitch;

       HeadingValue = (float) ((atan2f((float)fTiltedY,(float)fTiltedX))*180)/PI;*/
    }

    if(sensor==MAG)
    {

        magSensivity = 0.1;

        for (int i=0;i<3;i++)
        mag[i] = magRaw[i]*magSensivity;
    }
}

void QNeoSenors::filterData()
{
    //low pass filter first order
    currentTime=time->elapsed();
    //qDebug()<<"current time : "<<currentTime;
    //qDebug()<<"current time : "<<oldTime;
    int dt=0;
    if(cycle!=0)
        dt = cycle;
    else
        dt=1;
    //qDebug()<<"temps de cycle : "<<(currentTime-oldTime);
    oldTime=currentTime;

    float k =  dt/ (float)(ti+dt/2);
    for(int i=0;i<3;i++)
    {
        //gyro -------------------------------------------------------------------------------
        gyroFiltered[i]=gyroFilteredOld[i] + k*(0.5f*(gyro[i]+gyroOld[i])-gyroFilteredOld[i]);
        gyroOld[i]=gyro[i];
        gyroFilteredOld[i]=gyroFiltered[i];

        //acc --------------------------------------------------------------------------------
        accFiltered[i]=accFilteredOld[i] + k*(0.5f*(acc[i]+accOld[i])-accFilteredOld[i]);
        accOld[i]=acc[i];
        accFilteredOld[i]=accFiltered[i];

        //mag --------------------------------------------------------------------------------
        magFiltered[i]=magFilteredOld[i] + k*(0.5f*(mag[i]+magOld[i])-magFilteredOld[i]);
        magOld[i]=mag[i];
        magFilteredOld[i]=magFiltered[i];

        //angle ------------------------------------------------------------------------------
        angleFiltered[i]=angleFilteredOld[i] + k*(0.5f*(angle[i]+angleOld[i])-angleFilteredOld[i]);
        angleOld[i]=angle[i];
        angleFilteredOld[i]=angleFiltered[i];
    }

    //qDebug()<</*"acc X : "<<acc[0]<<*/"gyro X filetered : "<<gyroFiltered[0] ;
    //qDebug()<</*"acc Y : "<<acc[1]<<*/"gyro Y filetered : "<<gyroFiltered[1];
    //qDebug()<</*"acc Z : "<<acc[2]<<*/"gyro Z filetered : "<<gyroFiltered[2];

    //qDebug()<<endl;

    //qDebug()<</*"acc X : "<<acc[0]<<*/"acc X filetered : "<<accFiltered[0] ;
    //qDebug()<</*"acc Y : "<<acc[1]<<*/"acc Y filetered : "<<accFiltered[1];
    //qDebug()<</*"acc Z : "<<acc[2]<<*/"acc Z filetered : "<<accFiltered[2];

    //qDebug()<<endl;

    //qDebug()<</*"angle X : "<<angle[0]<<*/"angle X filetered : "<<angleFiltered[0] ;
    //qDebug()<</*"angle Y : "<<angle[1]<<*/"angle Y filetered : "<<angleFiltered[1];
    //qDebug()<</*"angle Z : "<<angle[2]<<*/"angle Z filetered : "<<angleFiltered[2];

    //qDebug()<<endl;

    //qDebug()<</*"mag X : "<<mag[0]<<*/"mag X filetered : "<<magFiltered[0] ;
    //qDebug()<</*"mag Y : "<<mag[1]<<*/"mag Y filetered : "<<magFiltered[1];
    //qDebug()<</*"mag Z : "<<mag[2]<<*/"mag Z filetered : "<<magFiltered[2];

    //qDebug()<<endl;

}

void QNeoSenors::setTi(const int filterTi)
{
    ti=filterTi;
}

int QNeoSenors::readTempGyro()
{
    if (ioctl(fileSensor, I2C_SLAVE, I2C_G_ADDRESS) < 0) {
        fprintf( stderr, "Failed to set slave address: %m\n" );
        /* ERROR HANDLING; you can check errno to see what went wrong */
        return false;
    }
    temp = i2c_smbus_read_byte_data(fileSensor,G_TEMP);

    if(temp >= 128)
        return (temp-256);
    else
        return temp;
}

void QNeoSenors::fillDataTable()
{
    data[0]=gyro[0];
    data[1]=gyro[1];
    data[2]=gyro[2];
    data[3]=gyroFiltered[0];
    data[4]=gyroFiltered[1];
    data[5]=gyroFiltered[2];
    data[6]=acc[0];
    data[7]=acc[1];
    data[8]=acc[2];
    data[9]=accFiltered[0];
    data[10]=accFiltered[1];
    data[11]=accFiltered[2];
    data[12]=mag[0];
    data[13]=mag[1];
    data[14]=mag[2];
    data[15]=magFiltered[0];
    data[16]=magFiltered[1];
    data[17]=magFiltered[2];
    data[18]=angle[0];
    data[19]=angle[1];
    data[20]=angle[2];
    data[21]=angleFiltered[0];
    data[22]=angleFiltered[1];
    data[23]=angleFiltered[2];
}

qNeoSensors.h

#ifndef QNEOSENSORS_H
#define QNEOSENSORS_H

#include <QObject>
#include <QFile>
#include <QFileInfo>
#include "neosensorsregisters.h"
#include <QProcess>
#include <QTime>
#include <QTimer>

class QNeoSenors : public QObject
{
    Q_OBJECT
public:
    explicit QNeoSenors(QObject *parent = 0);

signals:
    void calibrateState(const int state);

public slots:
    //gyro -----------------------------------------------
    bool initGyroI2c(const int scaleRange=500, const int fsDouble=0, const int odr=200);
    bool setGyroActive(const bool active);
    int readTempGyro();
    float*  readDataGyro();

    //acc -----------------------------------------------
    bool initAcc(const int scaleRange=2,const bool noise=false, const bool hybrid=true, const int odr=200);
    bool setAccActive(const bool active);
    float*  readDataAcc();

    void calibrateSens(int samples=1000);
    void setTi(const int filterTi);
    float * getData(const int ms,bool gyro,bool acc);

private slots:
    void convertData(const int sensor);
    void filterData();
    void fillDataTable();

private:

    //i2c variables
    QString i2cDev="/dev/i2c-3",i2cTemp="/dev/i2c-1";
    int fileSensor=0;

    //calibration
    bool calib=false;

    //sensors variables
    int16_t gyroRaw[3]={0},accRaw[3]={0},magRaw[3]={0},calibTable[6]={0};
    float gyro[3]={0},acc[3]={0},mag[3]={0},angle[3]={0};
    float gyroOld[3]={0},accOld[3]={0},magOld[3]={0},angleOld[3];
    float gyroFiltered[3]={0},accFiltered[3]={0},magFiltered[3]={0},angleFiltered[3]={0};
    float gyroFilteredOld[3]={0},accFilteredOld[3]={0},magFilteredOld[3]={0},angleFilteredOld[3]={0};
    float gyroData[6]={0},accData[6]={0},magData[6]={0},angleData[6]={0};
    float gyroSensivity=0,accSensivity=0,magSensivity=0;
    float data[24]={0};
    uint8_t temp=0;
    uint16_t gyroScale=0,gyroRange=0,accScale=0;

    //filter variable
    int ti=0;
    int32_t currentTime=0,oldTime=0;

    //cycles variables
    int cycle;
    QTime *time;
};

#endif // QNEOSENSORS_H