基于加速度传感器计算位置信息的学习过程

一下内容是对这篇文章的解读

【翻译】利用加速度求解位置的算法——三轴传感器 - cposture - 博客园

1. 校准程序

原文解释：

    这个校准程序移除了加速度传感器的输出偏移分量，因为存在重力加速度（静态的加速度）。

    校准程序在加速度计处于无运动状态时，对加速度求平均值。采样的数量越多，加速度的校准结果越精确。

voidCalibrate(void)
{
    unsignedint count1;
    count1 =0;
    do{
    ADC_GetAllAxis();
    sstatex = sstatex +Sample_X;// Accumulate Samples
    count1++;
    }while(count1!=0x0400);// 1024 times
    sstatex=sstatex>>10;// division between 1024
}

理解解读：

这一步的意思就是将本身z轴上的加速度给求出来，然后方便后面算运动产生的加速度（用当时滤波取出的加速度减此处算出的初始的加速度）。

在do-while循环中进行对sstatex的累加，累加完0x0400（1024次）后，用>>10位的方式进行取平均值，左移10位和除以1024是等价的，之前一直不理解这一步的取均值在哪。

2. 滤波

原文解释：

低通滤波是消除加速度计中信号噪音(包括机械的和电子的)很好的方法。为了当对信号进行积分时减少大部分的错误，减少噪音对定位应用是至关重要的。

    一个简单的低通过滤采样信号的方式是执行滚动平均值。过滤简单地获得一组采样的平均值，获得稳定的采样总数的平均值是很重要的。采样数太多可能造成数据丢失，而太少又可能导致结果不精确。

do{
    accelerationx[1]=accelerationx[1]+Sample_X;//filtering routine for noise attenuation
    count2++;// average represents the acceleration of
    // an instant.
}while(count2!=0x40);// 64 sums of the acceleration sample
accelerationx[1]= accelerationx[1]>>6;// division by 64

理解解读

将每一次的加速度输出结果accelerationx[1]进行64次叠加然后取均值得到数据滤波效果。

3. 机械滤波窗口

原文解释

    当处于无运动状态时，加速度上的微小错误可能会被当作一个常量速度，因为在采样值加和之后，它并不等于0。在没有运动的理想情况下，所有的采样值都为0。该常速表示一个连续的移动和不稳定的位置。

    即使之前过滤的一些数据可能不正确，因此一个用于区别无运动状态的"有效数据"和"无效数据"的窗口必须实现。

1 if ((accelerationx[1] <=3)&&(accelerationx[1] >= -3)) //Discrimination window applied to
2 {
3     accelerationx[1] = 0;
4 } // the X axis acceleration variable

理解解读

当某个时间微元中的加速度值在3和-3的范围中时我们判定此时对象的运动状态是停止的。

4. 定位

原文解释

二重积分是利用加速度数据获取位置所需要的步骤。第一次积分获得速度，第二次积分获得位置。

1 //first integration
2 velocitx[1]= velocitx[0]+ accelerationx[0]+((accelerationx[1]- accelerationx[0])>>1)
3 //second integration
4 positionX[1]= positionX[0]+ velocitx[0]+((velocitx[1]- velocitx[0])>>1);

 5. 数据传输

原文解释

这部分主要目的是用于调试和显示；这个函数中32位的结果被切割分4个8位数据。

if (positionX[1]>=0)
{ //This line compares the sign of the X direction data
    posx_seg[0]= positionX[1] & 0x000000FF; // is set to 1 else it is set to 8
    posx_seg[1]= (positionX[1]>>8) & 0x000000FF; // the data is also managed in the
    // subsequent lines in order to be sent.
    posx_seg[2]= (positionX[1]>>16) & 0x000000FF;// The 32 bit variable must be split into
    posx_seg[3]= (positionX[1]>>24) & 0x000000FF;// 4 different 8 bit variables in order to
    // be sent via the 8 bit SCI frame
}
else
{
    positionXbkp=positionX[1]-1;
    positionXbkp=positionXbkp^0xFFFFFFFF;
    posx_seg[0]= positionXbkp & 0x000000FF;
    posx_seg[1]= (positionXbkp>>8) & 0x000000FF;
    posx_seg[2]= (positionXbkp>>16) & 0x000000FF;
    posx_seg[3]= (positionXbkp>>24) & 0x000000FF;
}

 以上是我对开头的文章的初步解读，但还没实际运用，而且也是第一次啃这么硬的文章，如果用错误的地方，欢迎大家指出让我学习。

我发现原文的例程没有tab而且没有空行看起来太难受了，我翻译了部分函数解释和加入了基本的tab希望能够帮助大家理解

