void Accelerometer::clearFifo() { uint8_t tmpBuffer[6] = {0}; uint8_t fifoSrc; bool fifoFull = false; do{ // FIFO overrun test LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_FIFO_SRC_REG_A, &fifoSrc, 1); fifoFull = fifoFull || (fifoSrc & FIFO_OVERRUN) != 0; LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_X_L_A, tmpBuffer, 6); // FIFO empty test LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_FIFO_SRC_REG_A, &fifoSrc, 1); }while ((fifoSrc & FIFO_EMPTY) == 0); /* FIFO needs reset after being full */ if (fifoFull) resetFifo(); else sleep(CHANGE_DELAY); }
void Gyroscope::clearFifo() { uint8_t tmpbuffer[6] = {0}; uint8_t fifoSrc; bool fifoFull = false; do{ // FIFO overrun test L3GD20_Read(&fifoSrc, L3GD20_FIFO_SRC_REG_ADDR, 1); fifoFull = fifoFull || (fifoSrc & 0x40) != 0; L3GD20_Read(tmpbuffer, L3GD20_OUT_X_L_ADDR, 6); // FIFO empty test L3GD20_Read(&fifoSrc, L3GD20_FIFO_SRC_REG_ADDR, 1); }while ((fifoSrc & 0x20) == 0); /* FIFO needs reset after being full */ if (fifoFull) resetFifo(); else sleep(CHANGE_DELAY); }
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; }
bool RTIMUMPU9150::IMUInit() { unsigned char result; m_firstTime = true; #ifdef MPU9150_CACHE_MODE m_cacheIn = m_cacheOut = m_cacheCount = 0; #endif // set validity flags m_imuData.fusionPoseValid = false; m_imuData.fusionQPoseValid = false; m_imuData.gyroValid = true; m_imuData.accelValid = true; m_imuData.compassValid = true; m_imuData.pressureValid = false; m_imuData.temperatureValid = false; m_imuData.humidityValid = false; // configure IMU m_slaveAddr = m_settings->m_I2CSlaveAddress; setSampleRate(m_settings->m_MPU9150GyroAccelSampleRate); setCompassRate(m_settings->m_MPU9150CompassSampleRate); setLpf(m_settings->m_MPU9150GyroAccelLpf); setGyroFsr(m_settings->m_MPU9150GyroFsr); setAccelFsr(m_settings->m_MPU9150AccelFsr); setCalibrationData(); // enable the I2C bus if (!m_settings->HALOpen()) return false; // reset the MPU9150 if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 0x80, "Failed to initiate MPU9150 reset")) return false; m_settings->delayMs(100); if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 0x00, "Failed to stop MPU9150 reset")) return false; if (!m_settings->HALRead(m_slaveAddr, MPU9150_WHO_AM_I, 1, &result, "Failed to read MPU9150 id")) return false; if (result != MPU9150_ID) { HAL_ERROR1("Incorrect MPU9150 id %d\n", result); return false; } // now configure the various components if (!m_settings->HALWrite(m_slaveAddr, MPU9150_LPF_CONFIG, m_lpf, "Failed to set lpf")) return false; if (!setSampleRate()) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_GYRO_CONFIG, m_gyroFsr, "Failed to set gyro fsr")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_ACCEL_CONFIG, m_accelFsr, "Failed to set accel fsr")) return false; // now configure compass if (!configureCompass()) return false; // enable the sensors if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 1, "Failed to set pwr_mgmt_1")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_2, 0, "Failed to set pwr_mgmt_2")) return false; // select the data to go into the FIFO and enable if (!resetFifo()) return false; gyroBiasInit(); HAL_INFO("MPU9150 init complete\n"); return true; }
bool RTIMUMPU9250::IMURead() { unsigned char fifoCount[2]; unsigned int count; unsigned char fifoData[12]; unsigned char compassData[8]; if (!I2Cdev::readBytes(m_slaveAddr, MPU9250_FIFO_COUNT_H, 2, fifoCount)) return false; count = ((unsigned int)fifoCount[0] << 8) + fifoCount[1]; if (count == 1024) { resetFifo(); m_timestamp += m_sampleInterval * (1024 / MPU9250_FIFO_CHUNK_SIZE + 1); // try to fix timestamp return false; } if (count > MPU9250_FIFO_CHUNK_SIZE * 40) { // more than 40 samples behind - going too slowly so discard some samples but maintain timestamp correctly while (count >= MPU9250_FIFO_CHUNK_SIZE * 10) { if (!I2Cdev::readBytes(m_slaveAddr, MPU9250_FIFO_R_W, MPU9250_FIFO_CHUNK_SIZE, fifoData)) return false; count -= MPU9250_FIFO_CHUNK_SIZE; m_timestamp += m_sampleInterval; } } if (count < MPU9250_FIFO_CHUNK_SIZE) return false; if (!I2Cdev::readBytes(m_slaveAddr, MPU9250_FIFO_R_W, MPU9250_FIFO_CHUNK_SIZE, fifoData)) return false; if (!I2Cdev::readBytes(m_slaveAddr, MPU9250_EXT_SENS_DATA_00, 8, compassData)) return false; RTMath::convertToVector(fifoData, m_accel, m_accelScale, true); RTMath::convertToVector(fifoData + 6, m_gyro, m_gyroScale, true); RTMath::convertToVector(compassData + 1, m_compass, 0.6f, false); // sort out gyro axes m_gyro.setY(-m_gyro.y()); m_gyro.setZ(-m_gyro.z()); // sort out accel data; m_accel.setX(-m_accel.x()); // use the fuse data adjustments for compass m_compass.setX(m_compass.x() * m_compassAdjust[0]); m_compass.setY(m_compass.y() * m_compassAdjust[1]); m_compass.setZ(m_compass.z() * m_compassAdjust[2]); // sort out compass axes float temp; temp = m_compass.x(); m_compass.setX(m_compass.y()); m_compass.setY(-temp); // now do standard processing handleGyroBias(); calibrateAverageCompass(); if (m_firstTime) m_timestamp = millis(); else m_timestamp += m_sampleInterval; m_firstTime = false; return true; }
int RTIMUMPU9250::IMUInit() { unsigned char result; unsigned char asa[3]; m_firstTime = true; #ifdef MPU9250_CACHE_MODE m_cacheIn = m_cacheOut = m_cacheCount = 0; #endif // configure IMU m_slaveAddr = m_settings->m_I2CSlaveAddress; setSampleRate(m_settings->m_MPU9250GyroAccelSampleRate); setCompassRate(m_settings->m_MPU9250CompassSampleRate); setGyroLpf(m_settings->m_MPU9250GyroLpf); setAccelLpf(m_settings->m_MPU9250AccelLpf); setGyroFsr(m_settings->m_MPU9250GyroFsr); setAccelFsr(m_settings->m_MPU9250AccelFsr); setCalibrationData(); // reset the MPU9250 if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_PWR_MGMT_1, 0x80)) return -1; delay(100); if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_PWR_MGMT_1, 0x00)) return -4; if (!I2Cdev::readByte(m_slaveAddr, MPU9250_WHO_AM_I, &result)) return -5; if (result != MPU9250_ID) { return -6; } // now configure the various components if (!setGyroConfig()) return -7; if (!setAccelConfig()) return -8; if (!setSampleRate()) return -9; // now configure compass if (!bypassOn()) return -11; // get fuse ROM data if (!I2Cdev::writeByte(AK8963_ADDRESS, AK8963_CNTL, 0)) { bypassOff(); return -12; } if (!I2Cdev::writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0f)) { bypassOff(); return -13; } if (!I2Cdev::readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, asa)) { bypassOff(); return -14; } // convert asa to usable scale factor m_compassAdjust[0] = ((float)asa[0] - 128.0) / 256.0 + 1.0f; m_compassAdjust[1] = ((float)asa[1] - 128.0) / 256.0 + 1.0f; m_compassAdjust[2] = ((float)asa[2] - 128.0) / 256.0 + 1.0f; if (!I2Cdev::writeByte(AK8963_ADDRESS, AK8963_CNTL, 0)) { bypassOff(); return -15; } if (!bypassOff()) return -16; // now set up MPU9250 to talk to the compass chip if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_MST_CTRL, 0x40)) return -17; if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV0_ADDR, 0x80 | AK8963_ADDRESS)) return -18; if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV0_REG, AK8963_ST1)) return -19; if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV0_CTRL, 0x88)) return -20; if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV1_ADDR, AK8963_ADDRESS)) return -21; if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV1_REG, AK8963_CNTL)) return -22; if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV1_CTRL, 0x81)) return -23; if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV1_DO, 0x1)) return -24; if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_MST_DELAY_CTRL, 0x3)) return -25; if (!setCompassRate()) return -27; // enable the sensors if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_PWR_MGMT_1, 1)) return -28; if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_PWR_MGMT_2, 0)) return -29; // select the data to go into the FIFO and enable if (!resetFifo()) return -30; gyroBiasInit(); return 1; }
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; }
bool RTIMUMPU9255::IMUInit() { unsigned char result; m_firstTime = true; #ifdef MPU9255_CACHE_MODE m_cacheIn = m_cacheOut = m_cacheCount = 0; #endif // set validity flags m_imuData.fusionPoseValid = false; m_imuData.fusionQPoseValid = false; m_imuData.gyroValid = true; m_imuData.accelValid = true; m_imuData.compassValid = true; m_imuData.motion = true; m_imuData.IMUtemperatureValid = false; m_imuData.IMUtemperature = 0.0; m_imuData.humidityValid = false; m_imuData.humidity = -1.0; m_imuData.humidityTemperatureValid = false; m_imuData.humidityTemperature = 0.0; m_imuData.pressureValid = false; m_imuData.pressure = 0.0; m_imuData.pressureTemperatureValid = false; m_imuData.pressureTemperature = 0.0; // configure IMU m_slaveAddr = m_settings->m_I2CSlaveAddress; setSampleRate(m_settings->m_MPU9255GyroAccelSampleRate); setCompassRate(m_settings->m_MPU9255CompassSampleRate); setGyroLpf(m_settings->m_MPU9255GyroLpf); setAccelLpf(m_settings->m_MPU9255AccelLpf); setGyroFsr(m_settings->m_MPU9255GyroFsr); setAccelFsr(m_settings->m_MPU9255AccelFsr); setCalibrationData(); // enable the bus if (!m_settings->HALOpen()) return false; // reset the MPU9255 if (!m_settings->HALWrite(m_slaveAddr, MPU9255_PWR_MGMT_1, 0x80, "Failed to initiate MPU9255 reset")) return false; m_settings->delayMs(100); if (!m_settings->HALWrite(m_slaveAddr, MPU9255_PWR_MGMT_1, 0x00, "Failed to stop MPU9255 reset")) return false; if (!m_settings->HALRead(m_slaveAddr, MPU9255_WHO_AM_I, 1, &result, "Failed to read MPU9255 id")) return false; if (result != MPU9255_ID) { HAL_ERROR2("Incorrect %s id %d\n", IMUName(), result); return false; } // now configure the various components if (!setGyroConfig()) return false; if (!setAccelConfig()) return false; if (!setSampleRate()) return false; if(!compassSetup()) { return false; } if (!setCompassRate()) return false; // enable the sensors if (!m_settings->HALWrite(m_slaveAddr, MPU9255_PWR_MGMT_1, 1, "Failed to set pwr_mgmt_1")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9255_PWR_MGMT_2, 0, "Failed to set pwr_mgmt_2")) return false; // select the data to go into the FIFO and enable if (!resetFifo()) return false; gyroBiasInit(); HAL_INFO1("%s init complete\n", IMUName()); return true; }
void Accelerometer::retrieveValues() { const uint16_t shift = 16; uint8_t tmpBuffer[6] = {0}; math3d::Vector3<int16_t> vec; uint8_t ctrl4, fifoCtrl, fifoSrc; int i = 0; LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_FIFO_CTRL_REG_A, &fifoCtrl, 1); bool fifoMode = (fifoCtrl & IS_FIFO) != 0; bool fifoFull = false; if (fifoMode){ LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_FIFO_SRC_REG_A, &fifoSrc, 1); /* Test FIFO empty bit */ if ((fifoSrc & FIFO_EMPTY) != 0) return; } if(_scaleBuffer.empty()) return; LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG4_A, &ctrl4, 1); do{ // FIFO overrun test if (fifoMode){ LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_FIFO_SRC_REG_A, &fifoSrc, 1); fifoFull = fifoFull || (fifoSrc & FIFO_OVERRUN) != 0; } LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_X_L_A, tmpBuffer, 6); /* Check in the control register 4 the data alignment (Big Endian or Little Endian) */ if(ctrl4 & LSM303DLHC_BLE_MSB){ for(i = 0; i < 3; i++){ vec[i] = (int16_t)(((uint16_t)tmpBuffer[2*i] << 8) + tmpBuffer[2*i+1]); } } else{ for(i = 0; i < 3; i++){ vec[i] = (int16_t)(((uint16_t)tmpBuffer[2*i+1] << 8) + tmpBuffer[2*i]); } } // Check if buffer size isn't larger than maximum if(_maxBufferSize > 0 && _dataBuffer.size() >= _maxBufferSize) discard(); /* Place retrieved values in local buffer */ _dataBuffer.push_back(vec / shift); _scaleBuffer.back().second++; // FIFO empty test if (fifoMode){ // Let FIFO update sleep(10, microsecond); LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_FIFO_SRC_REG_A, &fifoSrc, 1); } }while (fifoMode && (fifoSrc & FIFO_EMPTY) == 0); /* FIFO needs reset after being full */ if (fifoMode && fifoFull) resetFifo(); }
// TODO: Bypass to stream support // Stream to FIFO support void Gyroscope::retrieveValues() { uint8_t tmpbuffer[6] = {0}; math3d::Vector3<int16_t> vec; uint8_t ctrl4, fifoCtrl, fifoSrc; int i = 0; L3GD20_Read(&fifoCtrl, L3GD20_FIFO_CTRL_REG_ADDR, 1); bool fifoMode = (fifoCtrl & 0x70) != 0; bool fifoFull = false; if (fifoMode){ L3GD20_Read(&fifoSrc, L3GD20_FIFO_SRC_REG_ADDR, 1); /* Test FIFO empty bit */ if ((fifoSrc & 0x20) != 0) return; } if(_scaleBuffer.empty()) return; L3GD20_Read(&ctrl4, L3GD20_CTRL_REG4_ADDR, 1); do{ // FIFO overrun test if (fifoMode){ L3GD20_Read(&fifoSrc, L3GD20_FIFO_SRC_REG_ADDR, 1); fifoFull = fifoFull || (fifoSrc & 0x40) != 0; } L3GD20_Read(tmpbuffer, L3GD20_OUT_X_L_ADDR, 6); /* Check in the control register 4 the data alignment (Big Endian or Little Endian) */ if(ctrl4 & 0x40){ for(i = 0; i < 3; i++){ vec[i] = (int16_t)(((uint16_t)tmpbuffer[2*i] << 8) + tmpbuffer[2*i+1]); } } else{ for(i = 0; i < 3; i++){ vec[i] = (int16_t)(((uint16_t)tmpbuffer[2*i+1] << 8) + tmpbuffer[2*i]); } } // Check if buffer size isn't larger than maximum if(_maxBufferSize > 0 && _dataBuffer.size() >= _maxBufferSize) discard(); /* Place retrieved values in local buffer */ _dataBuffer.push_back(vec); _scaleBuffer.back().second++; // FIFO empty test if (fifoMode){ // Let FIFO update sleep(10, microsecond); L3GD20_Read(&fifoSrc, L3GD20_FIFO_SRC_REG_ADDR, 1); } }while (fifoMode && (fifoSrc & 0x20) == 0); /* FIFO needs reset after being full */ if (fifoMode && fifoFull) resetFifo(); }
bool RTIMUMPU9150::IMUInit() { unsigned char result; unsigned char asa[3]; m_firstTime = true; #ifdef MPU9150_CACHE_MODE m_cacheIn = m_cacheOut = m_cacheCount = 0; #endif // set validity flags m_imuData.fusionPoseValid = false; m_imuData.fusionQPoseValid = false; m_imuData.gyroValid = true; m_imuData.accelValid = true; m_imuData.compassValid = true; m_imuData.pressureValid = false; m_imuData.temperatureValid = false; m_imuData.humidityValid = false; // configure IMU m_slaveAddr = m_settings->m_I2CSlaveAddress; setSampleRate(m_settings->m_MPU9150GyroAccelSampleRate); setCompassRate(m_settings->m_MPU9150CompassSampleRate); setLpf(m_settings->m_MPU9150GyroAccelLpf); setGyroFsr(m_settings->m_MPU9150GyroFsr); setAccelFsr(m_settings->m_MPU9150AccelFsr); setCalibrationData(); // enable the I2C bus if (!m_settings->HALOpen()) return false; // reset the MPU9150 if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 0x80, "Failed to initiate MPU9150 reset")) return false; m_settings->delayMs(100); if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 0x00, "Failed to stop MPU9150 reset")) return false; if (!m_settings->HALRead(m_slaveAddr, MPU9150_WHO_AM_I, 1, &result, "Failed to read MPU9150 id")) return false; if (result != MPU9150_ID) { HAL_ERROR1("Incorrect MPU9150 id %d\n", result); return false; } // now configure the various components if (!m_settings->HALWrite(m_slaveAddr, MPU9150_LPF_CONFIG, m_lpf, "Failed to set lpf")) return false; if (!setSampleRate()) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_GYRO_CONFIG, m_gyroFsr, "Failed to set gyro fsr")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_ACCEL_CONFIG, m_accelFsr, "Failed to set accel fsr")) return false; // now configure compass bypassOn(); // get fuse ROM data if (!m_settings->HALWrite(AK8975_ADDRESS, AK8975_CNTL, 0, "Failed to set compass in power down mode 1")) { bypassOff(); return false; } if (!m_settings->HALWrite(AK8975_ADDRESS, AK8975_CNTL, 0x0f, "Failed to set compass in fuse ROM mode")) { bypassOff(); return false; } if (!m_settings->HALRead(AK8975_ADDRESS, AK8975_ASAX, 3, asa, "Failed to read compass fuse ROM")) { bypassOff(); return false; } // convert asa to usable scale factor m_compassAdjust[0] = ((float)asa[0] - 128.0) / 256.0 + 1.0f; m_compassAdjust[1] = ((float)asa[1] - 128.0) / 256.0 + 1.0f; m_compassAdjust[2] = ((float)asa[2] - 128.0) / 256.0 + 1.0f; if (!m_settings->HALWrite(AK8975_ADDRESS, AK8975_CNTL, 0, "Failed to set compass in power down mode 2")) { bypassOff(); return false; } bypassOff(); // now set up MPU9150 to talk to the compass chip if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_MST_CTRL, 0x40, "Failed to set I2C master mode")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_ADDR, 0x80 | AK8975_ADDRESS, "Failed to set slave 0 address")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_REG, AK8975_ST1, "Failed to set slave 0 reg")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_CTRL, 0x88, "Failed to set slave 0 ctrl")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_ADDR, AK8975_ADDRESS, "Failed to set slave 1 address")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_REG, AK8975_CNTL, "Failed to set slave 1 reg")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_CTRL, 0x81, "Failed to set slave 1 ctrl")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_DO, 0x1, "Failed to set slave 1 DO")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_MST_DELAY_CTRL, 0x3, "Failed to set mst delay")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_YG_OFFS_TC, 0x80, "Failed to set yg offs tc")) return false; if (!setCompassRate()) return false; // enable the sensors if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 1, "Failed to set pwr_mgmt_1")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_2, 0, "Failed to set pwr_mgmt_2")) return false; // select the data to go into the FIFO and enable if (!resetFifo()) return false; gyroBiasInit(); HAL_INFO("MPU9150 init complete\n"); return true; }