Android设备中实现Orientation Sensor(图)兼谈陀螺仪

设备中的三自由度Orientation Sensor就是一个可以识别设备相对于地面，绕x、y、z轴转动角度的感应器(自己的理解，不够严谨)。智能手机，平板电脑有了它，可以实现很多好玩的应用，比如说指南针等。

 

我们可以用一个磁场感应器(magnetic sensor)来实现。

 

磁场感应器是用来测量磁场感应强度的。一个3轴的磁sensor IC可以得到当前环境下X、Y和Z方向上的磁场感应强度，对于Android中间层来说就是读取该感应器测量到的这3个值。当需要时，上报给上层应用程序。磁感应强度的单位是T（特斯拉）或者是Gs（高斯），1T等于10000Gs。

 

先来看看android定义的坐标系，在/hardware/libhardware/include/hardware/sensors.h中有个图。

图中表示设备的正上方是y轴方向，右边是x轴方向，垂直设备屏幕平面向上的是Z轴方向，这个很重要。因为应用程序就是根据这样的定义来写的，所以我们报给应用的数据要跟这个定义符合。还需要清楚磁sensor芯片贴在板上的坐标系。我们从芯片读出数据后要把芯片的坐标系转换为设备的实际坐标系。除非芯片贴在板上刚好跟设备的x、y、z轴方向刚好一致（去感谢你的硬件工程师吧）。

 

Orientation Sensor的实现是根据磁场感应强度的3个值计算出另外3个值。当需要时，我们计算出这3个值上报给应用程序，Orientation Sensor的功能就实现了。

这3个值具体含义和计算方法是：

 

1. azimuth 方位角：就是绕z轴转动的角度，0度=正北,（假设Y轴指向地磁正北方，直升机正前方的方向如下图）

 

90度=正东,

 

180度=正南,

 

270度=正西。

 

求x和y方向的磁感应强度的反正切，就可以得到方位角（算法看后面poll函数中的代码）。要实现指南针，只需要这个就可以了（不考虑设备非水平的情况）；

 

2. pitch 仰俯：绕X轴转动的角度 (-180<=pitch<=180), 如果设备水平放置，前方向下俯就是正，如图：

 

前方向上仰就是负值；

求磁sensor的y和z反正切可得到此角度值。

 

3. roll 滚转：绕Y轴转动(-90<=roll<=90)，向左翻滚是正值

 

向右翻滚是负值；

 

求z和x的反正切可得到此值。

 

 

sensors.h中还定义了其他各种sensor。要实现的就是这两个：

#define SENSOR_TYPE_MAGNETIC_FIELD      2

#define SENSOR_TYPE_ORIENTATION         3

 

在/hardware/sensors/sensors.cpp 中添加对MAGNETIC_FIELD和ORIENTATION 的支持

 

简单的说一下怎样添加,下面的代码不完整，请参考/sdk/emulator/sensors/sensors_qemu.c

 

[cpp] view plaincopy

1. //加入需要的宏定义   
2. #define  ID_BASE           SENSORS_HANDLE_BASE   

#define  ID_ACCELERATION   (ID_BASE+0)   

#define  ID_MAGNETIC_FIELD (ID_BASE+1)   

#define  ID_ORIENTATION (ID_BASE+2)   

#define S_HANDLE_ACCELEROMETER      (1<<ID_ACCELERATION)   

#define S_HANDLE_MAGNETIC_FIELD           (1<<ID_MAGNETIC_FIELD)   

#define S_HANDLE_ORIENTATION                 (1<<ID_ORIENTATION)   

#define SENSORS_NUM 4   

#define SUPPORTED_SENSORS  ((1<<NUM_SENSORS)-1)   

//在 sensor_t sensors_list[] 中添加两个sensor的信息,   

//这些只是一些Sensor的信息，应用程序可以获取到。   

#ifdef MAGNETIC_FIELD   

    {  

        name       : "XXX 3-axis Magnetic field sensor",  

        vendor    : "XXX company",  

        version    : 1,  

        handle     : S_HANDLE_MAGNETIC_FIELD,  

        type       : SENSOR_TYPE_MAGNETIC_FIELD,  

        maxRange   : 600.0f,//最大范围   

        resolution : 30.0f,//最小分辨率   

        power      : 6.7f,//这个不太懂   

    },  

#endif   

#ifdef ORIENTATION   

    {  

        name: "XXX Orientation sensor",  

        vendor: "XXX company",  

        version: 1,  

        handle: S_HANDLE_ORIENTATION,  

        type: SENSOR_TYPE_ORIENTATION,  

        maxRange: 360,    

        resolution: 0.1,   

        power: 20,   

    },  

#endif   

//定义一个结构来保存orientation的信息   

static struct orientation{  

    float azimuth;  

    float pitch;  

    float roll;  

}orientation;  

//在 control__open_data_source()函数中打开设备   

static native_handle_t*  

control__open_data_source(struct sensors_control_device_t *dev)  

{  

    SensorControl*  ctl = (void*)dev;  

    native_handle_t* handle;  

    int fd_m = open (MAGNETIC_DATA_DEVICE, O_RDONLY);  

    LOGD ("Open Magnetic Data source: %d, %d/n", fd_m, errno);  

    if (fd_m>= 0)   

    {  

        dev->fd[ID_MAGNETIC_FIELD] = dup(fd_m);  

    }  

    return handle;  

}  

//实现数据的打开和关闭函数   

static int  

data__data_open(struct sensors_data_device_t *dev, native_handle_t* handle)  

