int8_t Magnetometer::begin()
{
    uint8_t buff[3];
    i2cAddr_ = HMC5833L_I2CADD;

    // Join the I2C bus as master
    WIRE.begin();

    // read the ID registers
    if (i2cReadBytes(HMC5833L_REG_IDA, buff, 3) != 0) 
        return HMC5833L_ERROR_I2CREAD;
    
    // compare the ID registers
    if (buff[0] != HMC5833L_REG_IDA_ID || buff[1] != HMC5833L_REG_IDB_ID
        || buff[2] != HMC5833L_REG_IDC_ID)
        return HMC5833L_ERROR_WRONG_ID;

    // set data rate speed to 30hz
    if (i2cWriteByte(HMC5833L_REG_CFGA, 0x14) != 0)
        return HMC5833L_ERROR_I2CWRITE;

    // set to continuous mode
    // single mode not supported by lib
    if (i2cWriteByte(HMC5833L_REG_MODE, 0) != 0)
        return HMC5833L_ERROR_I2CWRITE;

    // set default gain
    if (setGain(HMC5833L_GAIN_1090) != 0)
        return HMC5833L_ERROR_I2CWRITE;

    return 0;
}
Esempio n. 2
0
void Accelerometer::readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address)
{
    setMemoryBank(bank);
    i2cWriteBytes(MPU6050_RA_MEM_START_ADDR, 1, &address); // Set memory start address
    uint8_t chunkSize;
    for (uint16_t i = 0; i < dataSize;) {
        chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
        if (i + chunkSize > dataSize)
            chunkSize = dataSize - i;
        if (chunkSize > 256 - address)
            chunkSize = 256 - address;

        i2cReadBytes(MPU6050_RA_MEM_R_W, chunkSize, data + i);

        i += chunkSize;
        address += chunkSize;

        if (i < dataSize) {
            if (address == 0)
                bank++;
            setMemoryBank(bank);
            i2cWriteBytes(MPU6050_RA_MEM_START_ADDR, 1, &address); // Set Memory start address
        }
    }
}
Esempio n. 3
0
bool Accelerometer::writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem)
{
    setMemoryBank(bank);
    i2cWriteBytes(MPU6050_RA_MEM_START_ADDR, 1, &address); // Set Memory start address
    uint8_t chunkSize;
    uint8_t *verifyBuffer = NULL;
    uint8_t *progBuffer = NULL;
    uint16_t i;
    uint8_t j;
    if (verify)
        verifyBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
    if (useProgMem)
        progBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
    for (i = 0; i < dataSize;) {
        chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;

        if (i + chunkSize > dataSize)
            chunkSize = dataSize - i;

        if (chunkSize > 256 - address)
            chunkSize = 256 - address;

        if (useProgMem)
            for (j = 0; j < chunkSize; j++)
                progBuffer[j] = pgm_read_byte(data + i + j);
        else
            progBuffer = (uint8_t *)data + i;

        i2cWriteBytes(MPU6050_RA_MEM_R_W, chunkSize, progBuffer);

        if (verify && verifyBuffer) {
            setMemoryBank(bank);
            i2cWriteBytes(MPU6050_RA_MEM_START_ADDR, 1, &address); // Set memory start address
            i2cReadBytes(MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer);
            if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
                free(verifyBuffer);
                if (useProgMem)
                    free(progBuffer);
                return false;
            }
        }

        i += chunkSize;
        address += chunkSize;

        if (i < dataSize) {
            if (address == 0)
                bank++;
            setMemoryBank(bank);
            i2cWriteBytes(MPU6050_RA_MEM_START_ADDR, 1, &address); // Set memory start address
        }
    }
    if (verify)
        free(verifyBuffer);
    if (useProgMem)
        free(progBuffer);
    return true;
}
Esempio n. 4
0
int8_t Accelerometer::i2cReadBits(uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data)
{
    uint8_t count, b;
    if ((count = i2cReadBytes(regAddr, 1, &b)) != 0) {
        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
        b &= mask;
        b >>= (bitStart - length + 1);
        *data = b;
    }
Esempio n. 5
0
uint16_t Accelerometer::getFIFOCount()
{
    i2cReadBytes(MPU6050_RA_FIFO_COUNTH, 2, buffer);
    uint16_t ret = (((uint16_t)buffer[0]) << 8) | buffer[1];
    if(ret == 1024)
    {
        resetFIFO();
        return 0;
    }
    else
        return ret;
}
Esempio n. 6
0
uint16_t SRF02::getRange(uint8_t cmd) {
    uint16_t range=0;
    uint8_t  data[4];
    bool     success=FALSE;
    
    if ( i2cWriteCmd(cmd) ) {
        usleep(PING_DELAY);
        success = i2cReadBytes( data, 255 );
    }
    if (!success) {
        fprintf(stderr, "[ERROR] SRF02::getRange(...) failed. %s\n", i2cErrorString);
    } else {
        range = WORD(data[2],data[3]);
    }
    return(range);
}
int8_t Magnetometer::readRaw(int16_t * x, int16_t * y, int16_t * z)
{
    if (i2cAddr_ == 0)
        return HMC5833L_ERROR_NOT_INITIALIZED;

    uint8_t data[6];

    if (i2cReadBytes(HMC5833L_REG_DATAXYZ, data, 6) != 0)
        return HMC5833L_ERROR_I2CREAD;

    conv2Byte2Signed16(data[1], data[0], x);
    conv2Byte2Signed16(data[3], data[2], z);
    conv2Byte2Signed16(data[5], data[4], y);

    if (*x == -4096 || *y == -4096 || *z == -4096)
        return HMC5833L_ERROR_GAINOVERFLOW;

    return 0;
}
Esempio n. 8
0
void Accelerometer::getFIFOBytes(uint8_t *data, uint8_t length)
{
    i2cReadBytes(MPU6050_RA_FIFO_R_W, length, data);
}
Esempio n. 9
0
uint8_t Accelerometer::getIntStatus()
{
    i2cReadBytes(MPU6050_RA_INT_STATUS, 1, buffer);
    return buffer[0];
}
Esempio n. 10
0
Accelerometer::Accelerometer()
{
    m_i2cfd = open("/dev/i2c-1", O_RDWR);
    if(!m_i2cfd < 0)
        abort();
    if(ioctl(m_i2cfd, I2C_SLAVE, 0x68) < 0)
        abort();

    i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO); // Set Clock source
    i2cWriteBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_250); // Full scale gyro range
    i2cWriteBits(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS_2); // Full scale accel range
    i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 1, false); // Disable sleep mode

    i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, 1, true); // Reset
    usleep(30000);
    i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 1, false); // Disable sleep mode
    setMemoryBank(0x10, true, true); // Get MPU hardware revision
    uint8_t value = 0x06; // Alright, this is dirty but a pointer to this value will be sent to every i2cWriteBytes...
    uint16_t value16; // Yeah... I know, I won't go to heaven but that's ok
    i2cWriteBytes(MPU6050_RA_MEM_START_ADDR, 1, &value); // Set memory start address
    i2cReadBytes(MPU6050_RA_MEM_R_W, 1, buffer); // Get hardware revision
    uint8_t hwRevision __attribute__((__unused__)) = buffer[0];
    setMemoryBank(0, false, false);

    i2cReadBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, 1, buffer);
    uint8_t otpValid __attribute__((__unused__)) = buffer[0]; // Check OTP bank validity

    i2cReadBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
    int8_t xgOffset = buffer[0];
    i2cReadBits(MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
    int8_t ygOffset = buffer[0];
    i2cReadBits(MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
    int8_t zgOffset = buffer[0];

    value = 0x7F; // Setup weird slave stuff
    i2cWriteBytes(MPU6050_RA_I2C_SLV0_ADDR, 1, &value);
    i2cWriteBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, 1, false); // Disable I2C Master Mode
    value = 0x68;
    i2cWriteBytes(MPU6050_RA_I2C_SLV0_ADDR, 1, &value);
    i2cWriteBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, 1, true); // Reset I2C Master
    usleep(20000);

    if (writeMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE, 0, 0, true, true)) { // Load DMP code into memory banks
        if (writeDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE, true)) { // Let's write the DMP configuration
            i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_ZGYRO); // Set clock source
            value = 0x12;
            i2cWriteBytes(MPU6050_RA_INT_ENABLE, 1, &value); // Enable 0x12 interuption
            value = 4; // 1khz / (1 + 4) = 200 Hz
            i2cWriteBytes(MPU6050_RA_SMPLRT_DIV, 1, &value);
            i2cWriteBits(MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, MPU6050_EXT_SYNC_TEMP_OUT_L); // External Frame Sync
            i2cWriteBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42); // DLPF Mode
            i2cWriteBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_2000); // Full scale gyro range
            value = 0x03;
            i2cWriteBytes(MPU6050_RA_DMP_CFG_1, 1, &value);
            value = 0x00;
            i2cWriteBytes(MPU6050_RA_DMP_CFG_2, 1, &value);

            // Setting X Y and Z gyro offsets to previous values
            i2cWriteBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, xgOffset);
            i2cWriteBits(MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, ygOffset);
            i2cWriteBits(MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, zgOffset);

            value16 = 0; // And gyro offsets users to 0
            i2cWriteWords(MPU6050_RA_XG_OFFS_USRH, 1, &value16);
            i2cWriteWords(MPU6050_RA_YG_OFFS_USRH, 1, &value16);
            i2cWriteWords(MPU6050_RA_ZG_OFFS_USRH, 1, &value16);

            // Memory update 1/7
            uint8_t dmpUpdate[16], j;
            uint16_t pos = 0;
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
                dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);

            //Memory update 2/7
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
                dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
            resetFIFO();
            uint8_t fifoCount = getFIFOCount();
            uint8_t fifoBuffer[128];
            value = 2;
            i2cWriteBytes(MPU6050_RA_MOT_THR, 1, &value); // Motion threshold
            value = 156;
            i2cWriteBytes(MPU6050_RA_ZRMOT_THR, 1, &value); // Zero Motion threshold
            value = 80;
            i2cWriteBytes(MPU6050_RA_MOT_DUR, 1, &value); // Motion duration
            value = 0;
            i2cWriteBytes(MPU6050_RA_ZRMOT_DUR, 1, &value); // Zero Motion duration
            resetFIFO();
            setFIFOEnabled(true);
            setDMPEnabled(true);
            i2cWriteBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, 1, true); // Reset DMP

            // Memory update 3/7
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
                dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);

            // Memory update 4/7
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
                dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);

            // Memory update 5/7
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
                dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
            while ((fifoCount = getFIFOCount()) < 3);
            getFIFOBytes(fifoBuffer, fifoCount);
            uint8_t mpuIntStatus __attribute__((__unused__)) = getIntStatus();

            // Memory update 6/7
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
                dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
            while ((fifoCount = getFIFOCount()) < 3);
            getFIFOBytes(fifoBuffer, fifoCount);
            mpuIntStatus = getIntStatus();

            // Memory update 7/7
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
                dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);

            setDMPEnabled(false);
            dmpPacketSize = 42;
            resetFIFO();
            getIntStatus();
        } else
            abort();
    } else
        abort();

    i2cReadBits(MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, buffer);
    if(buffer[0] != 0x34)
        abort();

    setDMPEnabled(true);
    zeroValues[0] = 0;
    zeroValues[1] = 0;
    zeroValues[2] = 0;
}
Esempio n. 11
0
bool SRF02::i2cReadBytes ( uint8_t *data ) {
    return(i2cReadBytes(data,1));
}