/*
* 函数名：void InitMPU6050(void)
* 描述 ：初始化Mpu6050
* 输入 ：无
* 输出 ：无
* 调用 ：外部调用
*/
void InitSensor(void)
{
/***********************MPU9250_SET********************************************/
delayms(10);
I2C_ByteWrite(MPU6050_ADDRESS,PWR_MGMT_1,0x00);//解除休眠状态
delayms(10);
I2C_ByteWrite(MPU6050_ADDRESS,SMPLRT_DIV,0x07);//采样率1000/(1+7)=125HZ
delayms(10);
I2C_ByteWrite(MPU6050_ADDRESS,CONFIG,0x06);//低通滤波器 0x06 5hz
delayms(10);
I2C_ByteWrite(MPU6050_ADDRESS,GYRO_CONFIG,0x18);//陀螺仪测量范围 0X18 正负2000度
delayms(10);
I2C_ByteWrite(MPU6050_ADDRESS,ACCEL_CONFIG,0x00);//加速读量程正负2g
delayms(10);
I2C_ByteWrite(MPU6050_ADDRESS,0x37,0x02);// Set by pass mode for the magnetometers
delayms(10);
/***********************MAG_SET********************************************/
I2C_ByteWrite(MAG_ADDRESS,0x0B,0x01); // Reset Device
delayms(10);
I2C_ByteWrite(MAG_ADDRESS,0x0A,0x10); // Power-down mode
delayms(10);
I2C_ByteWrite(MAG_ADDRESS,0x0A,0x11); //Request first magnetometer single measurement
delayms(10);
/***************************BMP280_SET*************************************/
I2C_ByteWrite(BMP280_ADDRESS,0xF4,0x25);//设置forced mode，每次测量完都要回到sleep mode
delayms(10);
}
/*
* 函数名：void InitMPU6050(void)
* 描述 ：初始化Mpu6050
* 输入 ：无
* 输出 ：无
* 调用 ：外部调用
*/
void InitMPU6050(void)
{
I2C_ByteWrite(PWR_MGMT_1, 0x00); //解除休眠状态
I2C_ByteWrite(SMPLRT_DIV, 0x07);
I2C_ByteWrite(CONFIG, 0x06);
I2C_ByteWrite(GYRO_CONFIG, 0x18);
I2C_ByteWrite(ACCEL_CONFIG, 0x00);
}
uint8_t MPU9150_RegWrite(int addr_i2c, int addr_reg, char v)
{
char data[2];
data[0] = addr_reg;
data[1] = v;
I2C_ByteWrite(addr_i2c);
I2C_ByteWrite(data[0]);
I2C_ByteWrite(data[1]);
return MPU9150_DataWrite(addr_i2c, data, 2, 0) == 0;
}
/*******************************************************************************
* Function Name : WriteReg
* Description : Generic Writing function. It must be fullfilled with either
* : I2C or SPI writing function
* Input : Register Address, Data to be written
* Output : None
* Return : None
*******************************************************************************/
u8_t WriteReg(u8_t deviceAddress, u8_t WriteAddr, u8_t Data) {
//To be completed with either I2c or SPI writing function
//SPI_Mems_Write_Reg(Reg, Data);
I2C_ByteWrite(&Data, deviceAddress, WriteAddr);
return 1;
}
/** 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);
}
//是否是带符号数，待确认
int16_t GetData_BMP280(unsigned char REG_Address)
{
uint8_t H,L;
I2C_ByteWrite(BMP280_ADDRESS,0xF4,0x25);//设置forced mode，每次测量完都要回到sleep mode
delayms(5);
H=I2C_ByteRead(BMP280_ADDRESS,REG_Address);
L=I2C_ByteRead(BMP280_ADDRESS,REG_Address+1);
return (H<<8)+L; //合成数据
}
uint8_t LSM303C::ACC_WriteReg(ACC_REG_t reg, uint8_t data)
{
// printf("EMPTY\n");
uint8_t ret;
if (interfaceMode == MODE_I2C)
{
ret = I2C_ByteWrite(ACC_I2C_ADDR, reg, data);
}
else
{
ret = IMU_GENERIC_ERROR;
}
return ret;
}
//有符号数 ，是否可设置为连续读取模式
int16_t GetData_MAG(unsigned char REG_Address)
{
uint8_t H,L;
uint8_t ST1;
I2C_ByteWrite(MAG_ADDRESS,0x0A,0x11);//根据手册知单次测量模什结束会回到powerdown模式，需要重置一下
delayms(1);
do// Read register Status 1 and wait for the DRDY: Data Ready
{
ST1=I2C_ByteRead(MAG_ADDRESS,0x02);
}
while(!(ST1&0x01));
L=I2C_ByteRead(MAG_ADDRESS,REG_Address);
H=I2C_ByteRead(MAG_ADDRESS,REG_Address+1);
return (H<<8)+L; //合成数据
}
int I2C_Write(I2C_TypeDef* I2Cx, int address, const char* data, int length, int stop){
uint16_t timeout;
int count;
I2C_Start(I2Cx);
// Wait until SB flag is set
timeout = FLAG_TIMEOUT;
while(I2C_GetFlagStatus(I2Cx, I2C_FLAG_SB) == RESET){
timeout--;
if(timeout == 0){
return -1;
}
}
I2Cx->DR = I2C_7BIT_ADD_WRITE(address);
// Wait address is acknowledged
timeout = FLAG_TIMEOUT;
while(I2C_GetFlagStatus(I2Cx, I2C_FLAG_ADDR) == RESET){
timeout--;
if(timeout == 0){
return -1;
}
}
__I2C_CLEAR_ADDRFLAG(I2Cx);
for (count = 0; count < length; count++) {
if (I2C_ByteWrite(I2Cx, data[count]) != 1) {
I2C_Stop(I2Cx);
return -1;
}
}
// If not repeated start, send stop.
if (stop) {
I2C_Stop(I2Cx);
}
return count;
}
uint8_t LSM303C::ACC_WriteReg(ACC_REG_t reg, uint8_t data)
{
debug_print(EMPTY);
uint8_t ret;
if (interfaceMode == MODE_I2C)
{
ret = I2C_ByteWrite(ACC_I2C_ADDR, reg, data);
}
else if (interfaceMode == MODE_SPI)
{
ret = SPI_WriteByte(ACC, reg, data);
}
else
{
ret = IMU_GENERIC_ERROR;
}
return ret;
}
/** 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);
}
int I2C_Write(int address, char *data, int length, int stop) //////////I2C write with event check
{
int timeout;
int count;
char byte_n;
/* update CR2 register */
I2C1->CR2 = (I2C1->CR2 & (uint32_t)~((uint32_t)(I2C_CR2_SADD | I2C_CR2_NBYTES | I2C_CR2_RELOAD | I2C_CR2_AUTOEND | I2C_CR2_RD_WRN | I2C_CR2_START | I2C_CR2_STOP)))
| (uint32_t)(((uint32_t)address & I2C_CR2_SADD) | (((uint32_t)length << 16) & I2C_CR2_NBYTES) | (uint32_t)I2C_SoftEnd_Mode | (uint32_t)I2C_Generate_Start_Write);
for (count = 0; count < length; count++) {
byte_n = data[count];
I2C_ByteWrite(byte_n);
}
// Wait transfer complete
timeout = FLAG_TIMEOUT;
while (I2C_GetFlagStatus(I2C1, I2C_FLAG_TC) == RESET) {
timeout--;
if (timeout == 0) {
return -1;
}
}
I2C_ClearFlag(I2C1, I2C_FLAG_TC);
// If not repeated start, send stop.
if (stop) {
I2C1->CR2 |= I2C_CR2_STOP;
/* Wait until STOPF flag is set */
timeout = FLAG_TIMEOUT;
while (I2C_GetFlagStatus(I2C1, I2C_FLAG_STOPF) == RESET) {
timeout--;
if (timeout == 0) {
return -1;
}
}
/* Clear STOP Flag */
I2C_ClearFlag(I2C1, I2C_FLAG_STOPF);
}
return count;
}