コード例 #1
0
bool RTIMULSM9DS1::IMURead()
{
    unsigned char status;
    unsigned char gyroData[6];
    unsigned char accelData[6];
    unsigned char compassData[6];

    if (!m_settings->HALRead(m_accelGyroSlaveAddr, LSM9DS1_STATUS, 1, &status, "Failed to read LSM9DS1 status"))
        return false;

    if ((status & 0x3) == 0)
        return false;

    for (int i = 0; i<6; i++){
        if (!m_settings->HALRead(m_accelGyroSlaveAddr, LSM9DS1_OUT_X_L_G + i, 1, &gyroData[i], "Failed to read LSM9DS1 gyro data"))
            return false;

        if (!m_settings->HALRead(m_accelGyroSlaveAddr, LSM9DS1_OUT_X_L_XL + i, 1, &accelData[i], "Failed to read LSM9DS1 accel data"))
            return false;

        if (!m_settings->HALRead(m_magSlaveAddr, LSM9DS1_MAG_OUT_X_L + i, 1, &compassData[i], "Failed to read LSM9DS1 compass data"))
            return false;
    }


    m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();

    RTMath::convertToVector(gyroData, m_imuData.gyro, m_gyroScale, false);
    RTMath::convertToVector(accelData, m_imuData.accel, m_accelScale, false);
    RTMath::convertToVector(compassData, m_imuData.compass, m_compassScale, false);

    //  sort out gyro axes and correct for bias

    m_imuData.gyro.setZ(-m_imuData.gyro.z());

    //  sort out accel data;

    m_imuData.accel.setX(-m_imuData.accel.x());
    m_imuData.accel.setY(-m_imuData.accel.y());

    //  sort out compass axes

    m_imuData.compass.setX(-m_imuData.compass.x());
    m_imuData.compass.setZ(-m_imuData.compass.z());

    //  now do standard processing

    handleGyroBias();
    calibrateAverageCompass();
    calibrateAccel();

    //  now update the filter

    updateFusion();

    return true;
}
コード例 #2
0
bool RTIMUMPU9150::IMURead()
{
    unsigned char fifoCount[2];
    unsigned int count;
    unsigned char fifoData[12];
    unsigned char compassData[8];

    if (!m_settings->HALRead(m_slaveAddr, MPU9150_FIFO_COUNT_H, 2, fifoCount, "Failed to read fifo count"))
         return false;

    count = ((unsigned int)fifoCount[0] << 8) + fifoCount[1];

    if (count == 1024) {
        HAL_INFO("MPU9150 fifo has overflowed");
        resetFifo();
        m_imuData.timestamp += m_sampleInterval * (1024 / MPU9150_FIFO_CHUNK_SIZE + 1); // try to fix timestamp
        return false;
    }


#ifdef MPU9150_CACHE_MODE
    if ((m_cacheCount == 0) && (count  >= MPU9150_FIFO_CHUNK_SIZE) && (count < (MPU9150_CACHE_SIZE * MPU9150_FIFO_CHUNK_SIZE))) {
        // special case of a small fifo and nothing cached - just handle as simple read

        if (!m_settings->HALRead(m_slaveAddr, MPU9150_FIFO_R_W, MPU9150_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data"))
            return false;

        if (!m_settings->HALRead(m_slaveAddr, MPU9150_EXT_SENS_DATA_00, m_compassDataLength, compassData, "Failed to read compass data"))
            return false;
    } else {
        if (count >= (MPU9150_CACHE_SIZE * MPU9150_FIFO_CHUNK_SIZE)) {
            if (m_cacheCount == MPU9150_CACHE_BLOCK_COUNT) {
                // all cache blocks are full - discard oldest and update timestamp to account for lost samples
                m_imuData.timestamp += m_sampleInterval * m_cache[m_cacheOut].count;
                if (++m_cacheOut == MPU9150_CACHE_BLOCK_COUNT)
                    m_cacheOut = 0;
                m_cacheCount--;
            }

            int blockCount = count / MPU9150_FIFO_CHUNK_SIZE;   // number of chunks in fifo

            if (blockCount > MPU9150_CACHE_SIZE)
                blockCount = MPU9150_CACHE_SIZE;

            if (!m_settings->HALRead(m_slaveAddr, MPU9150_FIFO_R_W, MPU9150_FIFO_CHUNK_SIZE * blockCount,
                                m_cache[m_cacheIn].data, "Failed to read fifo data"))
                return false;

            if (!m_settings->HALRead(m_slaveAddr, MPU9150_EXT_SENS_DATA_00, 8, m_cache[m_cacheIn].compass, "Failed to read compass data"))
                return false;

            m_cache[m_cacheIn].count = blockCount;
            m_cache[m_cacheIn].index = 0;

            m_cacheCount++;
            if (++m_cacheIn == MPU9150_CACHE_BLOCK_COUNT)
                m_cacheIn = 0;

        }

        //  now fifo has been read if necessary, get something to process

        if (m_cacheCount == 0)
            return false;

        memcpy(fifoData, m_cache[m_cacheOut].data + m_cache[m_cacheOut].index, MPU9150_FIFO_CHUNK_SIZE);
        memcpy(compassData, m_cache[m_cacheOut].compass, 8);

        m_cache[m_cacheOut].index += MPU9150_FIFO_CHUNK_SIZE;

        if (--m_cache[m_cacheOut].count == 0) {
            //  this cache block is now empty

            if (++m_cacheOut == MPU9150_CACHE_BLOCK_COUNT)
                m_cacheOut = 0;
            m_cacheCount--;
        }
    }

#else

    if (count > MPU9150_FIFO_CHUNK_SIZE * 40) {
        // more than 40 samples behind - going too slowly so discard some samples but maintain timestamp correctly
        while (count >= MPU9150_FIFO_CHUNK_SIZE * 10) {
            if (!m_settings->HALRead(m_slaveAddr, MPU9150_FIFO_R_W, MPU9150_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data"))
                return false;
            count -= MPU9150_FIFO_CHUNK_SIZE;
            m_imuData.timestamp += m_sampleInterval;
        }
    }

    if (count < MPU9150_FIFO_CHUNK_SIZE)
        return false;

    if (!m_settings->HALRead(m_slaveAddr, MPU9150_FIFO_R_W, MPU9150_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data"))
        return false;

    if (!m_settings->HALRead(m_slaveAddr, MPU9150_EXT_SENS_DATA_00, m_compassDataLength, compassData, "Failed to read compass data"))
        return false;

#endif

    RTMath::convertToVector(fifoData, m_imuData.accel, m_accelScale, true);
    RTMath::convertToVector(fifoData + 6, m_imuData.gyro, m_gyroScale, true);

    if (m_compassIs5883)
        RTMath::convertToVector(compassData, m_imuData.compass, 0.092f, true);
    else
        RTMath::convertToVector(compassData + 1, m_imuData.compass, 0.3f, false);

    //  sort out gyro axes

    m_imuData.gyro.setX(m_imuData.gyro.x());
    m_imuData.gyro.setY(-m_imuData.gyro.y());
    m_imuData.gyro.setZ(-m_imuData.gyro.z());

    //  sort out accel data;

    m_imuData.accel.setX(-m_imuData.accel.x());


    if (m_compassIs5883) {
        //  sort out compass axes

        float temp;

        temp = m_imuData.compass.y();
        m_imuData.compass.setY(-m_imuData.compass.z());
        m_imuData.compass.setZ(-temp);

    } else {

        //  use the compass fuse data adjustments

        m_imuData.compass.setX(m_imuData.compass.x() * m_compassAdjust[0]);
        m_imuData.compass.setY(m_imuData.compass.y() * m_compassAdjust[1]);
        m_imuData.compass.setZ(m_imuData.compass.z() * m_compassAdjust[2]);

        //  sort out compass axes

        float temp;

        temp = m_imuData.compass.x();
        m_imuData.compass.setX(m_imuData.compass.y());
        m_imuData.compass.setY(-temp);
    }


    //  now do standard processing

    handleGyroBias();
    calibrateAverageCompass();
    calibrateAccel();

    if (m_firstTime)
        m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
    else
        m_imuData.timestamp += m_sampleInterval;

    m_firstTime = false;

    //  now update the filter

    updateFusion();

    return true;
}
コード例 #3
0
ファイル: RTIMULSM9DS0.cpp プロジェクト: Siyra/Quadcopter
bool RTIMULSM9DS0::IMURead()
{
    unsigned char status;
    unsigned char gyroData[6];
    unsigned char accelData[6];
    unsigned char compassData[6];


#ifdef LSM9DS0_CACHE_MODE
    int count;

    if (!m_settings->HALRead(m_gyroSlaveAddr, LSM9DS0_GYRO_FIFO_SRC, 1, &status, "Failed to read LSM9DS0 gyro fifo status"))
        return false;

    if ((status & 0x40) != 0) {
        HAL_INFO("LSM9DS0 gyro fifo overrun\n");
        if (!m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL5, 0x10, "Failed to set LSM9DS0 gyro CTRL5"))
            return false;

        if (!m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_FIFO_CTRL, 0x0, "Failed to set LSM9DS0 gyro FIFO mode"))
            return false;

        if (!m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_FIFO_CTRL, 0x3f, "Failed to set LSM9DS0 gyro FIFO mode"))
            return false;

        if (!setGyroCTRL5())
            return false;

        m_imuData.timestamp += m_sampleInterval * 32;
        return false;
    }

    // get count of samples in fifo
    count = status & 0x1f;

    if ((m_cacheCount == 0) && (count > 0) && (count < LSM9DS0_FIFO_THRESH)) {
        // special case of a small fifo and nothing cached - just handle as simple read

        if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | LSM9DS0_GYRO_OUT_X_L, 6, gyroData, "Failed to read LSM9DS0 gyro data"))
            return false;

        if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_A, 6, accelData, "Failed to read LSM9DS0 accel data"))
            return false;

        if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_M, 6, compassData, "Failed to read LSM9DS0 compass data"))
            return false;

        if (m_firstTime)
            m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
        else
            m_imuData.timestamp += m_sampleInterval;

        m_firstTime = false;
   } else {
        if (count >=  LSM9DS0_FIFO_THRESH) {
            // need to create a cache block

            if (m_cacheCount == LSM9DS0_CACHE_BLOCK_COUNT) {
                // all cache blocks are full - discard oldest and update timestamp to account for lost samples
                m_imuData.timestamp += m_sampleInterval * m_cache[m_cacheOut].count;
                if (++m_cacheOut == LSM9DS0_CACHE_BLOCK_COUNT)
                    m_cacheOut = 0;
                m_cacheCount--;
            }

            if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | LSM9DS0_GYRO_OUT_X_L, LSM9DS0_FIFO_CHUNK_SIZE * LSM9DS0_FIFO_THRESH,
                         m_cache[m_cacheIn].data, "Failed to read LSM9DS0 fifo data"))
                return false;

            if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_A, 6,
                         m_cache[m_cacheIn].accel, "Failed to read LSM9DS0 accel data"))
                return false;

            if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_M, 6,
                         m_cache[m_cacheIn].compass, "Failed to read LSM9DS0 compass data"))
                return false;

            m_cache[m_cacheIn].count = LSM9DS0_FIFO_THRESH;
            m_cache[m_cacheIn].index = 0;

            m_cacheCount++;
            if (++m_cacheIn == LSM9DS0_CACHE_BLOCK_COUNT)
                m_cacheIn = 0;

        }

        //  now fifo has been read if necessary, get something to process

        if (m_cacheCount == 0)
            return false;

        memcpy(gyroData, m_cache[m_cacheOut].data + m_cache[m_cacheOut].index, LSM9DS0_FIFO_CHUNK_SIZE);
        memcpy(accelData, m_cache[m_cacheOut].accel, 6);
        memcpy(compassData, m_cache[m_cacheOut].compass, 6);

        m_cache[m_cacheOut].index += LSM9DS0_FIFO_CHUNK_SIZE;

        if (--m_cache[m_cacheOut].count == 0) {
            //  this cache block is now empty

            if (++m_cacheOut == LSM9DS0_CACHE_BLOCK_COUNT)
                m_cacheOut = 0;
            m_cacheCount--;
        }
        if (m_firstTime)
            m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
        else
            m_imuData.timestamp += m_sampleInterval;

        m_firstTime = false;
    }

