Ejemplo n.º 1
0
bool RTIMULSM9DS0::IMUInit()
{
    unsigned char result;

#ifdef LSM9DS0_CACHE_MODE
    m_firstTime = true;
    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_gyroSlaveAddr = m_settings->m_I2CSlaveAddress;
    if (m_gyroSlaveAddr == LSM9DS0_GYRO_ADDRESS0)
        m_accelCompassSlaveAddr = LSM9DS0_ACCELMAG_ADDRESS0;
    else
        m_accelCompassSlaveAddr = LSM9DS0_ACCELMAG_ADDRESS1;

    m_bus = m_settings->m_I2CBus;

    setCalibrationData(m_settings->m_compassCalValid, m_settings->m_compassCalMin,
                              m_settings->m_compassCalMax);


    //  enable the I2C bus
    setI2CBus(m_bus);
    if (!I2COpen())
        return false;

    //  Set up the gyro

    if (!I2CWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL5, 0x80, "Failed to boot LSM9DS0"))
        return false;

    if (!I2CRead(m_gyroSlaveAddr, LSM9DS0_GYRO_WHO_AM_I, 1, &result, "Failed to read LSM9DS0 gyro id"))
        return false;

    if (result != LSM9DS0_GYRO_ID) {
        HAL_ERROR1("Incorrect LSM9DS0 gyro id %d\n", result);
        return false;
    }

    if (!setGyroSampleRate())
            return false;

    if (!setGyroCTRL2())
            return false;

    if (!setGyroCTRL4())
            return false;

    //  Set up the accel

    if (!I2CRead(m_accelCompassSlaveAddr, LSM9DS0_WHO_AM_I, 1, &result, "Failed to read LSM9DS0 accel/mag id"))
        return false;

    if (result != LSM9DS0_ACCELMAG_ID) {
        HAL_ERROR1("Incorrect LSM9DS0 accel/mag id %d\n", result);
        return false;
    }

    if (!setAccelCTRL1())
        return false;

    if (!setAccelCTRL2())
        return false;

    if (!setCompassCTRL5())
        return false;

    if (!setCompassCTRL6())
        return false;

    if (!setCompassCTRL7())
        return false;

#ifdef LSM9DS0_CACHE_MODE

    //  turn on gyro fifo

    if (!I2CWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_FIFO_CTRL, 0x3f, "Failed to set LSM9DS0 FIFO mode"))
        return false;
#endif

    if (!setGyroCTRL5())
            return false;

    gyroBiasInit();

    HAL_INFO("LSM9DS0 init complete\n");
    return true;
}
Ejemplo n.º 2
0
bool RTIMUGD20HM303DLHC::IMUInit()
{
    unsigned char result;

#ifdef GD20HM303DLHC_CACHE_MODE
    m_firstTime = true;
    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_gyroSlaveAddr = m_settings->m_I2CSlaveAddress;
    m_accelSlaveAddr = LSM303DLHC_ACCEL_ADDRESS;
    m_compassSlaveAddr = LSM303DLHC_COMPASS_ADDRESS;

    setCalibrationData();

    //  enable the I2C bus

    if (!m_settings->HALOpen())
        return false;

    //  Set up the gyro

    if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_LOW_ODR, 0x04, "Failed to reset L3GD20H"))
        return false;

    if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL5, 0x80, "Failed to boot L3GD20H"))
        return false;

    if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20H_WHO_AM_I, 1, &result, "Failed to read L3GD20H id"))
        return false;

    if (result != L3GD20H_ID) {
        HAL_ERROR1("Incorrect L3GD20H id %d\n", result);
        return false;
    }

    if (!setGyroSampleRate())
            return false;

    if (!setGyroCTRL2())
            return false;

    if (!setGyroCTRL4())
            return false;

    //  Set up the accel

    if (!setAccelCTRL1())
        return false;

    if (!setAccelCTRL4())
        return false;

    //  Set up the compass

    if (!setCompassCRA())
        return false;

    if (!setCompassCRB())
        return false;

    if (!setCompassCRM())
        return false;

#ifdef GD20HM303DLHC_CACHE_MODE

    //  turn on gyro fifo

    if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_FIFO_CTRL, 0x3f, "Failed to set L3GD20 FIFO mode"))
        return false;
#endif

    if (!setGyroCTRL5())
            return false;

    gyroBiasInit();

    HAL_INFO("GD20HM303DLHC init complete\n");
    return true;
}
int RTIMUGD20HM303DLHC::IMUInit()
{
    unsigned char result;

    //  configure IMU

    m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress;
    m_accelSlaveAddr = LSM303DLHC_ACCEL_ADDRESS;
    m_compassSlaveAddr = LSM303DLHC_COMPASS_ADDRESS;

    setCalibrationData();

    //  Set up the gyro

    if (!I2CWrite(m_gyroSlaveAddr, L3GD20H_LOW_ODR, 0x04))
        return -1;

    if (!I2CWrite(m_gyroSlaveAddr, L3GD20H_CTRL5, 0x80))
        return -2;

    if (!I2CRead(m_gyroSlaveAddr, L3GD20H_WHO_AM_I, 1, &result))
        return -3;

    if (result != L3GD20H_ID) {
        return -4;
    }

    if (!setGyroSampleRate())
            return -5;

    if (!setGyroCTRL2())
            return -6;

    if (!setGyroCTRL4())
            return -7;

    //  Set up the accel

    if (!setAccelCTRL1())
        return -8;

    if (!setAccelCTRL4())
        return -9;

    //  Set up the compass

    if (!setCompassCRA())
        return -10;

    if (!setCompassCRB())
        return -11;

    if (!setCompassCRM())
        return -12;

    if (!setGyroCTRL5())
            return -13;

    gyroBiasInit();

    return true;
}
Ejemplo n.º 4
0
int RTIMUGD20HM303D::IMUInit()
{
    unsigned char result;

    //  configure IMU

    m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress;
    if (m_gyroSlaveAddr == L3GD20H_ADDRESS0)
        m_accelCompassSlaveAddr = LSM303D_ADDRESS0;
    else
        m_accelCompassSlaveAddr = LSM303D_ADDRESS1;

    setCalibrationData();

    //  Set up the gyro

    if (!I2CWrite(m_gyroSlaveAddr, L3GD20H_LOW_ODR, 0x04))
        return -1;

    if (!I2CWrite(m_gyroSlaveAddr, L3GD20H_CTRL5, 0x80))
        return -2;

    if (!I2CRead(m_gyroSlaveAddr, L3GD20H_WHO_AM_I, 1, &result))
        return -3;

    if (result != L3GD20H_ID) {
        return -4;
    }

    if (!setGyroSampleRate())
            return -5;

    if (!setGyroCTRL2())
            return -6;

    if (!setGyroCTRL4())
            return -7;

    //  Set up the accel/compass

    if (!I2CRead(m_accelCompassSlaveAddr, LSM303D_WHO_AM_I, 1, &result))
        return -8;

    if (result != LSM303D_ID) {
        return -9;
    }

    if (!setAccelCTRL1())
        return -10;

    if (!setAccelCTRL2())
        return -11;

    if (!setCompassCTRL5())
        return -12;

    if (!setCompassCTRL6())
        return -13;

    if (!setCompassCTRL7())
        return -14;

    if (!setGyroCTRL5())
            return -16;

    gyroBiasInit();

    return true;
}