#include <hidef.h>
#include "derivative.h"
#include "adc.h"
#include "buzzer.h"
#include "SCItx.h"
#pragma DATA_SEG MY_ZEROPAGE
unsigned char near Sample_X;
unsigned char near Sample_Y;
unsigned char near Sample_Z;
unsigned char near Sensor_Data[8];
unsigned char near countx,county ;
signed int near accelerationx[2], accelerationy[2];
signed long near velocityx[2], velocityy[2];
signed long near positionX[2];
signed long near positionY[2];
signed long near positionZ[2];
unsigned char near direction;
unsigned long near sstatex,sstatey;
#pragma DATA_SEG DEFAULT
void init(void);
void Calibrate(void);
void data_transfer(void);
void concatenate_data(void);
void movement_end_check(void);
void position(void);
void main (void)
{
 init();
 get_threshold();
 do
 {
 position();
 }while(1);
}
/*******************************************************************************
校准例程的目的是获得参考阈值的值。

在无运动条件下，它由1024个样本平均值组成。
********************************************************************************/
void Calibrate(void)
{
	 unsigned int count1;
	 count1 = 0;
	 
	 do{
		 ADC_GetAllAxis();
		 sstatex = sstatex + Sample_X; // 积累的样本
		 sstatey = sstatey + Sample_Y;
		 count1++;
	 }while(count1!=0x0400); // 1024 times
	 
	 
	 sstatex=sstatex>>10; // division between 1024
	 sstatey=sstatey>>10;
}
/*****************************************************************************************/
/******************************************************************************************
这个函数得到大小和方向

在这个特殊的协议中，方向和大小以不同的变量发送。

管理可以通过许多其他不同的方式来完成。
*****************************************************************************************/
void data_transfer(void)
{
	 signed long positionXbkp;
	 signed long positionYbkp;
	 unsigned int delay;
	 unsigned char posx_seg[4], posy_seg[4];
	 if (positionX[1]>=0)
	 { //这一行比较X方向数据的符号
		 direction= (direction | 0x10); //如果它是正的，那就是最高位字节
		 posx_seg[0]= positionX[1] & 0x000000FF; // is set to 1 else it is set to 8
		 posx_seg[1]= (positionX[1]>>8) & 0x000000FF; // the data is also managed in the
		 // subsequent lines in order to
		 posx_seg[2]= (positionX[1]>>16) & 0x000000FF; // be sent. The 32 bit variable must be
		 posx_seg[3]= (positionX[1]>>24) & 0x000000FF; // split into 4 different 8 bit
		 // variables in order to be sent via
		 // the 8 bit SCI frame
	}
	else {direction=(direction | 0x80);
		 positionXbkp=positionX[1]-1;
		 positionXbkp=positionXbkp^0xFFFFFFFF;
		 posx_seg[0]= positionXbkp & 0x000000FF;
		 posx_seg[1]= (positionXbkp>>8) & 0x000000FF;
		 posx_seg[2]= (positionXbkp>>16) & 0x000000FF;
		 posx_seg[3]= (positionXbkp>>24) & 0x000000FF;
	}
	if (positionY[1]>=0) { // Same management than in the previous case
		 direction= (direction | 0x08); // but with the Y data.
		 posy_seg[0]= positionY[1] & 0x000000FF;
		 posy_seg[1]= (positionY[1]>>8) & 0x000000FF;
		 posy_seg[2]= (positionY[1]>>16) & 0x000000FF;
		 posy_seg[3]= (positionY[1]>>24) & 0x000000FF;
	}
	else {direction= (direction | 0x01);
		 positionYbkp=positionY[1]-1;
		 positionYbkp=positionYbkp^0xFFFFFFFF;
		 posy_seg[0]= positionYbkp & 0x000000FF;
		 posy_seg[1]= (positionYbkp>>8) & 0x000000FF;
		 posy_seg[2]= (positionYbkp>>16) & 0x000000FF;
		 posy_seg[3]= (positionYbkp>>24) & 0x000000FF;
	}
	 delay = 0x0100;
	 Sensor_Data[0] = 0x03;
	 Sensor_Data[1] = direction;
	 Sensor_Data[2] = posx_seg[3];
	 Sensor_Data[3] = posy_seg[3];
	 Sensor_Data[4] = 0x01;
	 Sensor_Data[5] = 0x01;
	 Sensor_Data[6] = END_OF_FRAME;
	 while (--delay);
	 SCITxMsg(Sensor_Data); // Data transferring function
	 while (SCIC2 & 0x08);
}
/*****************************************************************************************/
/******************************************************************************************
这个函数将数据格式返回到原始状态。当得到大小和

在位置的方向上，做一个逆二补。这个函数等于2

补充，以便将数据返回到原始状态。

重要的是要注意，灵敏度调整在这里受到很大的影响

在掩模中插入的“1”必须等于在移位中丢失的“1”

对前面的功能进行敏感性修改。
 ******************************************************************************************/
