2013.4.2，今天提交完代码，指南针的调试工作可以告一段落了。这段时间主要做了2项工作，1、写了一个自己的函数，在.c文件中去读acc的input event，因为原来的读值函数会引起驱动资源抢占。2、写了一个有效的滤波函数。滤波函数我前前后后写了4个，之前想的很复杂，今天下午看了一篇论文，试了下，发现原来有效的滤波函数如此简单，完全没有技术含量（取9次、报一次，去掉最大最小，取平均），如下（其它函数在分割下之前的版本中已经列出，见下文）：
//daiyyr add @2013.4.2
int acount, myx[9], myy[9];
//return 0 for collect; 1 for report
int Mean_filter(int16 *bData){
signed short i, x, y, z;
x = bData[1] + (bData[2] << 8);
y = bData[3] + (bData[4] << 8);
z = bData[5] + (bData[6] << 8);//don't do this
// printf("x:%d, y:%d, z:%d\n",x,y,z);//no z
myx[acount] = x;
myy[acount] = y;
acount++;
if (acount == 9){
signed short maxx = -1000, minx = 1000, maxy = -1000, miny = 1000, avgx = 0, avgy = 0;
acount = 0;
//do sort and average
for (i=0; i<9; i++){
if (maxx < myx[i])
maxx = myx[i];
else if (minx > myx[i])
minx = myx[i];
avgx += myx[i];
if (maxy < myy[i])
maxy = myy[i];
else if (miny > myy[i])
miny = myy[i];
avgy += myy[i];
// printf("avgx:%d, myx[i]:%d, avgy:%d, myy[i]:%d\n", avgx, myx[i], avgy, myy[i]);
}
avgx = (avgx - maxx - minx) / 7;
avgy = (avgy - maxy - miny) / 7;
// printf("bdata1:%x, bdata2:%x\n", bData[1], bData[2]);
bData[1] = avgx & ((int16)255);
bData[2] = avgx >> 8;
bData[3] = avgy & ((int16)255);
bData[4] = avgy >> 8;
// printf("maxx:%d, minx:%d, avgx:%d,avgy:%d; bdata1:%x, bdata2:%x,report!*********************\n",maxx, minx, avgx, avgy, bData[1], bData[2]);
return 0;
}
return 1;
}
----------------------------下面内容为2013.4.2之前--------------------------------------------------------------------------------------
7023Q
https://192.168.0.220:8443/svn/coffee/trunk
驱动 coffee/kernel/drivers/misc/akm8975.c
HAL device/cct/common/libsku7sensors/AkmSensor.cpp
HAL特殊线层 device/cct/common/libsku7sensors/ak8975/
在此开启线程system/core/rootdir
编译生成的守护进程的可执行文件在手机中的位置：/system/bin/akmd8975
8000R
驱动 \\cts-server\sourcecode\rockchip-update\kernel\drivers\input\sensors\compass
sensors/sensor-dev.c
HAL：hardware/rk29/sensor/st/ak8975/…
开启线程：device/rockchip/rk30sdk/init.rk30board.rc
板子的GPIO脚变了，所以先修改板子配置源文件：
HAL层向服务层上报数据之前，经过以下几个流程：
A:开机运行一个叫akm8975的进程。这个进程源代码位于HAL层。8000R是hardware/rk29/sensor/st/；7023Q是device/cct/common/libsku7sensors/ak8975/
B:当指南针应用被打开后，通过HAL调用到驱动的enable函数，设备开始产生中断。
C:此时，akm8975这个进程捕获这个中断，读取驱动获得的原始数据，并作一番神秘的修改，具体的修改函数被封装于HAL层的…./ak8975/libak8975/libak8975.a这个令人蛋疼菊紧的文件中，该文件的存在亵渎了自由软件精神，使业界良心荡然无存，让代码民工情何以堪。
D:之后这个进程呼叫ioctl与内核文件搞基，驱动的ioctl去调用驱动的报值函数AKECS_SetYPR，该函数通过input_report_abs上报。
E:HAL层通过读文件/dev/input/compass获取上报的值，并作最后的处理，最后报给服务层
rbuf[0] = prms->m_theta; // yaw 航向
rbuf[1] = prms->m_phi180; // pitch 俯仰角
rbuf[2] = prms->m_eta90; // roll 翻滚角
该器件最终输出到应用层的大约是这六个值：
磁场强度X轴、y轴、z轴、航向、俯仰角、翻滚角。其中俯仰角和翻滚角是依据重力传感器的值计算出的结果
最后调通的方法是，利用已经由可执行文件中的秘密函数计算出的x和y轴磁感强度值（即磁感线在水平面的投影值的分解值）
rbuf[9] = prms->m_hvec.u.x; // M_x
rbuf[10] = prms->m_hvec.u.y; // M_y
用arctan三角函数算出正北方向与手机的某个轴（x或y）的偏移角（实际上.a文件内部也是这样运算的），把角度值赋予：rbuf[0] // yaw，当设备处于水平面的时候，顶层就是凭借这一个值来判断方向的！而设备若存在俯仰和翻滚角，则根据另外几个数据计算补偿。
下面是几个可执行文件中的关键函数,我用它们架空了.a文件，即自己通过磁感设备和加速度感应设备计算6个上报的值：三轴磁数据，航向、俯仰、翻滚三个方位数据。
同时注意值得正负，习惯上，确定了设备的“底部”后，将底部抬起，俯仰角pitch为正，反之为负；将设备右侧抬起，翻滚角roll为正，反之为负；航向为设备“纵轴”与正北方向的顺时针偏离角度（yaw小于360°时顺时针旋转设备，yaw递增）。
现在的问题是我的Gsensor——bma020会频繁出现大的尖波，这样造成俯仰角和翻滚角也出现尖波。我在尝试使用卡曼滤波算法过滤尖波。
频繁大尖波的原因找到了。我之前一直纳闷，为什么当我运行akmd守护进程时，ACC本身的报值会出现尖波影响，硬件上，AKM影响ACC的可能性可以立刻排除。那就是软件了，我看了ACC的驱动，原来，通过input event报值和open dev/bma020 ioctl()报值，这两个报值方式调用的是同一个函数：int bma020_read_accel_xyz(bma020acc_t * acc)。而这个函数没有用自旋锁锁住，所以几乎可以肯定，当两个通过不同方式读acc值的进程同时运行时（ACC本身使用input报值，AKM通过ioctl读值），在上述函数里发生了内存抢占。
解决的方式有两个，1、给驱动函数bma020_read_accel_xyz加自旋锁；2、改变akmd读acc值的方式，通过input方式读值。
这个系统的设定是，不轮AKMD是否运行，ACC驱动不停地向input报值，所以相比用ioctl去读值，akmd去读ACC的input不会增加内核负担。
下面两个函数是用c语言写的读取acc的input event值的函数：
//daiyyr add @2013.03.30, to getting acc data by input. begin
static int accOpened = 0, fd;
extern int16_t acc_data[3]; //defined in main.c
int getAccData(void){
float fData[3];
int err = 1;
if (!accOpened){
fd = openAccInputEvent();
if (fd < 0){
printf("open acc input event failed\n");
return -1;
}
accOpened = 1;
}
struct input_event event;
while(err > 0){
err = read(fd, &event, sizeof(event));
if (err < 0){
printf("read err, fd=%d,err=%d\n", fd, err);
return -2;
}
printf("dy-code:%d, value:%d\n",event.code, event.value);
if(event.type == 0){
printf("dy-data[0]:%d, data[1]:%d, data[2]:%d\n",acc_data[0], acc_data[1], acc_data[2]);
return 0;
}
if(event.type == 2){
switch (event.code){
case 3:
acc_data[0] = event.value;
continue;
case 4:
acc_data[1] = event.value;
continue;
case 5:
acc_data[2] = event.value;
continue;
}
}
}
return 0;
}
int openAccInputEvent(void){
char *str, *p, dev[60];
int i, fd = -1;
str = "/dev/input/event";
strcpy(dev, str);
for(i=0;i<20;i++){
p = dev + strlen(dev);
*p++ = i+48;
*p = '\0';
// printf("mybuffer:%s\n", dev);
fd = open(dev,0);
if (fd>=0) {
char name[80];
if (ioctl(fd, EVIOCGNAME(sizeof(name) - 1), &name) < 1) {
name[0] = '\0';
}
if (!strcmp(name, "acc")) {
printf("open dev succeed\n");
return fd;
} else {
close(fd);
p--;
*p = '\0';
fd = -1;
}
}
else{
printf("err:open dev failed dev:%s\n", dev);
return -1;
}
}
return fd;
}
//daiyyr add end
主循环：
void MeasureSNGLoop(AK8975PRMS* prms)
{
BYTE i2cData[AKSC_BDATA_SIZE];
int16 i;
int16 bData[AKSC_BDATA_SIZE]; // Measuring block data
int16 ret;
int32 ch;
int32 doze;
int32_t delay;
AKMD_INTERVAL interval;
struct timespec tsstart, tsend;
if (openKey() < 0) {
DBGPRINT(DBG_LEVEL1,
"%s:%d Error.\n", __FUNCTION__, __LINE__);
return;
}
if (openFormation() < 0) {
DBGPRINT(DBG_LEVEL1,
"%s:%d Error.\n", __FUNCTION__, __LINE__);
return;
}
// Get initial interval
GetValidInterval(CSPEC_INTERVAL_SNG, &interval);
// Initialize
if(InitAK8975_Measure(prms) != AKD_SUCCESS){
return;
}
while(TRUE){
// Get start time
if (clock_gettime(CLOCK_REALTIME, &tsstart) < 0) {
DBGPRINT(DBG_LEVEL1,
"%s:%d Error.\n", __FUNCTION__, __LINE__);
return;
}
// Set to SNG measurement pattern (Set CNTL register)
if (AKD_SetMode(AK8975_MODE_SNG_MEASURE) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1,
"%s:%d Error.\n", __FUNCTION__, __LINE__);
return;
}
// .! : 获取 M snesor 的原始数据. 这里可能阻塞.
// Get measurement data from AK8975
// ST1 + (HXL + HXH) + (HYL + HYH) + (HZL + HZH) + ST2
// = 1 + (1 + 1) + (1 + 1) + (1 + 1) + 1 = 8 bytes
if (AKD_GetMagneticData(i2cData) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1,
"%s:%d Error.\n", __FUNCTION__, __LINE__);
return;
}
// Copy to local variable
// DBGPRINT(DBG_LEVEL3, "%s: bData(Hex)=", __FUNCTION__);
printf("dyyr-");
for(i=0; i<AKSC_BDATA_SIZE; i++){
bData[i] = i2cData[i];
// DBGPRINT(DBG_LEVEL3, "%02x,", bData[i]);
printf("%02x,", bData[i]);
}
printf("\n");
// DBGPRINT(DBG_LEVEL3, "\n");
D_WHEN_REPEAT(100,
"raw mag x : %d, raw mag y : %d, raw mag z : %d.",
(signed short)(bData[1] + (bData[2] << 8) ),
(signed short)(bData[3] + (bData[4] << 8) ),
(signed short)(bData[5] + (bData[6] << 8) ) );
// .! :
// Get acceelration sensor's measurement data.
if (GetAccVec(prms) != AKRET_PROC_SUCCEED) {
return;
}
/*
DBGPRINT(DBG_LEVEL3,
"%s: acc(Hex)=%02x,%02x,%02x\n", __FUNCTION__,
prms->m_avec.u.x, prms->m_avec.u.y, prms->m_avec.u.z);
*/
//printf("dyyr-MeasuringEventProcess");
ret = MeasuringEventProcess(
bData,
prms,
getFormation(),
interval.decimator,
CSPEC_CNTSUSPEND_SNG
);
// Check the return value
if(ret == AKRET_PROC_SUCCEED){
if(prms->m_cntSuspend > 0){
// Show message
DBGPRINT(DBG_LEVEL2,
"Suspend cycle count = %d\n", prms->m_cntSuspend);
}
else if (prms->m_callcnt <= 1){
// Check interval
if (AKD_GetDelay(&delay) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1,
"%s:%d Error.\n", __FUNCTION__, __LINE__);
} else {
GetValidInterval(delay, &interval);
}
}
//printf("dyyr- measureresulthook\n");
// Display(or dispatch) the result.
Disp_MeasurementResultHook(prms);
}
//下面几个是位于main.c 的报值函数和我的数值处理函数
/*!
Daiyyr@2013.03.29
Get acc data and convert to pitch and roll orientation
acc_data: acc data.
pitch: pitch orientation to report
roll: roll orientation to report
获取加速度数据并转换为俯仰角和翻滚角
acc_data:存储加速度数据
pitch:将上报的俯仰角
roll:将上报的翻滚角
*/
int16_t acc_data[3];
int acc2pitch_roll(int *pitch, int *roll)
{
if(getAccData() < 0)
return -1;
*pitch = acc_data[2] > 0 ? (acc_data[0] > 0 ? -11520+acc_data[0]*64/264*90 : 11520+acc_data[0]*64/248*90) : (acc_data[0] > 0 ? -acc_data[0]*64/264*90 : -acc_data[0]*64/248*90);
*roll = acc_data[1] > 0 ? acc_data[1]*64/242*90 : acc_data[1]*64/273*90;
return 0;
}
/*!
Daiyyr@2013.03.29
Calibration for x & y axis magnetic data.
*/
int xmax = 1, ymax = 1, xmin = 0, ymin = 0;
int mag_x_y_calibration(int *x, int *y){
int xsf, ysf, xoff, yoff;
// printf("xy:%d,%d\n", *x, *y);
if(*x > xmax)
xmax = *x;
else if(*x < xmin)
xmin = *x;
if(*y > ymax)
ymax = *y;
else if(*y < ymin)
ymin = *y;
xsf = 1 > (ymax-ymin)/(2*(xmax-ymin)) ? 1 : (ymax-ymin)/(2*(xmax-ymin));
ysf = 1 > (xmax-ymin)/(2*(ymax-ymin)) ? 1 : (xmax-ymin)/(2*(ymax-ymin));
xoff = ((xmax-xmin)/2-xmax)*xsf;
yoff = ((ymax-ymin)/2-ymax)*ysf;
// printf("xoff:%d, xsf:%d\n", xoff, xsf);
*x = xsf + *x + xoff;
*y = ysf + *y + yoff;
// printf("hhll:%d,%d,%d,%d\n", xmax, ymax, xmin, ymin);
return 0;
}
int16_t acc_data[3];
void Disp_MeasurementResultHook(AK8975PRMS * prms)
{
int err;
int16 acc[3]; /* 将缓存 acc sensor 返回的数据. */
if (!s_opmode) {
int rbuf[12] = { 0 };
// rbuf[0] = prms->m_theta; // yaw
// rbuf[1] = prms->m_phi180; // pitch
// rbuf[2] = prms->m_eta90; // roll
// rbuf[6] = prms->m_avec.u.x; // G_Sensor x
// rbuf[7] = prms->m_avec.u.y; // G_Sensor y
// rbuf[8] = prms->m_avec.u.z; // G_Sensor z
acc2pitch_roll(&rbuf[1], &rbuf[2]);
rbuf[3] = 25; // tmp (AK8975 doesn't have temperature sensor)
rbuf[4] = prms->m_hdst; // m_stat
rbuf[5] = 3; // g_stat
rbuf[9] = prms->m_hvec.u.x; // M_x
rbuf[10] = prms->m_hvec.u.y; // M_y
mag_x_y_calibration(&rbuf[9], &rbuf[10]);
rbuf[11] = prms->m_hvec.u.z; // M_z
rbuf[0] = axis2angle(rbuf[10], -rbuf[9]); // yaw
//printf("pitch=%d, roll=%d,\n", rbuf[1]/64, rbuf[2]/64);
/* .! : 将计算得到的结果回写到驱动. */
err=ioctl(g_file, ECS_IOCTL_SET_YPR, &rbuf); // 之后, 驱动会将该数据上报 sensor HAL.
}
/* 否则, ... */
else {
Disp_MeasurementResult(prms);
}
}
下面是用三角函数求偏移角度的函数
/*!
返回地磁感线在水平面的投影与【设备水平放置时y轴】的夹角 Daiyyr@2013.02.23
*/
int axis2angle(int x, int y)
{
double dx = x, dy = y;
double angle = 180/3.1415*atan2(dx, dy);
angle = angle >= 0 ? angle : (angle+360);
// printf("dyyr-angle: %f\n", angle);
return (int)(angle*64);
}