inline void MPU6050_getMotion6(int16_t* ax,
int16_t* ay,
int16_t* az,
int16_t* gx,
int16_t* gy,
int16_t* gz,
uint8_t use_calib)
{
uint8_t tx_buf[1]; uint8_t tx_len;
tx_buf[0] = MPU6050_RA_ACCEL_XOUT_H;
tx_len = 1;
uint8_t rx_len = 14;
uint8_t rx_buf[rx_len];
i2c_writeBytes(mpu6050_dev_addr, tx_buf, tx_len, 0);
i2c_readBytes(mpu6050_dev_addr, rx_buf, rx_len, 0);
*ax = (((int16_t)rx_buf[0]) << 8) | rx_buf[1];
*ay = (((int16_t)rx_buf[2]) << 8) | rx_buf[3];
*az = (((int16_t)rx_buf[4]) << 8) | rx_buf[5];
*gx = (((int16_t)rx_buf[8]) << 8) | rx_buf[9];
*gy = (((int16_t)rx_buf[10]) << 8) | rx_buf[11];
*gz = (((int16_t)rx_buf[12]) << 8) | rx_buf[13];
if(use_calib == 1)
{
*gx -= gx0;
*gy -= gy0;
*gz -= gz0;
}
}
void MPU6050_getFIFOBytes(uint8_t *data, uint8_t length) {
uint8_t tx_buf;
tx_buf = MPU6050_RA_FIFO_R_W;
i2c_writeBytes(mpu6050_dev_addr, &tx_buf, 1, 0);
i2c_readBytes(mpu6050_dev_addr, data, length, 0);
}
void hmc5883l_readConfiguration( hmc5883l_configuration_t *conf ){
DEBUG_FUNC_ENTER();
i2c_readBytes( HMC5883L_SLAVER, HMC5883L_REG_ADDR_CONF_A,
(uint8_t*)conf, 3);
DEBUG_FUNC_EXIT();
}
void hmc5883l_getSensorData( hmc5883l_sensor_data_t *data ){
DEBUG_FUNC_ENTER();
uint8_t *data_p = (uint8_t *)data;
i2c_readBytes( HMC5883L_SLAVER, HMC5883L_REG_ADDR_X_MSB,
data_p, 6 );
__swap_uint16( data_p );
__swap_uint16( data_p+2 );
__swap_uint16( data_p+4 );
DEBUG_FUNC_EXIT();
}