void data_reintegration(void)
{
	 if (direction >=10)
	{
		positionX[1]= positionX[1]|0xFFFFC000;
	} 
	// 18 "ones" inserted. Same size as the
	 //amount of shifts
	 
	 direction = direction & 0x01;
	 if (direction ==1)
	 {
		positionY[1]= positionY[1]|0xFFFFC000;
	}
}
/******************************************************************************************
此功能允许运动结束检测。如果有一定数量的加速度样本

等于0，我们可以假设运动停止了。速度产生的累积误差

通过重新设置速度变量来消除计算。这将停止位置增量

并大大消除了位置误差。
******************************************************************************************/
void movement_end_check(void)
{
	if (accelerationx[1]==0) //we count the number of acceleration samples that equals cero
	{ countx++;}
		else { countx =0;}
		if (countx>=25) //if this number exceeds 25, we can assume that velocity is cero
		{
			velocityx[1]=0;
			velocityx[0]=0;
		}
	if (accelerationy[1]==0) //we do the same for the Y axis
	{ county++;}
	else { county =0;}
	if (county>=25)
	{
		velocityy[1]=0;
		velocityy[0]=0;
	}
}
/*****************************************************************************************/
/******************************************************************************************
这个函数通过积分将加速度转换成一个比例位置

两次加速数据。它还通过乘以“positionX”和来调整灵敏度

“positionY”变量。

这个积分算法带有误差，在“movenemt_end_check”中得到补偿。

子例程。更快的采样频率意味着更少的错误，但需要更多的内存。保持

记住，同样的过程也适用于X轴和Y轴
*****************************************************************************************/
void position(void)
{
	unsigned char count2 ;
	count2=0;

	 do{
		 ADC_GetAllAxis();
		 accelerationx[1]=accelerationx[1] + Sample_X; //filtering routine for noise attenuation
		 accelerationy[1]=accelerationy[1] + Sample_Y; //64 samples are averaged. The resulting
		//average represents the acceleration of
		 //an instant
		 count2++;
	 }
	 while (count2!=0x40); // 64 sums of the acceleration sample
	 
	 
	 accelerationx[1]= accelerationx[1]>>6; // division by 64
	 accelerationy[1]= accelerationy[1]>>6;
	 accelerationx[1] = accelerationx[1] - (int)sstatex; //eliminating zero reference
	 //offset of the acceleration data
	 accelerationy[1] = accelerationy[1] - (int)sstatey; // to obtain positive and negative
	 
	 
	 //acceleration
	 if ((accelerationx[1] <=3)&&(accelerationx[1] >= -3)) //Discrimination window applied
	 {accelerationx[1] = 0;} // to the X axis acceleration
	 
	 
	 //variable
	 if ((accelerationy[1] <=3)&&(accelerationy[1] >= -3))
	 {accelerationy[1] = 0;}
	 
	 
	 //first X integration:
	velocityx[1]= velocityx[0]+ accelerationx[0]+ ((accelerationx[1] -accelerationx[0])>>1);
	 //second X integration:
	positionX[1]= positionX[0] + velocityx[0] + ((velocityx[1] - velocityx[0])>>1);
	 //first Y integration:
	velocityy[1] = velocityy[0] + accelerationy[0] + ((accelerationy[1] -accelerationy[0])>>1);
	 //second Y integration:
	positionY[1] = positionY[0] + velocityy[0] + ((velocityy[1] - velocityy[0])>>1);
	
	
	 accelerationx[0] = accelerationx[1]; //The current acceleration value must be sent
	//to the previous acceleration
	 accelerationy[0] = accelerationy[1]; //variable in order to introduce the new
	//acceleration value.
	
	
	 velocityx[0] = velocityx[1]; //Same done for the velocity variable
	 velocityy[0] = velocityy[1];
	 positionX[1] = positionX[1]<<18; //The idea behind this shifting (multiplication)
	 //is a sensibility adjustment.
	 positionY[1] = positionY[1]<<18; //Some applications require adjustments to a
	 //particular situation
	 //i.e. mouse application
	 data_transfer();
	 positionX[1] = positionX[1]>>18; //once the variables are sent them must return to
	 positionY[1] = positionY[1]>>18; //their original state
	 movement_end_check();
	 positionX[0] = positionX[1]; //actual position data must be sent to the
	 positionY[0] = positionY[1]; //previous position
	 direction = 0; // data variable to direction variable reset
}
/*****************************************************************************************/