六轴传感器ASM330LHH驱动函数

#include "IMU.h" /********************************************************* 函数功能：写惯导器件一个字节数据 入口参数：器件写地址、寄存器地址、数据 返 回：无。 备 注：无。 作者：Roy 时间：2022-10-12 *********************************************************/ u8 IMU_Write_Byte(u8 addr,u8 reg,u8 data) { IIC_Start(); IIC_Send_Byte((addr|0)); //发送器件地址+写命令 if(IIC_Wait_Ack()) //等待应答 { IIC_Stop(); return 1; } IIC_Send_Byte(reg); //写寄存器地址 IIC_Wait_Ack(); //等待应答 IIC_Send_Byte(data); //发送数据 if(IIC_Wait_Ack()) //等待ACK { IIC_Stop(); return 1; } IIC_Stop(); return 0; } /********************************************************* 函数功能：读惯导器件一个字节数据 入口参数：器件地址、寄存器地址 返 回：读取的数据。 备 注：无。 作者：Roy 时间：2022-10-12 *********************************************************/ u8 IMU_Read_Byte(u8 addr,u8 reg) { u8 res; IIC_Start(); IIC_Send_Byte((addr|0)); //发送器件地址+写命令 IIC_Wait_Ack(); //等待应答 IIC_Send_Byte(reg); //写寄存器地址 IIC_Wait_Ack(); //等待应答 IIC_Start(); IIC_Send_Byte((addr|1)); //发送器件地址+读命令 IIC_Wait_Ack(); //等待应答 res=IIC_Read_Byte(0); //读数据,发送nACK IIC_Stop(); //产生一个停止条件 return res; } /********************************************************* 函数功能：写惯导器件多个字节数据 入口参数：器件写地址、寄存器地址、长度、数据 返 回：无。 备 注：无。 作者：Roy 时间：2022-10-12 *********************************************************/ u8 IMU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf) { u8 i; IIC_Start(); IIC_Send_Byte((addr)); //发送器件地址+写命令 if(IIC_Wait_Ack()) //等待应答 { IIC_Stop(); return 1; } IIC_Send_Byte(reg); //写寄存器地址 IIC_Wait_Ack(); //等待应答 for(i=0;i<len;i++) { IIC_Send_Byte(buf[i]); //发送数据 if(IIC_Wait_Ack()) //等待ACK { IIC_Stop(); return 1; } } IIC_Stop(); return 0; } /********************************************************* 函数功能：读惯导器件一个字节数据 入口参数：地址、数据、长度、读取到的数据存储区 返 回：无。 备 注：无。 作者：Roy 时间：2022-10-12 *********************************************************/ u8 IMU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf) { IIC_Start(); IIC_Send_Byte(addr); //发送器件地址+写命令 if(IIC_Wait_Ack()) //等待应答 { IIC_Stop(); return 1; } IIC_Send_Byte(reg); //写寄存器地址 IIC_Wait_Ack(); //等待应答 IIC_Start(); IIC_Send_Byte(addr|1); //发送器件地址+读命令 IIC_Wait_Ack(); //等待应答 while(len) { if(len==1)*buf=IIC_Read_Byte(0);//读数据,发送nACK else *buf=IIC_Read_Byte(1); //读数据,发送ACK len--; buf++; } IIC_Stop(); //产生一个停止条件 return 0; } /********************************************************* 函数功能：检测芯片状态 入口参数：无 返 回：无。 备 注：无。 作者：Roy 时间：2022-10-12 *********************************************************/ u8 IMU_Check(void) { u8 res; IMU_Write_Byte(ASM330LHH_I2C_ADD_L_W,ASM330LHH_CTRL9_XL,0x20);//必须有 res=IMU_Read_Byte(ASM330LHH_I2C_ADD_L_W,ASM330LHH_ID_ADD); if(res==ASM330LHH_ID_Data) { printf("IMU-ASM330LHH Check successfully\n"); } else { printf("Not find IMU chip"); } return 0; } u8 IMU_Init(void) { //IMU_Write_Byte(ASM330LHH_I2C_ADD_L_W,ASM330LHH_CTRL3_C,0x01); // // delay_ms(100); //IMU_Write_Byte(ASM330LHH_I2C_ADD_L_W,ASM330LHH_CTRL3_C,0x00); IMU_Write_Byte(ASM330LHH_I2C_ADD_L_W,ASM330LHH_CTRL9_XL,0x20);//必须有 IMU_Write_Byte(ASM330LHH_I2C_ADD_L_W,ASM330LHH_CTRL1_XL,0x32); //加速度参数:52 2g IMU_Write_Byte(ASM330LHH_I2C_ADD_L_W,ASM330LHH_CTRL2_G,0x30); //陀罗计参数:52Hz 250dps IMU_Write_Byte(ASM330LHH_I2C_ADD_L_W,ASM330LHH_CTRL3_C,0x02); //使能地址自增模式 IMU_Write_Byte(ASM330LHH_I2C_ADD_L_W,ASM330LHH_CTRL4_C,0x08); printf("IMU-ASM330LHH Initialized successfully\n"); return 0; } u8 IMU_Get_Data(short *G_16Data, short*A_16Data) { u8 G_Data[6]={0}; u8 A_Data[6]={0}; if(IMU_Read_Byte(ASM330LHH_I2C_ADD_L_W,ASM330LHH_STATUS_REG)&0x02) { IMU_Read_Len(ASM330LHH_I2C_ADD_L_W,ASM330LHH_OUTX_L_G,2,&G_Data[0]); IMU_Read_Len(ASM330LHH_I2C_ADD_L_W,ASM330LHH_OUTY_L_G,2,&G_Data[2]); IMU_Read_Len(ASM330LHH_I2C_ADD_L_W,ASM330LHH_OUTZ_L_G,2,&G_Data[4]); G_16Data[0]=G_Data[1]<<8|G_Data[0]; G_16Data[1]=G_Data[3]<<8|G_Data[2]; G_16Data[2]=G_Data[5]<<8|G_Data[4]; CC2OC(G_16Data,3); } else { printf("Wait G_Data Update\n"); } if(IMU_Read_Byte(ASM330LHH_I2C_ADD_L_W,ASM330LHH_STATUS_REG)&0x01) { IMU_Read_Len(ASM330LHH_I2C_ADD_L_W,ASM330LHH_OUTX_L_A,2,&A_Data[0]); IMU_Read_Len(ASM330LHH_I2C_ADD_L_W,ASM330LHH_OUTY_L_A,2,&A_Data[2]); IMU_Read_Len(ASM330LHH_I2C_ADD_L_W,ASM330LHH_OUTZ_L_A,2,&A_Data[4]); A_16Data[0]=A_Data[1]<<8|A_Data[0]; A_16Data[1]=A_Data[3]<<8|A_Data[2]; A_16Data[2]=A_Data[5]<<8|A_Data[4]; CC2OC(A_16Data,3); } else { printf("Wait A_Data Update\n"); } return 0; } void CC2OC(short *Data,int n) //补码转原码 { int i=0; for(i=0;i<n;i++) { if(Data[i]&0x8000) //判定为负数 { Data[i]=~Data[i]+1; // 不考虑符号 // Data[i]=~Data[i]+1+0x8000; } } }