以屏幕的左下方为原点(2d编程的时候,是以屏幕左上方为原点的,这个值得注意一下),箭头指向的方向为正。从-10到10,以浮点数为等级单位,想象一下以下情形:
手机屏幕向上(z轴朝天)水平放置的时侯,(x,y,z)的值分别为(0,0,10);
手机屏幕向下(z轴朝地)水平放置的时侯,(x,y,z)的值分别为(0,0,-10);
手机屏幕向左侧放(x轴朝天)的时候,(x,y,z)的值分别为(10,0,0);
手机竖直(y轴朝天)向上的时候,(x,y,z)的值分别为(0,10,0);
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);
}