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