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;
}
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
}
}
}
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;
}
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;
}
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;
}
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;
}
void Accelerometer::getFIFOBytes(uint8_t *data, uint8_t length)
{
i2cReadBytes(MPU6050_RA_FIFO_R_W, length, data);
}
uint8_t Accelerometer::getIntStatus()
{
i2cReadBytes(MPU6050_RA_INT_STATUS, 1, buffer);
return buffer[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;
}
bool SRF02::i2cReadBytes ( uint8_t *data ) {
return(i2cReadBytes(data,1));
}
