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;
}
Exemplo n.º 2
0
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;
}