陀螺仪使用

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

来自   Fgamers

2011年  8月  25日

5,808 次阅读 评论 （0）

设备中的三自由度陀螺仪就是一个可以识别设备相对于地面，绕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轴方向刚好一致（去感谢你的硬件工程师吧）。

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

这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。要实现的就是这两个：

1 2 3

#define SENSOR_TYPE_MAGNETIC_FIELD 2   #define SENSOR_TYPE_ORIENTATION 3

在/hardware/sensors/sensors.cpp 中添加对MAGNETIC_FIELD和ORIENTATION 的支持
简单的说一下怎样添加,下面的代码不完整，请参考/sdk/emulator/sensors/sensors_qemu.c

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182

//加入需要的宏定义 #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; } }