/*******************************************************************************
* Function Name : ReadReg
* Description : Generic Reading function. It must be fullfilled with either
* : I2C or SPI reading functions
* Input : Register Address
* Output : Data REad
* Return : None
*******************************************************************************/
u8_t ReadReg(u8_t deviceAddr, u8_t Reg, u8_t* Data) {
//To be completed with either I2c or SPI reading function
//*Data = SPI_Mems_Read_Reg( Reg );
if(!I2C_BufferRead(Data, deviceAddr, Reg, 1))
return MEMS_ERROR;
else
return MEMS_SUCCESS;
}
/** Read a single bit from an 8-bit device register.
* @param slaveAddr I2C slave device address
* @param regAddr Register regAddr to read from
* @param bitNum Bit position to read (0-7)
* @param data Container for single bit value
* @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in readTimeout)
*/
void clMPU6050::ReadBit(uint8_t slaveAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data)
{
uint8_t tmp;
if(!I2C_BufferRead(slaveAddr, &tmp, regAddr, 1)){
restartI2CBus();
return;
}
*data = tmp & (1 << bitNum);
}
/** write a single bit in an 8-bit device register.
* @param slaveAddr I2C slave device address
* @param regAddr Register regAddr to write to
* @param bitNum Bit position to write (0-7)
* @param value New bit value to write
*/
void clMPU6050::WriteBit(uint8_t slaveAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data)
{
uint8_t tmp;
if(!I2C_BufferRead(slaveAddr, &tmp, regAddr, 1)){
restartI2CBus();
return;
}
tmp = (data != 0) ? (tmp | (1 << bitNum)) : (tmp & ~(1 << bitNum));
I2C_ByteWrite(slaveAddr,&tmp,regAddr);
}
/** Read multiple bits from an 8-bit device register.
* @param slaveAddr I2C slave device address
* @param regAddr Register regAddr to read from
* @param bitStart First bit position to read (0-7)
* @param length Number of bits to read (not more than 8)
* @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
* @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in readTimeout)
*/
void clMPU6050::ReadBits(uint8_t slaveAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data)
{
// 01101001 read byte
// 76543210 bit numbers
// xxx args: bitStart=4, length=3
// 010 masked
// -> 010 shifted
uint8_t tmp;
if(!I2C_BufferRead(slaveAddr, &tmp, regAddr, 1)){
restartI2CBus();
return;
}
uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
tmp &= mask;
tmp >>= (bitStart - length + 1);
*data = tmp;
}
/** Get raw 6-axis motion sensor readings (accel/gyro).
* Retrieves all currently available motion sensor values.
* @param AccelGyro 16-bit signed integer array of length 6
* @see MPU6050_RA_ACCEL_XOUT_H
*/
void clMPU6050::GetRawAccelGyro(s16* AccelGyro, s16* temperature){
static u8 tmpBuffer[14];
if(!I2C_BufferRead(MPU6050_DEFAULT_ADDRESS, tmpBuffer, MPU6050_RA_ACCEL_XOUT_H, 14)){
// If the bus gets blocked by a slave, restart it.
restartI2CBus();
// Exit method, since we could not receive any valid data.
return;
}
/* Get acceleration */
for(int i=0; i<3; i++){
AccelGyro[i]=((s16)((u16)tmpBuffer[2*i] << 8) + tmpBuffer[2*i+1]);
}
/* Get Angular rate */
for(int i=4; i<7; i++){
AccelGyro[i-1]=((s16)((u16)tmpBuffer[2*i] << 8) + tmpBuffer[2*i+1]);
}
// Get temperature:
*temperature = ((s16)((u16)tmpBuffer[6] << 8) + tmpBuffer[7]);;
}
/** Write multiple bits in an 8-bit device register.
* @param slaveAddr I2C slave device address
* @param regAddr Register regAddr to write to
* @param bitStart First bit position to write (0-7)
* @param length Number of bits to write (not more than 8)
* @param data Right-aligned value to write
*/
void clMPU6050::WriteBits(uint8_t slaveAddr, 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;
if(!I2C_BufferRead(slaveAddr, &tmp, regAddr, 1)){
restartI2CBus();
return;
}
uint8_t 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_ByteWrite(slaveAddr,&tmp,regAddr);
}
void inline readMag(uint8_t *data)
{
I2C_BufferRead(data, MAG_I2C_ADDRESS, OUT_X_H_M, 6);
}
void inline ReadStatusM(uint8_t *status)
{
I2C_BufferRead(status,MAG_I2C_ADDRESS,SR_REG_M,1);
}
void inline readACC(uint8_t * data)
{
I2C_BufferRead(data, ACC_I2C_ADDRESS, OUT_X_L_A, 6);
}
