void I2C_writeBit(uint8_t slaveAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data)
{
uint8_t tmp;
I2C_readByte(slaveAddr, regAddr, &tmp);
tmp = (data != 0) ? (tmp | (1 << bitNum)) : (tmp & ~(1 << bitNum));
I2C_writeByte(slaveAddr,regAddr,tmp);
}
void setMemoryBank(unsigned char bank, unsigned char prefetchEnabled, unsigned char userBank)
{
bank &= 0x1F;
if (userBank) bank |= 0x20;
if (prefetchEnabled) bank |= 0x40;
I2C_writeByte(devAddr, MPU6050_RA_BANK_SEL, bank);
}
void I2C_writeBytes(uint8_t slaveAddr, uint8_t writeAddr, uint8_t length, uint8_t* data){
int i;
for(i=0; i<=length; i++){
I2C_writeByte(slaveAddr, writeAddr, data[i]);
writeAddr++;
}
}
void I2C_writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data)
{
// 010 value to write
// 76543210 bit numbers
// xxx args: bitStart=4, length=3
// 00011100 mask byte
// 10101111 original value (sample)
// 10100011 original & ~mask
// 10101011 masked | value
uint8_t tmp,mask;
I2C_readByte(devAddr, regAddr, &tmp);
mask = ((1 << length) - 1) << (bitStart - length + 1);
data <<= (bitStart - length + 1); // shift data into correct position
data &= mask; // zero all non-important bits in data
tmp &= ~(mask); // zero all important bits in existing byte
tmp |= data; // combine data with existing byte
I2C_writeByte(devAddr, regAddr, tmp);
}
// H* registers
void AK8975_getHeading(int16_t *x, int16_t *y, int16_t *z) {
if(ChronographRead(ChronMAG)>= AK8975_READ_DELAY)
{
ChronographSet(ChronMAG);
I2C_readBytes(devAddr, AK8975_RA_HXL, 6, buffer, 0);
hx = (((int16_t)buffer[1]) << 8) | buffer[0];
hy = (((int16_t)buffer[3]) << 8) | buffer[2];
hz = (((int16_t)buffer[5]) << 8) | buffer[4];
I2C_writeByte(devAddr, AK8975_RA_CNTL, AK8975_MODE_SINGLE);
//DelayMsec(10);
//while(!AK8975_getDataReady());
}
*x = hx;
*y = hy;
*z = hz;
}
unsigned char writeDMPConfigurationSet(const unsigned char *d, unsigned int dataSize, unsigned char useProgMem)
{
unsigned char special;
unsigned int i;
// config set dmpConfig is a long string of blocks with the following structure:
// [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]]
unsigned char bank, offset, length;
for (i = 0; i < MPU6050_DMP_CONFIG_SIZE;)
{
bank = pgm_read_byte(dmpConfig + i++); // pgm_read_byte() is a macro that reads a byte of d stored in a specified address(PROGMEM area)
offset = pgm_read_byte(dmpConfig + i++);
length = pgm_read_byte(dmpConfig + i++);
if (length > 0) // regular block of data to write
{
setMemoryBank(bank, false, false); // bank number = bank
setMemoryStartAddress(offset); // startaddress = offset from the beginning (0) of the bank
I2C_writeBytes(devAddr,0x6f , length, (unsigned char*) &dmpConfig[i]);
i = i + length;
}
else // length = 0; special instruction to write
{
// NOTE: this kind of behavior (what and when to do certain things)
// is totally undocumented. This code is in here based on observed
// behavior only, and exactly why (or even whether) it has to be here
// is anybody's guess for now.
special = pgm_read_byte(dmpConfig + i++);
if (special == 0x01)
{
// enable DMP-related interrupts (ZeroMotion, FIFOBufferOverflow, DMP)
I2C_writeByte(devAddr, 0x38, 0x32); // write 00110010: ZMOT_EN, FIFO_OFLOW_EN, DMP_INT_EN true
// by the way: this sets all other interrupt enables to false
}
}
}
return true;
}
//****************************************************************************//
//
// writeRegister
//
// Parameters:
// offset -- register to write
// dataToWrite -- 8 bit data to write to register
//
//****************************************************************************//
status_t writeRegister(uint8_t offset, uint8_t dataToWrite) {
status_t returnError = IMU_SUCCESS;
I2C_writeByte(I2CAddress, offset, dataToWrite);
return returnError;
}
void AK8975_setAdjustmentZ(uint8_t z) {
I2C_writeByte(devAddr, AK8975_RA_ASAZ, z);
}
void setIntEnabled(unsigned char enabled)
{
I2C_writeByte(devAddr, MPU6050_RA_INT_ENABLE, enabled);
}
void AK8975_setAdjustmentX(uint8_t x) {
I2C_writeByte(devAddr, AK8975_RA_ASAX, x);
}
void AK8975_setAdjustmentY(uint8_t y) {
I2C_writeByte(devAddr, AK8975_RA_ASAY, y);
}
void setRate(unsigned char rate)
{
I2C_writeByte(devAddr, MPU6050_RA_SMPLRT_DIV, rate);
}
int16_t AK8975_getHeadingZ() {
I2C_writeByte(devAddr, AK8975_RA_CNTL, AK8975_MODE_SINGLE);
DelayMsec(8);
I2C_readBytes(devAddr, AK8975_RA_HZL, 2, buffer,0);
return (((int16_t)buffer[1]) << 8) | buffer[0];
}
void setDMPConfig2(unsigned char config)
{
I2C_writeByte(devAddr, MPU6050_RA_DMP_CFG_2, config);
}
void setSlaveAddress(unsigned char num, unsigned char address)
{
if (num > 3) return;
I2C_writeByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num*3, address);
}
void setMemoryStartAddress(unsigned char address)
{
I2C_writeByte(devAddr, MPU6050_RA_MEM_START_ADDR, address);
}
void setZeroMotionDetectionThreshold(unsigned char threshold)
{
I2C_writeByte(devAddr, MPU6050_RA_ZRMOT_THR, threshold);
}
void setZeroMotionDetectionDuration(unsigned char duration)
{
I2C_writeByte(devAddr, MPU6050_RA_ZRMOT_DUR, duration);
}
