Context Navigation

Back to Ticket #71

Ticket #71: sensors.archos.c

File sensors.archos.c, 12.1 KB (added by illifant , 2 years ago)
tricky orientation

Line
1	/*
2	* Copyright (C) 2008 The Android Open Source Project
3	*
4	* Licensed under the Apache License, Version 2.0 (the "License");
5	* you may not use this file except in compliance with the License.
6	* You may obtain a copy of the License at
7	*
8	* http://www.apache.org/licenses/LICENSE-2.0
9	*
10	* Unless required by applicable law or agreed to in writing, software
11	* distributed under the License is distributed on an "AS IS" BASIS,
12	* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13	* See the License for the specific language governing permissions and
14	* limitations under the License.
15	*/
16
17	/* Based on sensors library for HTC Dream
18	* http://android.git.kernel.org/?p=platform/hardware/htc/dream.git;a=blob;f=sensors.c
19	* Adapted for gen7 Archos 5IT
20	*/
21	#define LOG_TAG "Sensors"
22
23	#include
24	#include
25	#include
26	#include
27	#include
28	#include
29	#include
30
31	#include
32	#include "mma7456l.h"
33
34	#include
35	#include
36	#include
37	/*****************************************************************************/
38
39	#define MAX_NUM_SENSORS 3
40
41	#define SUPPORTED_SENSORS ((1<
42
43	#define ID_A (0)
44	#define ID_M (1)
45	#define ID_O (2)
46	/#define ID_T (3) /
47
48	#define SENSORS_ACCELERATION (1<
49	#define SENSORS_MAGNETIC_FIELD (1<
50	#define SENSORS_ORIENTATION (1<
51	/*#define SENSORS_TEMPERATURE (1<
52
53	// with 2g limits and 8 bit output 1g == 64
54	#define CONVERT_A (GRAVITY_EARTH / 64.0f)
55
56	#define AKM_DEVICE_NAME "/dev/accel_ctrl"
57	#define DEFAULT_DELAY 200
58	/*****************************************************************************/
59
60	struct sensors_control_context_t {
61	struct sensors_control_device_t device;
62	int akmd_fd;
63	uint32_t active_sensors;
64	};
65
66	struct sensors_data_context_t {
67	struct sensors_data_device_t device;
68	int events_fd;
69	sensors_data_t sensors[MAX_NUM_SENSORS];
70	uint32_t pendingSensors;
71	};
72
73	/*
74	* The SENSORS Module
75	*/
76
77	static const struct sensor_t sSensorList[] = {
78	{
79	name: "MMA7456l 3-axis Accelerometer",
80	vendor: "OpenAOS",
81	version: 1,
82	handle: SENSORS_HANDLE_BASE+ID_A,
83	type: SENSOR_TYPE_ACCELEROMETER,
84	maxRange: 2*GRAVITY_EARTH,
85	resolution: CONVERT_A,
86	power: 3.0f,
87	{ },
88	},
89	{
90	name: "Orientation Sensor Emulator",
91	vendor: "OpenAOS",
92	version: 1,
93	handle: SENSORS_HANDLE_BASE+ID_O,
94	type: SENSOR_TYPE_ORIENTATION,
95	maxRange: 180.0,
96	resolution: CONVERT_A,
97	power: 0.0f,
98	{ },
99	},
100	{
101	name: "Magnetic compass stub",
102	vendor: "OpenAOS",
103	version: 1,
104	handle: SENSORS_HANDLE_BASE+ID_M,
105	type: SENSOR_TYPE_MAGNETIC_FIELD,
106	maxRange: 1.0,
107	resolution: 0.1,
108	power: 0.0f, // =)
109	{ },
110	},
111	};
112
113	static int open_sensors(const struct hw_module_t* module, const char* name,
114	struct hw_device_t** device);
115
116	static int sensors__get_sensors_list(struct sensors_module_t* module,
117	struct sensor_t const** list)
118	{
119	*list = sSensorList;
120	return sizeof(sSensorList)/sizeof(sSensorList[0]);
121	}
122
123	static struct hw_module_methods_t sensors_module_methods = {
124	.open = open_sensors
125	};
126
127	struct sensors_module_t HAL_MODULE_INFO_SYM = {
128	.common = {
129	.tag = HARDWARE_MODULE_TAG,
130	.version_major = 1,
131	.version_minor = 0,
132	.id = SENSORS_HARDWARE_MODULE_ID,
133	.name = "MMA7456l SENSORS Module",
134	.author = "The Android Open Source Project",
135	.methods = &sensors_module_methods,
136	},
137	.get_sensors_list = sensors__get_sensors_list
138	};
139
140	/*****************************************************************************/
141
142	static int open_input(int mode)
143	{
144	/* scan all input drivers and look for accelerometer */
145	int fd = -1;
146	const char *dirname = "/dev/input";
147	char devname[PATH_MAX];
148	char *filename;
149	DIR *dir;
150	struct dirent *de;
151	dir = opendir(dirname);
152	if(dir == NULL)
153	return -1;
154	strcpy(devname, dirname);
155	filename = devname + strlen(devname);
156	*filename++ = '/';
157	while((de = readdir(dir))) {
158	if(de->d_name[0] == '.' &&
159	(de->d_name[1] == '\0' \|\|
160	(de->d_name[1] == '.' && de->d_name[2] == '\0')))
161	continue;
162	strcpy(filename, de->d_name);
163	fd = open(devname, mode);
164	if (fd>=0) {
165	char name[80];
166	if (ioctl(fd, EVIOCGNAME(sizeof(name) - 1), &name) < 1) {
167	name[0] = '\0';
168	}
169	if (!strcmp(name, "MMA7456L Accelerometer")) {
170	//LOGD("using %s (name=%s)", devname, name);
171	break;
172	}
173	close(fd);
174	fd = -1;
175	}
176	}
177	closedir(dir);
178
179	if (fd < 0) {
180	LOGE("Couldn't find or open 'compass' driver (%s)", strerror(errno));
181	}
182	return fd;
183	}
184
185	static int open_akm(struct sensors_control_context_t* dev)
186	{
187	if (dev->akmd_fd <= 0) {
188	dev->akmd_fd = open(AKM_DEVICE_NAME, O_RDONLY);
189	//LOGD("%s, fd=%d", __PRETTY_FUNCTION__, dev->akmd_fd);
190	LOGE_IF(dev->akmd_fd<0, "Couldn't open %s (%s)",
191	AKM_DEVICE_NAME, strerror(errno));
192	if (dev->akmd_fd >= 0) {
193	dev->active_sensors = 0;
194	}
195	}
196	return dev->akmd_fd;
197	}
198
199	static void close_akm(struct sensors_control_context_t* dev)
200	{
201	if (dev->akmd_fd > 0) {
202	//LOGD("%s, fd=%d", __PRETTY_FUNCTION__, dev->akmd_fd);
203	close(dev->akmd_fd);
204	dev->akmd_fd = -1;
205	}
206	}
207
208	static int mma_set_delay(int fd, int delay)
209	{
210	if (fd <= 0) {
211	return -1;
212	}
213	if (!ioctl(fd, MMA7456L_IOCTL_S_POLL_DELAY, &delay)) {
214	return -errno;
215	}
216	return 0;
217	}
218
219	/*****************************************************************************/
220
221	static native_handle_t* control__open_data_source(struct sensors_control_context_t *dev)
222	{
223	native_handle_t* handle;
224	int fd = open_input(O_RDONLY);
225	if (fd < 0) {
226	return NULL;
227	}
228
229	handle = native_handle_create(1, 0);
230	handle->data[0] = fd;
231	return handle;
232	}
233
234	static int control__activate(struct sensors_control_context_t *dev,
235	int handle, int enabled)
236	{
237	LOGI("control__activate handle=%d enabled=%d", handle, enabled);
238	if ((handle<SENSORS_HANDLE_BASE) \|\|
239	(handle>=SENSORS_HANDLE_BASE+MAX_NUM_SENSORS)) {
240	return -1;
241	}
242
243	if (handle == SENSORS_HANDLE_BASE + ID_A) {
244	int fd = open_akm(dev);
245	int delay = enabled ? DEFAULT_DELAY : 0;
246	mma_set_delay(fd, delay);
247	close_akm(dev);
248	}
249	return 0;
250	}
251
252	static int control__set_delay(struct sensors_control_context_t *dev, int32_t ms)
253	{
254	if (mma_set_delay(dev->akmd_fd, ms) < 0) {
255	return -errno;
256	}
257	return 0;
258	}
259
260	static int control__wake(struct sensors_control_context_t *dev)
261	{
262
263	int err = 0;
264	int fd = open_input(O_WRONLY);
265	if (fd > 0) {
266	struct input_event event[1];
267	event[0].type = EV_SYN;
268	event[0].code = SYN_CONFIG;
269	event[0].value = 0;
270	err = write(fd, event, sizeof(event));
271	LOGD_IF(err<0, "control__wake, err=%d (%s)", errno, strerror(errno));
272	close(fd);
273	}
274	return err;
275	}
276
277	/*****************************************************************************/
278
279	static int data__data_open(struct sensors_data_context_t dev, native_handle_t handle)
280	{
281	int i;
282	memset(&dev->sensors, 0, sizeof(dev->sensors));
283
284	for (i=0 ; i<MAX_NUM_SENSORS ; i++) {
285	// by default all sensors have high accuracy
286	// (we do this because we don't get an update if the value doesn't
287	// change).
288	dev->sensors[i].vector.status = SENSOR_STATUS_ACCURACY_HIGH;
289	}
290	dev->pendingSensors = 0;
291	dev->events_fd = dup(handle->data[0]);
292	LOGI("data__data_open: fd = %d", handle->data[0]);
293	native_handle_close(handle);
294	native_handle_delete(handle);
295	return 0;
296	}
297
298	static int data__data_close(struct sensors_data_context_t *dev)
299	{
300	if (dev->events_fd > 0) {
301	LOGI("(data close) about to close fd=%d", dev->events_fd);
302	close(dev->events_fd);
303	dev->events_fd = -1;
304	}
305	return 0;
306	}
307
308	static int pick_sensor(struct sensors_data_context_t *dev,
309	sensors_data_t* values)
310	{
311	uint32_t mask = SUPPORTED_SENSORS;
312	while (mask) {
313	uint32_t i = 31 - __builtin_clz(mask);
314	//LOGI("pick_sensor mask=%x", mask);
315	mask &= ~(1<<i);
316	if (dev->pendingSensors & (1<<i)) {
317	dev->pendingSensors &= ~(1<<i);
318	*values = dev->sensors[i];
319	values->sensor = (1<<i);
320	//LOGI("%d [%f, %f, %f]", (1<
321	//values->vector.x,
322	//values->vector.y,
323	//values->vector.z);
324	return i;
325	}
326	}
327	LOGE("No sensor to return!!! pendingSensors=%08x", dev->pendingSensors);
328	// we may end-up in a busy loop, slow things down, just in case.
329	usleep(100000);
330	return -1;
331	}
332
333	static int data__poll(struct sensors_data_context_t dev, sensors_data_t values)
334	{
335	//LOGI("data__poll start");
336	int fd = dev->events_fd;
337	if (fd < 0) {
338	LOGE("invalid file descriptor, fd=%d", fd);
339	return -1;
340	}
341
342	// there are pending sensors, returns them now...
343	if (dev->pendingSensors) {
344	return pick_sensor(dev, values);
345	}
346
347	// wait until we get a complete event for an enabled sensor
348	uint32_t new_sensors = 0;
349	while (1) {
350	/* read the next event */
351	struct input_event event;
352	int nread = read(fd, &event, sizeof(event));
353	if (nread == sizeof(event)) {
354	uint32_t v;
355	if (event.type == EV_ABS) {
356	/*LOGI("type: %d code: %d value: %-5d time: %ds",
357	event.type, event.code, event.value,
358	(int)event.time.tv_sec);*/
359	switch (event.code) {
360	case ABS_X:
361	new_sensors \|= SENSORS_ACCELERATION;
362	dev->sensors[ID_A].acceleration.x = event.value * CONVERT_A;
363	break;
364	case ABS_Y:
365	new_sensors \|= SENSORS_ACCELERATION;
366	dev->sensors[ID_A].acceleration.y = event.value * CONVERT_A + 2;
367	break;
368	case ABS_Z:
369	new_sensors \|= SENSORS_ACCELERATION;
370	dev->sensors[ID_A].acceleration.z = event.value * CONVERT_A;
371	break;
372	}
373	} else if (event.type == EV_SYN) {
374	if (event.code == SYN_CONFIG) {
375	// we use SYN_CONFIG to signal that we need to exit the
376	// main loop.
377	//LOGD("got empty message: value=%d", event.value);
378	return 0x7FFFFFFF;
379	}
380	if (new_sensors) {
381	/* Calculate Orientation */
382	dev->sensors[ID_O].orientation.azimuth = 0;
383	dev->sensors[ID_O].orientation.roll = atan2f(-dev->sensors[ID_A].acceleration.y, dev->sensors[ID_A].acceleration.z)180.0fM_1_PI;
384	float g_xz = hypotf(dev->sensors[ID_A].acceleration.y, dev->sensors[ID_A].acceleration.z);
385	if (g_xz == 0.0) {
386	dev->sensors[ID_O].orientation.pitch = (dev->sensors[ID_A].acceleration.x < 0)? 90.0 : -90.0;
387	} else {
388	dev->sensors[ID_O].orientation.pitch = -atanf(dev->sensors[ID_A].acceleration.x / g_xz)180.0fM_1_PI;
389	}
390	new_sensors \|= SENSORS_ORIENTATION;
391
392	dev->pendingSensors = new_sensors;
393	int64_t t = event.time.tv_sec*1000000000LL +
394	event.time.tv_usec*1000;
395	while (new_sensors) {
396	uint32_t i = 31 - __builtin_clz(new_sensors);
397	new_sensors &= ~(1<<i);
398	dev->sensors[i].time = t;
399	}
400	return pick_sensor(dev, values);
401	}
402	}
403	}
404	}
405	}
406
407	/*****************************************************************************/
408
409	static int control__close(struct hw_device_t *dev)
410	{
411	struct sensors_control_context_t* ctx = (struct sensors_control_context_t*)dev;
412	if (ctx) {
413	if (ctx->akmd_fd > 0)
414	close(ctx->akmd_fd);
415	free(ctx);
416	}
417	return 0;
418	}
419
420	static int data__close(struct hw_device_t *dev)
421	{
422	struct sensors_data_context_t* ctx = (struct sensors_data_context_t*)dev;
423	if (ctx) {
424	if (ctx->events_fd > 0) {
425	//LOGD("(device close) about to close fd=%d", ctx->events_fd);
426	close(ctx->events_fd);
427	}
428	free(ctx);
429	}
430	return 0;
431	}
432
433
434	/** Open a new instance of a sensor device using name */
435	static int open_sensors(const struct hw_module_t* module, const char* name,
436	struct hw_device_t** device)
437	{
438	int status = -EINVAL;
439	if (!strcmp(name, SENSORS_HARDWARE_CONTROL)) {
440	struct sensors_control_context_t *dev;
441	dev = malloc(sizeof(*dev));
442	memset(dev, 0, sizeof(*dev));
443	dev->akmd_fd = -1;
444	dev->device.common.tag = HARDWARE_DEVICE_TAG;
445	dev->device.common.version = 0;
446	dev->device.common.module = module;
447	dev->device.common.close = control__close;
448	dev->device.open_data_source = control__open_data_source;
449	dev->device.activate = control__activate;
450	dev->device.set_delay= control__set_delay;
451	dev->device.wake = control__wake;
452	*device = &dev->device.common;
453	} else if (!strcmp(name, SENSORS_HARDWARE_DATA)) {
454	struct sensors_data_context_t *dev;
455	dev = malloc(sizeof(*dev));
456	memset(dev, 0, sizeof(*dev));
457	dev->events_fd = -1;
458	dev->device.common.tag = HARDWARE_DEVICE_TAG;
459	dev->device.common.version = 0;
460	dev->device.common.module = module;
461	dev->device.common.close = data__close;
462	dev->device.data_open = data__data_open;
463	dev->device.data_close = data__data_close;
464	dev->device.poll = data__poll;
465	*device = &dev->device.common;
466	}
467	return status;
468	}
469
470

Download in other formats:

Original Format