{  

    struct sensors_data_context_t *dev;  

    dev = (struct sensors_data_context_t *)device;  

    for(int i=0 ;i<SENSORS_NUM; i++)  

    {  

        dev->fd[i] = dup(handle->data[i]);  

    }  

    native_handle_close(handle);  

    native_handle_delete(handle);  

    return 0;  

}  

static int  

data__data_close(struct sensors_data_device_t *dev)  

{  

    struct sensors_data_context_t *dev;  

    dev = (struct sensors_data_context_t *)device;  

      

    for(int i=0 ;i<SENSORS_NUM; i++)  

    {  

        if (dev->fd[i] >= 0)  

        {  

            close(dev->fd[i]);  

        }  

        dev->fd[i] = -1;  

    }  

    return 0;  

}  

//最关键的poll函数   

static int  

data__poll(struct sensors_data_device_t *dev, sensors_data_t* values)  

{  

    SensorData*  data = (void*)dev;  

    int fd = data->events_fd;  

    //判断设备是否打开   

    if(dev->fd[ID_MAGNETIC_FIELD] < 0)  

    {  

        LOGD("In %s dev[%d] is not open!/n",__FUNCTION__ ,ID_MAGNETIC_FIELD);  

        return -1;  

    }  

    pollfd pfd[SENSORS_NUM] =   

    {  

        //省略其他sensor代码   

        {  

            fd: dev->fd[ID_MAGNETIC_FIELD],   

            events: POLLIN,   

            revents: 0  

        },  

        //省略其他sensor代码   

    };  

    int err = poll (pfd, SENSORS_NUM, s_timeout);  

      

    unsigned int  mask = SUPPORTED_SENSORS;  

    static unsigned int poll_flag=0;  

    if(poll_flag==0)  

    {  

        poll_flag = mask;  

    }  

    //省略其他sensor   

    if(poll_flag&(1<<ID_MAGNETIC_FIELD))  

    {  

        if((pfd[ID_MAGNETIC_FIELD].revents&POLLIN) == POLLIN)  

        {  

            char rawData[6];  

            err = read (dev->fd[ID_MAGNETIC_FIELD], &rawData, sizeof(rawData));  

            if(err<0)  

            {  

                LOGE("read magnetic field ret:%d errno:%d/n", err, errno);  

                return err;  

            }  

            struct timespec t;  

            clock_gettime(CLOCK_REALTIME, &t);  

            data->time = timespec_to_ns(&t);  

            data->sensor = SENSOR_TYPE_MAGNETIC_FIELD;  

            data->magnetic.status = SENSOR_STATUS_ACCURACY_HIGH;  

            //上报的数据单位要转换成 uTesla   

            data->magnetic.x = ( (rawData[1] << 8 ) | rawData[0])/ MAGNETIC_CONVERT;  

            data->magnetic.y = ( (rawData[3] << 8 ) | rawData[2])/ MAGNETIC_CONVERT;  

            data->magnetic.z = ( (rawData[5] << 8 ) | rawData[4])/ MAGNETIC_CONVERT;  

              

            //把陀螺仪需要的数据计算出来，用atan2()，头文件要加上#include <math.h>   

            float azimuth = atan2(  (float)(data->magnetic.x ),(float)(data->magnetic.y) );  

            if(azimuth<0)  

            {  

                azimuth = 360 - fabs(azimuth*180/PI);  

            }  

            else  

            {  

                azimuth = azimuth*180/PI;  

            }  

            orientation.azimuth = 360-azimuth;   

              

            //rotation around the X axis.+180~-180 degree   

            orientation.pitch = atan2( (float)(data->magnetic.y ),(float)(data->magnetic.z)    

)*180/PI;  

            //rotation around the Y axis +90~-90 degree   

            float roll = atan2( (float)(data->magnetic.x ),(float)(data->magnetic.z) )  

*180/PI;  

            if (roll > 90)  

            {  

                roll = -(180.0-roll);  

            }  

            else if (roll < -90)  

            {  

                roll = 180 + roll;  

            }  

            orientation.roll =  roll;  

        }  

        return S_HANDLE_MAGNETIC_FIELD;  

    }  

    if(poll_flag&(1<<ID_MAGNETIC_FIELD))  

    {  

        //数据已经计算好了直接上报就行   

        struct timespec t;  

        clock_gettime(CLOCK_REALTIME, &t);  

        data->time = timespec_to_ns(&t);  

        data->sensor = SENSOR_TYPE_ORIENTATION;  

        data->orientation.azimuth = orientation.azimuth;  

        data->orientation.pitch = orientation.pitch;  

        data->orientation.roll = orientation.roll;  

        poll_flag &= ~(1<<ID_ORIENTATION);  

        return S_HANDLE_ORIENTATION;  

    }  

}  

 

写好后可以用一个叫做sensorlist的程序先测试一下，看报上去的数据是否正常。然后可以试试一个叫做Pacific Navy Fighter 的游戏来爽一爽了。

 

由于涉及到很多方面的内容，错误难免，敬请指正。

 =====================

补充：我之前搞错了，我以为Orientation Sensor就叫做陀螺仪。谢谢syz85。

之后我又看了一下Android中对陀螺仪（gyroscope Sersor）的定义，是指在上述定义的x y x三个方向的转速，单位是radians/second，正负遵循右手规则。

想一想实现应该也不难，把从Orientation Sensor两次得到的 x y z角度变化除以时间就可以得到转速。

（其实真正的陀螺仪包括Orientation Sensor和gyroscope Sersor这两个功能）

但转速对于手机或其他消费类电子有意义是什么呢？谁指点我一下，gyroscope 会带来什么好玩的应用。