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);

}