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

时间:2024-02-20 12:53:55

设备中的三*度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中有个图。

Android 坐标系定义

图中表示设备的正上方是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), 如果设备水平放置,前方向下俯就是正,如图:

pitch 正值

 

前方向上仰就是负值;

pitch 负值

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

 

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

roll正值

 

向右翻滚是负值;

roll负值

 

求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  
  3. #define  ID_ACCELERATION   (ID_BASE+0)  
  4. #define  ID_MAGNETIC_FIELD (ID_BASE+1)  
  5. #define  ID_ORIENTATION (ID_BASE+2)  
  6. #define S_HANDLE_ACCELEROMETER      (1<<ID_ACCELERATION)  
  7. #define S_HANDLE_MAGNETIC_FIELD           (1<<ID_MAGNETIC_FIELD)  
  8. #define S_HANDLE_ORIENTATION                 (1<<ID_ORIENTATION)  
  9. #define SENSORS_NUM 4  
  10. #define SUPPORTED_SENSORS  ((1<<NUM_SENSORS)-1)  
  11. //在 sensor_t sensors_list[] 中添加两个sensor的信息,  
  12. //这些只是一些Sensor的信息,应用程序可以获取到。  
  13. #ifdef MAGNETIC_FIELD  
  14.     {  
  15.         name       : "XXX 3-axis Magnetic field sensor",  
  16.         vendor    : "XXX company",  
  17.         version    : 1,  
  18.         handle     : S_HANDLE_MAGNETIC_FIELD,  
  19.         type       : SENSOR_TYPE_MAGNETIC_FIELD,  
  20.         maxRange   : 600.0f,//最大范围  
  21.         resolution : 30.0f,//最小分辨率  
  22.         power      : 6.7f,//这个不太懂  
  23.     },  
  24. #endif  
  25. #ifdef ORIENTATION  
  26.     {  
  27.         name: "XXX Orientation sensor",  
  28.         vendor: "XXX company",  
  29.         version: 1,  
  30.         handle: S_HANDLE_ORIENTATION,  
  31.         type: SENSOR_TYPE_ORIENTATION,  
  32.         maxRange: 360,    
  33.         resolution: 0.1,   
  34.         power: 20,   
  35.     },  
  36. #endif  
  37. //定义一个结构来保存orientation的信息  
  38. static struct orientation{  
  39.     float azimuth;  
  40.     float pitch;  
  41.     float roll;  
  42. }orientation;  
  43. //在 control__open_data_source()函数中打开设备  
  44. static native_handle_t*  
  45. control__open_data_source(struct sensors_control_device_t *dev)  
  46. {  
  47.     SensorControl*  ctl = (void*)dev;  
  48.     native_handle_t* handle;  
  49.     int fd_m = open (MAGNETIC_DATA_DEVICE, O_RDONLY);  
  50.     LOGD ("Open Magnetic Data source: %d, %d/n", fd_m, errno);  
  51.     if (fd_m>= 0)   
  52.     {  
  53.         dev->fd[ID_MAGNETIC_FIELD] = dup(fd_m);  
  54.     }  
  55.     return handle;  
  56. }  
  57. //实现数据的打开和关闭函数  
  58. static int  
  59. data__data_open(struct sensors_data_device_t *dev, native_handle_t* handle)  
  60. {  
  61.     struct sensors_data_context_t *dev;  
  62.     dev = (struct sensors_data_context_t *)device;  
  63.     for(int i=0 ;i<SENSORS_NUM; i++)  
  64.     {  
  65.         dev->fd[i] = dup(handle->data[i]);  
  66.     }  
  67.     native_handle_close(handle);  
  68.     native_handle_delete(handle);  
  69.     return 0;  
  70. }  
  71. static int  
  72. data__data_close(struct sensors_data_device_t *dev)  
  73. {  
  74.     struct sensors_data_context_t *dev;  
  75.     dev = (struct sensors_data_context_t *)device;  
  76.       
  77.     for(int i=0 ;i<SENSORS_NUM; i++)  
  78.     {  
  79.         if (dev->fd[i] >= 0)  
  80.         {  
  81.             close(dev->fd[i]);  
  82.         }  
  83.         dev->fd[i] = -1;  
  84.     }  
  85.     return 0;  
  86. }  
  87. //最关键的poll函数  
  88. static int  
  89. data__poll(struct sensors_data_device_t *dev, sensors_data_t* values)  
  90. {  
  91.     SensorData*  data = (void*)dev;  
  92.     int fd = data->events_fd;  
  93.     //判断设备是否打开  
  94.     if(dev->fd[ID_MAGNETIC_FIELD] < 0)  
  95.     {  
  96.         LOGD("In %s dev[%d] is not open!/n",__FUNCTION__ ,ID_MAGNETIC_FIELD);  
  97.         return -1;  
  98.     }  
  99.     pollfd pfd[SENSORS_NUM] =   
  100.     {  
  101.         //省略其他sensor代码  
  102.         {  
  103.             fd: dev->fd[ID_MAGNETIC_FIELD],   
  104.             events: POLLIN,   
  105.             revents: 0  
  106.         },  
  107.         //省略其他sensor代码  
  108.     };  
  109.     int err = poll (pfd, SENSORS_NUM, s_timeout);  
  110.       
  111.     unsigned int  mask = SUPPORTED_SENSORS;  
  112.     static unsigned int poll_flag=0;  
  113.     if(poll_flag==0)  
  114.     {  
  115.         poll_flag = mask;  
  116.     }  
  117.     //省略其他sensor  
  118.     if(poll_flag&(1<<ID_MAGNETIC_FIELD))  
  119.     {  
  120.         if((pfd[ID_MAGNETIC_FIELD].revents&POLLIN) == POLLIN)  
  121.         {  
  122.             char rawData[6];  
  123.             err = read (dev->fd[ID_MAGNETIC_FIELD], &rawData, sizeof(rawData));  
  124.             if(err<0)  
  125.             {  
  126.                 LOGE("read magnetic field ret:%d errno:%d/n", err, errno);  
  127.                 return err;  
  128.             }  
  129.             struct timespec t;  
  130.             clock_gettime(CLOCK_REALTIME, &t);  
  131.             data->time = timespec_to_ns(&t);  
  132.             data->sensor = SENSOR_TYPE_MAGNETIC_FIELD;  
  133.             data->magnetic.status = SENSOR_STATUS_ACCURACY_HIGH;  
  134.             //上报的数据单位要转换成 uTesla  
  135.             data->magnetic.x = ( (rawData[1] << 8 ) | rawData[0])/ MAGNETIC_CONVERT;  
  136.             data->magnetic.y = ( (rawData[3] << 8 ) | rawData[2])/ MAGNETIC_CONVERT;  
  137.             data->magnetic.z = ( (rawData[5] << 8 ) | rawData[4])/ MAGNETIC_CONVERT;  
  138.               
  139.             //把陀螺仪需要的数据计算出来,用atan2(),头文件要加上#include <math.h>  
  140.             float azimuth = atan2(  (float)(data->magnetic.x ),(float)(data->magnetic.y) );  
  141.             if(azimuth<0)  
  142.             {  
  143.                 azimuth = 360 - fabs(azimuth*180/PI);  
  144.             }  
  145.             else  
  146.             {  
  147.                 azimuth = azimuth*180/PI;  
  148.             }  
  149.             orientation.azimuth = 360-azimuth;   
  150.               
  151.             //rotation around the X axis.+180~-180 degree  
  152.             orientation.pitch = atan2( (float)(data->magnetic.y ),(float)(data->magnetic.z)    
  153. )*180/PI;  
  154.             //rotation around the Y axis +90~-90 degree  
  155.             float roll = atan2( (float)(data->magnetic.x ),(float)(data->magnetic.z) )  
  156. *180/PI;  
  157.             if (roll > 90)  
  158.             {  
  159.                 roll = -(180.0-roll);  
  160.             }  
  161.             else if (roll < -90)  
  162.             {  
  163.                 roll = 180 + roll;  
  164.             }  
  165.             orientation.roll =  roll;  
  166.         }  
  167.         return S_HANDLE_MAGNETIC_FIELD;  
  168.     }  
  169.     if(poll_flag&(1<<ID_MAGNETIC_FIELD))  
  170.     {  
  171.         //数据已经计算好了直接上报就行  
  172.         struct timespec t;  
  173.         clock_gettime(CLOCK_REALTIME, &t);  
  174.         data->time = timespec_to_ns(&t);  
  175.         data->sensor = SENSOR_TYPE_ORIENTATION;  
  176.         data->orientation.azimuth = orientation.azimuth;  
  177.         data->orientation.pitch = orientation.pitch;  
  178.         data->orientation.roll = orientation.roll;  
  179.         poll_flag &= ~(1<<ID_ORIENTATION);  
  180.         return S_HANDLE_ORIENTATION;  
  181.     }  
  182. }  

 

写好后可以用一个叫做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 会带来什么好玩的应用。