【嵌入式模块芯片开发】ADXL345的优化精确测量和角度计算（中断单次测量、卡尔曼滤波）
文章目录
ADXL345的一般读取方式
要将ADXL345_POWER_CTL寄存器写入0x08打开测量功能
ADXL345_DATA_FORMAT寄存器采用默认的十位数据 ±2g
十位精度就是1024
±2g也就是4g
4/1024=0.00390625g
其他寄存器可以不配置
读取时 读取X Y Z值
转成有符号整型再*单位即可
void Init_ADXL345(void)
{
uint8_t dat=0;
// dat = ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DEVID);
// printf("[INFO] ADXL345_DEVID: %x\n",dat);
WriteOneByte((ADXL345_Slave_Add<<1),ADXL345_POWER_CTL,0x08);
WriteOneByte((ADXL345_Slave_Add<<1),ADXL345_DATA_FORMAT,0x00);
}
void Count_ADXL345(void)
{
uint8_t dat_H=0;
uint8_t dat_L=0;
uint16_t dat=0;
float x=0.0f;
float y=0.0f;
float z=0.0f;
dat_L = ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAX0);
dat_H = ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAX1);
dat = (dat_H<<8)|dat_L;
x=((int16_t)dat)*0.00390625f;
dat_L = ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAY0);
dat_H = ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAY1);
dat = (dat_H<<8)|dat_L;
y=((int16_t)dat)*0.00390625f;
dat_L = ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAZ0);
dat_H = ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAZ1);
dat = (dat_H<<8)|dat_L;
z=((int16_t)dat)*0.00390625f;
printf("[INFO] x: %0.4f y: %0.4f z: %0.4f\n",x,y,z);
}
但是这样有个弊端 就是读取时 是寄存器依次写入 可能造成两次数据不是同一个测量时间的
那么就会有概率数据出错
ADXL345的中断读取方式（单次测量）
利用中断 即可实现单次测量
通过控制中断的DATA_READY位来判断是否测量完成
初始化时 先置一
测量时 先将各个数据寄存器清空
然后再将ADXL345_POWER_CTL寄存器写入0x08开启测量
并且不断读取ADXL345_INT_SOURCE位 判断是否已经产生中断
中断产生则ADXL345_POWER_CTL写入0x00关闭测量
在判断ADXL345_INT_SOURCE是否产生中断时 也需要判断D0位是否被置一（溢出）
若溢出 则清空数据寄存器
角度计算
ADXL345得到的是XYZ三轴上的加速度大小
根据三角函数关系 通过重力加速度计求角度
得到如下公式：
printf("[INFO] Ax: %0.4f Ay: %0.4f Az: %0.4f\n",x,y,z);
x_2=pow(x,2);
y_2=pow(y,2);
z_2=pow(z,2);
x_rad = atan(x/(sqrt(y_2+z_2)));
y_rad = atan(y/(sqrt(x_2+z_2)));
z_rad = atan(z/(sqrt(y_2+x_2)));
x_cir=x_rad/pi*180;
y_cir=y_rad/pi*180;
z_cir=z_rad/pi*180;
printf("[INFO] x: %0.4f y: %0.4f z: %0.4f\n",x_cir,y_cir,z_cir);
卡尔曼滤波
为了减小ADXL345的数据方差 通过卡尔曼滤波的方式进行
最终测量结果显示，滤波前，x、y的角度方差都比较稳定，为0.04-0.05左右，而z角度的方差变化较大，甚至出现了一次0.08的方差。滤波前，峰峰值都为1.2-1.4。
滤波后，方差、峰峰值显著减小，基本为0.008-0.012左右，但z轴仍然效果欠佳。
参考：
【C语言/Python】嵌入式常用数据滤波处理：卡尔曼滤波器的简易实现方式（Kalman Filter）
优化后完整代码
相关代码如下：
void Reset_ADXL345_DATA(void)
{
ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAX0);
ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAX1);
ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAY0);
ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAY1);
ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAZ0);
ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAZ1);
}
bool While_ADXL345_Ready(void)
{
uint8_t dat=0;
uint8_t i=0;
for(i=0;i<10;i++)
{
dat=ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_INT_SOURCE);
if((dat&0x80) && !(dat&0x01))
{
return true;
}
delay_ms(10);
if((dat&0x01))
{
Reset_ADXL345_DATA();
}
}
return false;
}
bool Wait_ADXL345_Ready(void)
{
Reset_ADXL345_DATA();
WriteOneByte((ADXL345_Slave_Add<<1),ADXL345_POWER_CTL,0x08);
delay_ms(10);
if(While_ADXL345_Ready())
{
// Reset_ADXL345_DATA();
// if(While_ADXL345_Ready())
// {
WriteOneByte((ADXL345_Slave_Add<<1),ADXL345_POWER_CTL,0x00);
return true;
// }
}
return false;
}
void Count_ADXL345(void)
{
uint8_t dat_H=0;
uint8_t dat_L=0;
uint16_t dat=0;
float x=0.0f;
float y=0.0f;
float z=0.0f;
uint8_t i=0;
uint8_t Error_Flag=0;
float x_sum=0.0f;
float y_sum=0.0f;
float z_sum=0.0f;
// float x_min=0.0f;
// float y_min=0.0f;
// float z_min=0.0f;
//
// float x_max=0.0f;
// float y_max=0.0f;
// float z_max=0.0f;
double x_2=0.0f;
double y_2=0.0f;
double z_2=0.0f;
double x_rad=0.0f;
double y_rad=0.0f;
double z_rad=0.0f;
float x_cir=0.0f;
float y_cir=0.0f;
float z_cir=0.0f;
double pi=3.14159265358979;
for(i=0;i<1;i++)
{
if(Wait_ADXL345_Ready())
{
dat_L = ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAX0);
dat_H = ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAX1);
dat = (dat_H<<8)|dat_L;
x=((int16_t)dat)*0.00390625f;
dat_L = ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAY0);
dat_H = ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAY1);
dat = (dat_H<<8)|dat_L;
y=((int16_t)dat)*0.00390625f;
dat_L = ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAZ0);
dat_H = ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAZ1);
dat = (dat_H<<8)|dat_L;
z=((int16_t)dat)*0.00390625f;
x_sum=x+x_sum;
y_sum=y+y_sum;
z_sum=z+z_sum;
}
else
{
Error_Flag++;
}
}
if(Error_Flag>=1)
{
printf("[INFO] ADXL345 Error\n");
return;
}
x=x_sum/(1.0f-Error_Flag);
y=y_sum/(1.0f-Error_Flag);
z=z_sum/(1.0f-Error_Flag);
printf("[INFO] Ax: %0.4f Ay: %0.4f Az: %0.4f\n",x,y,z);
x_2=pow(x,2);
y_2=pow(y,2);
z_2=pow(z,2);
x_rad = atan(x/(sqrt(y_2+z_2)));
y_rad = atan(y/(sqrt(x_2+z_2)));
z_rad = atan(z/(sqrt(y_2+x_2)));
x_cir=x_rad/pi*180;
y_cir=y_rad/pi*180;
z_cir=z_rad/pi*180;
printf("[INFO] x: %0.4f y: %0.4f z: %0.4f\n",x_cir,y_cir,z_cir);
ADXL345_X_Stu.Measure_Now=x_cir;
ADXL345_Y_Stu.Measure_Now=y_cir;
ADXL345_Z_Stu.Measure_Now=z_cir;
ADXL345_X_Stu=Kalman_Filter_Normal(ADXL345_X_Stu);
ADXL345_Y_Stu=Kalman_Filter_Normal(ADXL345_Y_Stu);
ADXL345_Z_Stu=Kalman_Filter_Normal(ADXL345_Z_Stu);
printf("[INFO] Kx: %0.4f Ky: %0.4f Kz: %0.4f\n",ADXL345_X_Stu.Result_Now,ADXL345_Y_Stu.Result_Now,ADXL345_Z_Stu.Result_Now);
}
void Init_ADXL345(void)
{
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_8,GPIO_PIN_SET);
WriteOneByte((ADXL345_Slave_Add<<1),ADXL345_POWER_CTL,0x08);
WriteOneByte((ADXL345_Slave_Add<<1),ADXL345_DATA_FORMAT,0x00);
WriteOneByte((ADXL345_Slave_Add<<1),ADXL345_INT_MAP,0x80);
WriteOneByte((ADXL345_Slave_Add<<1),ADXL345_INT_ENABLE,0x80);
ADXL345_X_Stu.Q=0.1f;
ADXL345_Y_Stu.Q=0.1f;
ADXL345_Z_Stu.Q=0.1f;
ADXL345_X_Stu.R=0.5f;
ADXL345_Y_Stu.R=0.5f;
ADXL345_Z_Stu.R=0.5f;
ADXL345_X_Stu.Result_Last=0.0f;
ADXL345_X_Stu.Prediction_Last=0.0f;
ADXL345_X_Stu.Result_Now=0.0f;
ADXL345_Y_Stu.Result_Last=0.0f;
ADXL345_Y_Stu.Prediction_Last=0.0f;
ADXL345_Y_Stu.Result_Now=0.0f;
ADXL345_Z_Stu.Result_Last=90.0f;
ADXL345_Z_Stu.Prediction_Last=90.0f;
ADXL345_Z_Stu.Result_Now=90.0f;
}
附录：压缩字符串、大小端格式转换
压缩字符串
首先HART数据格式如下：
重点就是浮点数和字符串类型
Latin-1就不说了 基本用不到
浮点数
浮点数里面 如 0x40 80 00 00表示4.0f
在HART协议里面 浮点数是按大端格式发送的 就是高位先发送 低位后发送
发送出来的数组为：40,80,00,00
但在C语言对浮点数的存储中 是按小端格式来存储的 也就是40在高位 00在低位
浮点数：4.0f
地址0x1000对应00
地址0x1001对应00
地址0x1002对应80
地址0x1003对应40
若直接使用memcpy函数 则需要进行大小端转换 否则会存储为：
地址0x1000对应40
地址0x1001对应80
地址0x1002对应00
地址0x1003对应00
大小端转换：
void swap32(void * p)
{
uint32_t *ptr=p;
uint32_t x = *ptr;
x = (x << 16) | (x >> 16);
x = ((x & 0x00FF00FF) << 8) | ((x >> 8) & 0x00FF00FF);
*ptr=x;
}
压缩Packed-ASCII字符串
本质上是将原本的ASCII的最高2位去掉 然后拼接起来 比如空格(0x20)
四个空格拼接后就成了
1000 0010 0000 1000 0010 0000
十六进制：82 08 20
对了一下表 0x20之前的识别不了
也就是只能识别0x20-0x5F的ASCII表
压缩/解压函数后面再写：
//传入的字符串和数字必须提前声明 且字符串大小至少为str_len 数组大小至少为str_len%4*3 str_len必须为4的倍数
uint8_t Trans_ASCII_to_Pack(uint8_t * str,uint8_t * buf,const uint8_t str_len)
{
if(str_len%4)
{
return 0;
}
uint8_t i=0;
memset(buf,0,str_len/4*3);
for(i=0;i<str_len;i++)
{
if(str[i]==0x00)
{
str[i]=0x20;
}
}
for(i=0;i<str_len/4;i++)
{
buf[3*i]=(str[4*i]<<2)|((str[4*i+1]>>4)&0x03);
buf[3*i+1]=(str[4*i+1]<<4)|((str[4*i+2]>>2)&0x0F);
buf[3*i+2]=(str[4*i+2]<<6)|(str[4*i+3]&0x3F);
}
return 1;
}
//传入的字符串和数字必须提前声明 且字符串大小至少为str_len 数组大小至少为str_len%4*3 str_len必须为4的倍数
uint8_t Trans_Pack_to_ASCII(uint8_t * str,uint8_t * buf,const uint8_t str_len)
{
if(str_len%4)
{
return 0;
}
uint8_t i=0;
memset(str,0,str_len);
for(i=0;i<str_len/4;i++)
{
str[4*i]=(buf[3*i]>>2)&0x3F;
str[4*i+1]=((buf[3*i]<<4)&0x30)|(buf[3*i+1]>>4);
str[4*i+2]=((buf[3*i+1]<<2)&0x3C)|(buf[3*i+2]>>6);
str[4*i+3]=buf[3*i+2]&0x3F;
}
return 1;
}
大小端转换
在串口等数据解析中 难免遇到大小端格式问题
什么是大端和小端
所谓的大端模式，就是高位字节排放在内存的低地址端，低位字节排放在内存的高地址端。
所谓的小端模式，就是低位字节排放在内存的低地址端，高位字节排放在内存的高地址端。
简单来说：大端——高尾端，小端——低尾端
举个例子，比如数字 0x12 34 56 78在内存中的表示形式为：
1)大端模式：
低地址 -----------------> 高地址
0x12 | 0x34 | 0x56 | 0x78
2)小端模式：
低地址 ------------------> 高地址
0x78 | 0x56 | 0x34 | 0x12
可见，大端模式和字符串的存储模式类似。
数据传输中的大小端
比如地址位、起止位一般都是大端格式
如：
起始位：0x520A
则发送的buf应为{0x52,0x0A}
而数据位一般是小端格式（单字节无大小端之分）
如：
一个16位的数据发送出来为{0x52,0x0A}
则对应的uint16_t类型数为： 0x0A52
而对于浮点数4.0f 转为32位应是：
40 80 00 00
以大端存储来说 发送出来的buf就是依次发送 40 80 00 00
以小端存储来说 则发送 00 00 80 40
由于memcpy等函数 是按字节地址进行复制 其复制的格式为小端格式 所以当数据为小端存储时 不用进行大小端转换
如：
uint32_t dat=0;
uint8_t buf[]={0x00,0x00,0x80,0x40};
memcpy(&dat,buf,4);
float f=0.0f;
f=*((float*)&dat); //地址强转
printf("%f",f);
或更优解：
uint8_t buf[]={0x00,0x00,0x80,0x40};
float f=0.0f;
memcpy(&f,buf,4);
而对于大端存储的数据（如HART协议数据 全为大端格式） 其复制的格式仍然为小端格式 所以当数据为小端存储时 要进行大小端转换
如：
uint32_t dat=0;
uint8_t buf[]={0x40,0x80,0x00,0x00};
memcpy(&dat,buf,4);
float f=0.0f;
swap32(&dat); //大小端转换
f=*((float*)&dat); //地址强转
printf("%f",f);
或：
uint8_t buf[]={0x40,0x80,0x00,0x00};
memcpy(&dat,buf,4);
float f=0.0f;
swap32(&f); //大小端转换
printf("%f",f);
或更优解：
uint32_t dat=0;
uint8_t buf[]={0x40,0x80,0x00,0x00};
float f=0.0f;
dat=(buf[0]<<24)|(buf[0]<<16)|(buf[0]<<8)|(buf[0]<<0)
f=*((float*)&dat);
总结
固 若数据为小端格式 则可以直接用memcpy函数进行转换 否则通过移位的方式再进行地址强转
对于多位数据 比如同时传两个浮点数 则可以定义结构体之后进行memcpy复制（数据为小端格式）
对于小端数据 直接用memcpy写入即可 若是浮点数 也不用再进行强转
对于大端数据 如果不嫌麻烦 或想使代码更加简洁（但执行效率会降低） 也可以先用memcpy写入结构体之后再调用大小端转换函数 但这里需要注意的是 结构体必须全为无符号整型 浮点型只能在大小端转换写入之后再次强转 若结构体内采用浮点型 则需要强转两次
所以对于大端数据 推荐通过移位的方式来进行赋值 然后再进行个别数的强转 再往通用结构体进行写入
多个不同变量大小的结构体 要主要字节对齐的问题
可以用#pragma pack(1) 使其对齐为1
但会影响效率
大小端转换函数
直接通过对地址的操作来实现 传入的变量为32位的变量
中间变量ptr是传入变量的地址
void swap16(void * p)
{
uint16_t *ptr=p;
uint16_t x = *ptr;
x = (x << 8) | (x >> 8);
*ptr=x;
}
void swap32(void * p)
{
uint32_t *ptr=p;
uint32_t x = *ptr;
x = (x << 16) | (x >> 16);
x = ((x & 0x00FF00FF) << 8) | ((x >> 8) & 0x00FF00FF);
*ptr=x;
}
void swap64(void * p)
{
uint64_t *ptr=p;
uint64_t x = *ptr;
x = (x << 32) | (x >> 32);
x = ((x & 0x0000FFFF0000FFFF) << 16) | ((x >> 16) & 0x0000FFFF0000FFFF);
x = ((x & 0x00FF00FF00FF00FF) << 8) | ((x >> 8) & 0x00FF00FF00FF00FF);
*ptr=x;
}
8287
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