#else
    if (!m_settings->HALRead(m_gyroSlaveAddr, LSM9DS0_GYRO_STATUS, 1, &status, "Failed to read LSM9DS0 status"))
        return false;

    if ((status & 0x8) == 0)
        return false;

    if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | LSM9DS0_GYRO_OUT_X_L, 6, gyroData, "Failed to read LSM9DS0 gyro data"))
        return false;

    m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();

    if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_A, 6, accelData, "Failed to read LSM9DS0 accel data"))
        return false;

    if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_M, 6, compassData, "Failed to read LSM9DS0 compass data"))
        return false;

#endif

    RTMath::convertToVector(gyroData, m_imuData.gyro, m_gyroScale, false);
    RTMath::convertToVector(accelData, m_imuData.accel, m_accelScale, false);
    RTMath::convertToVector(compassData, m_imuData.compass, m_compassScale, false);

    //  sort out gyro axes and correct for bias

    m_imuData.gyro.setX(m_imuData.gyro.x());
    m_imuData.gyro.setY(-m_imuData.gyro.y());
    m_imuData.gyro.setZ(-m_imuData.gyro.z());

    //  sort out accel data;

    m_imuData.accel.setX(-m_imuData.accel.x());

    //  sort out compass axes

    m_imuData.compass.setY(-m_imuData.compass.y());

    //  now do standard processing

    handleGyroBias();
    calibrateAverageCompass();
    calibrateAccel();

    //  now update the filter

    updateFusion();

    return true;
}
コード例 #4
0
bool RTIMUMPU9255::IMURead()
{
    unsigned char fifoCount[2];
    unsigned int count;
    unsigned char fifoData[MPU9255_FIFO_CHUNK_SIZE];
    #if MPU9255_FIFO_WITH_COMPASS == 0
    unsigned char compassData[8]; // compass data goes here if it is not coming in through FIFO
    #endif
    #if MPU9255_FIFO_WITH_TEMP == 0
    unsigned char temperatureData[2]; // if temperature data is not coming in through FIFO
    #endif
    if (!m_settings->HALRead(m_slaveAddr, MPU9255_FIFO_COUNT_H, 2, fifoCount, "Failed to read fifo count"))
         return false;

    count = ((unsigned int)fifoCount[0] << 8) + fifoCount[1];
    if (count == 512) {
        HAL_INFO("MPU-9255 fifo has overflowed");
        resetFifo();
        m_imuData.timestamp += m_sampleInterval * (512 / MPU9255_FIFO_CHUNK_SIZE + 1); // try to fix timestamp
        return false;
    }

#ifdef MPU9255_CACHE_MODE
    if ( (m_cacheCount == 0) && (count  < MPU9255_FIFO_CHUNK_SIZE) ) 
        return false; // no new set of data available
    if ((m_cacheCount == 0) && (count  >= MPU9255_FIFO_CHUNK_SIZE) && (count < (MPU9255_CACHE_SIZE * MPU9255_FIFO_CHUNK_SIZE))) {
        // special case of a small fifo and nothing cached - just handle as simple read

        if (!m_settings->HALRead(m_slaveAddr, MPU9255_FIFO_R_W, MPU9255_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data"))
            return false;

        #if MPU9255_FIFO_WITH_TEMP == 0 // read temp from registers
        if (!m_settings->HALRead(m_slaveAddr, MPU9255_TEMP_OUT_H, 2,
                            temperatureData, "Failed to read temperature data"))
            return false; 
        #endif

        #if MPU9255_FIFO_WITH_COMPASS == 0 // read compass without fifo
        if (!m_settings->HALRead(m_slaveAddr, MPU9255_EXT_SENS_DATA_00, 8, compassData, "Failed to read compass data"))
            return false;
		#endif
    } else {
        if (count >= (MPU9255_CACHE_SIZE * MPU9255_FIFO_CHUNK_SIZE)) {
            if (m_cacheCount == MPU9255_CACHE_BLOCK_COUNT) {
                // all cache blocks are full - discard oldest and update timestamp to account for lost samples
                m_imuData.timestamp += m_sampleInterval * m_cache[m_cacheOut].count;
                if (++m_cacheOut == MPU9255_CACHE_BLOCK_COUNT)
                    m_cacheOut = 0;
                m_cacheCount--;
            }

            int blockCount = count / MPU9255_FIFO_CHUNK_SIZE;   // number of chunks in fifo

            if (blockCount > MPU9255_CACHE_SIZE)
                blockCount = MPU9255_CACHE_SIZE;

            if (!m_settings->HALRead(m_slaveAddr, MPU9255_FIFO_R_W, MPU9255_FIFO_CHUNK_SIZE * blockCount,
                    m_cache[m_cacheIn].data, "Failed to read fifo data"))
                return false;

            #if MPU9255_FIFO_WITH_TEMP == 0 // read temp from registers
            if (!m_settings->HALRead(m_slaveAddr, MPU9255_TEMP_OUT_H, 2,
                                m_cache[m_cacheIn].temperature, "Failed to read temperature data"))
                return false; 
            #endif
            #if MPU9255_FIFO_WITH_COMPASS == 0 // read compass from register
            if (!m_settings->HALRead(m_slaveAddr, MPU9255_EXT_SENS_DATA_00, 8, m_cache[m_cacheIn].compass, "Failed to read compass data"))
                return false;
			#endif
            m_cache[m_cacheIn].count = blockCount;
            m_cache[m_cacheIn].index = 0;

            m_cacheCount++;
            if (++m_cacheIn == MPU9255_CACHE_BLOCK_COUNT)
                m_cacheIn = 0;

        }

        //  now fifo has been read if necessary, get something to process

        if (m_cacheCount == 0)
            return false;

        memcpy(fifoData, m_cache[m_cacheOut].data + m_cache[m_cacheOut].index, MPU9255_FIFO_CHUNK_SIZE);
        #if MPU9255_FIFO_WITH_COMPASS == 0
        memcpy(compassData, m_cache[m_cacheOut].compass, 8);
        #endif
        #if MPU9255_FIFO_WITH_TEMP == 0
        memcpy(temperatureData, m_cache[m_cacheOut].temperature, 2);            
        #endif

        m_cache[m_cacheOut].index += MPU9255_FIFO_CHUNK_SIZE;

        if (--m_cache[m_cacheOut].count == 0) {
            //  this cache block is now empty

            if (++m_cacheOut == MPU9255_CACHE_BLOCK_COUNT)
                m_cacheOut = 0;
            m_cacheCount--;
        }
    }

#else

    if (count > MPU9255_FIFO_CHUNK_SIZE * 40) {
        // more than 40 samples behind - going too slowly so discard some samples but maintain timestamp correctly
        while (count >= MPU9255_FIFO_CHUNK_SIZE * 10) {
            if (!m_settings->HALRead(m_slaveAddr, MPU9255_FIFO_R_W, MPU9255_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data"))
                return false;
            count -= MPU9255_FIFO_CHUNK_SIZE;
            m_imuData.timestamp += m_sampleInterval;
        }
    }

    if (count < MPU9255_FIFO_CHUNK_SIZE)
        return false;

    if (!m_settings->HALRead(m_slaveAddr, MPU9255_FIFO_R_W, MPU9255_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data"))
        return false;
    #if MPU9255_FIFO_WITH_TEMP == 0
    if (!m_settings->HALRead(m_slaveAddr, MPU9255_TEMP_OUT_H, 2, temperatureData, "Failed to read temperature data"))
        return false;
    #endif

    #if MPU9255_FIFO_WITH_COMPASS == 0	
    if (!m_settings->HALRead(m_slaveAddr, MPU9255_EXT_SENS_DATA_00, 8, compassData, "Failed to read compass data"))
        return false;
	#endif
#endif

   // Accelerometer
    RTMath::convertToVector(fifoData, m_imuData.accel, m_accelScale, true);
    #if MPU9255_FIFO_WITH_TEMP == 1
        // Temperature
       
        m_imuData.IMUtemperature =  ((RTFLOAT)((int16_t)(((uint16_t)fifoData[6] << 8) | (uint16_t)fifoData[7])) / 333.87f ) + 21.0f;  // combined registers and convert to temperature
        m_imuData.IMUtemperatureValid = true;
        // Gyroscope
        RTMath::convertToVector(fifoData + 8, m_imuData.gyro, m_gyroScale, true);
         // Compass
        #if MPU9255_FIFO_WITH_COMPASS == 1
			RTMath::convertToVector(fifoData + 14 + 1, m_imuData.compass, 0.6f, false);
        #else
			RTMath::convertToVector(compassData + 1, m_imuData.compass, 0.6f, false);
		#endif
    #else // no temp in fifo
	    // Temperature

        m_imuData.IMUtemperature = ((RTFLOAT)((int16_t)(((uint16_t)fifoData[6] << 8) | (uint16_t)fifoData[7])) / 333.87f ) + 21.0f; // combined registers and convert to temperature
        m_imuData.IMUtemperatureValid = true;
		// ((TEMP_OUT – RoomTemp_Offset)/Temp_Sensitivity) + 21degC
		// 333.87 = sensitivity
		// 0 = room temp offset at 21
        // Gyroscope
        RTMath::convertToVector(fifoData + 6, m_imuData.gyro, m_gyroScale, true);

		// Compass
		#if MPU9255_FIFO_WITH_COMPASS == 1 // without temp but with compass in FIFO
            RTMath::convertToVector(fifoData + 12 + 1, m_imuData.compass, 0.6f, false);
        #else
            RTMath::convertToVector(compassData + 1, m_imuData.compass, 0.6f, false);
        #endif
	#endif

    //  sort out gyro axes

    m_imuData.gyro.setX(m_imuData.gyro.x());
    m_imuData.gyro.setY(-m_imuData.gyro.y());
    m_imuData.gyro.setZ(-m_imuData.gyro.z());

    //  sort out accel data;

    m_imuData.accel.setX(-m_imuData.accel.x());

    //  use the compass fuse data adjustments

    m_imuData.compass.setX(m_imuData.compass.x() * m_compassAdjust[0]);
    m_imuData.compass.setY(m_imuData.compass.y() * m_compassAdjust[1]);
    m_imuData.compass.setZ(m_imuData.compass.z() * m_compassAdjust[2]);

    //  sort out compass axes

    float temp;

    temp = m_imuData.compass.x();
    m_imuData.compass.setX(m_imuData.compass.y());
    m_imuData.compass.setY(-temp);

    if (m_firstTime)
        m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
    else
        m_imuData.timestamp += m_sampleInterval;

    m_firstTime = false;
	
    //  now do standard processing
    if (m_imuData.IMUtemperatureValid == true) {
        // Check if temperature changed
        if (fabs(m_imuData.IMUtemperature - m_IMUtemperature_previous) >= TEMPERATURE_DELTA) {
            // If yes, update bias
            updateTempBias(m_imuData.IMUtemperature);
            m_IMUtemperature_previous = m_imuData.IMUtemperature;
        }
        // Then do
        handleTempBias(); 	// temperature Correction
    }
    handleGyroBias();
    calibrateAverageCompass();
    calibrateAccel();

    //  now update the filter

    updateFusion();

    return true;
}
コード例 #5
0
bool RTIMUBMX055::IMURead()
{
    unsigned char status;
    unsigned char gyroData[6];
    unsigned char accelData[6];
    unsigned char magData[8];

    if (!m_settings->HALRead(m_gyroSlaveAddr, BMX055_GYRO_FIFO_STATUS, 1, &status, "Failed to read BMX055 gyro fifo status"))
        return false;

    if (status & 0x80) {
        // fifo overflowed
        HAL_ERROR("BMX055 fifo overflow\n");

        // this should clear it
        if (!m_settings->HALWrite(m_gyroSlaveAddr, BMX055_GYRO_FIFO_CONFIG_1, 0x40, "Failed to set BMX055 FIFO config"))
            return false;

        m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();  // try to fix timestamp
        return false;
    }

    if (status == 0)
        return false;

    if (!m_settings->HALRead(m_gyroSlaveAddr, BMX055_GYRO_FIFO_DATA, 6, gyroData, "Failed to read BMX055 gyro data"))
            return false;

    if (!m_settings->HALRead(m_accelSlaveAddr, BMX055_ACCEL_X_LSB, 6, accelData, "Failed to read BMX055 accel data"))
        return false;

    if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_X_LSB, 8, magData, "Failed to read BMX055 mag data"))
        return false;

    RTMath::convertToVector(gyroData, m_imuData.gyro, m_gyroScale, false);

    //  need to prepare accel data

    accelData[0] &= 0xf0;
    accelData[2] &= 0xf0;
    accelData[4] &= 0xf0;

    RTMath::convertToVector(accelData, m_imuData.accel, m_accelScale, false);

    float mx, my, mz;

    processMagData(magData, mx, my, mz);
    m_imuData.compass.setX(mx);
    m_imuData.compass.setY(my);
    m_imuData.compass.setZ(mz);

    //  sort out gyro axes

    m_imuData.gyro.setY(-m_imuData.gyro.y());
    m_imuData.gyro.setZ(-m_imuData.gyro.z());

    //  sort out accel axes

    m_imuData.accel.setX(-m_imuData.accel.x());

    //  sort out mag axes

#ifdef BMX055_REMAP
    m_imuData.compass.setY(-m_imuData.compass.y());
    m_imuData.compass.setZ(-m_imuData.compass.z());
#else
    RTFLOAT temp =  m_imuData.compass.x();
    m_imuData.compass.setX(-m_imuData.compass.y());
    m_imuData.compass.setY(-temp);
    m_imuData.compass.setZ(-m_imuData.compass.z());
#endif
    //  now do standard processing

    handleGyroBias();
    calibrateAverageCompass();
    calibrateAccel();

    if (m_firstTime)
        m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
    else
        m_imuData.timestamp += m_sampleInterval;

    m_firstTime = false;

    //  now update the filter

    updateFusion();

    return true;
}