1 /*
2 * This file is part of the OpenKinect Project. http://www.openkinect.org
3 *
4 * Copyright (c) 2010 individual OpenKinect contributors. See the CONTRIB file
5 * for details.
6 *
7 * This code is licensed to you under the terms of the Apache License, version
8 * 2.0, or, at your option, the terms of the GNU General Public License,
9 * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses,
10 * or the following URLs:
11 * http://www.apache.org/licenses/LICENSE-2.0
12 * http://www.gnu.org/licenses/gpl-2.0.txt
13 *
14 * If you redistribute this file in source form, modified or unmodified, you
15 * may:
16 * 1) Leave this header intact and distribute it under the same terms,
17 * accompanying it with the APACHE20 and GPL20 files, or
18 * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or
19 * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file
20 * In all cases you must keep the copyright notice intact and include a copy
21 * of the CONTRIB file.
22 *
23 * Binary distributions must follow the binary distribution requirements of
24 * either License.
25 */
26
27 #include <stdio.h>
28 #include <stdlib.h>
29 #include <string.h>
30 #include <unistd.h>
31 #include <math.h>
32
33 #include "freenect_internal.h"
34
35 // The kinect can tilt from +31 to -31 degrees in what looks like 1 degree increments
36 // The control input looks like 2*desired_degrees
37 #define MAX_TILT_ANGLE 31
38 #define MIN_TILT_ANGLE (-31)
39
40 #define GRAVITY 9.80665
41
freenect_get_tilt_state(freenect_device * dev)42 freenect_raw_tilt_state* freenect_get_tilt_state(freenect_device *dev)
43 {
44 return &dev->raw_state;
45 }
46
freenect_update_tilt_state(freenect_device * dev)47 int freenect_update_tilt_state(freenect_device *dev)
48 {
49 freenect_context *ctx = dev->parent;
50 uint8_t buf[10];
51 uint16_t ux, uy, uz;
52 int ret = fnusb_control(&dev->usb_motor, 0xC0, 0x32, 0x0, 0x0, buf, 10);
53 if (ret != 10) {
54 FN_ERROR("Error in accelerometer reading, libusb_control_transfer returned %d\n", ret);
55 return ret < 0 ? ret : -1;
56 }
57
58 ux = ((uint16_t)buf[2] << 8) | buf[3];
59 uy = ((uint16_t)buf[4] << 8) | buf[5];
60 uz = ((uint16_t)buf[6] << 8) | buf[7];
61
62 dev->raw_state.accelerometer_x = (int16_t)ux;
63 dev->raw_state.accelerometer_y = (int16_t)uy;
64 dev->raw_state.accelerometer_z = (int16_t)uz;
65 dev->raw_state.tilt_angle = (int8_t)buf[8];
66 dev->raw_state.tilt_status = buf[9];
67
68 return ret;
69 }
70
freenect_set_tilt_degs(freenect_device * dev,double angle)71 int freenect_set_tilt_degs(freenect_device *dev, double angle)
72 {
73 int ret;
74 uint8_t empty[0x1];
75
76 angle = (angle<MIN_TILT_ANGLE) ? MIN_TILT_ANGLE : ((angle>MAX_TILT_ANGLE) ? MAX_TILT_ANGLE : angle);
77 angle = angle * 2;
78
79 ret = fnusb_control(&dev->usb_motor, 0x40, 0x31, (uint16_t)angle, 0x0, empty, 0x0);
80 return ret;
81 }
82
freenect_set_led(freenect_device * dev,freenect_led_options option)83 int freenect_set_led(freenect_device *dev, freenect_led_options option)
84 {
85 int ret;
86 uint8_t empty[0x1];
87 ret = fnusb_control(&dev->usb_motor, 0x40, 0x06, (uint16_t)option, 0x0, empty, 0x0);
88 return ret;
89 }
90
freenect_get_tilt_degs(freenect_raw_tilt_state * state)91 double freenect_get_tilt_degs(freenect_raw_tilt_state *state)
92 {
93 return ((double)state->tilt_angle) / 2.;
94 }
95
freenect_get_mks_accel(freenect_raw_tilt_state * state,double * x,double * y,double * z)96 void freenect_get_mks_accel(freenect_raw_tilt_state *state, double* x, double* y, double* z)
97 {
98 //the documentation for the accelerometer (http://www.kionix.com/Product%20Sheets/KXSD9%20Product%20Brief.pdf)
99 //states there are 819 counts/g
100 *x = (double)state->accelerometer_x/FREENECT_COUNTS_PER_G*GRAVITY;
101 *y = (double)state->accelerometer_y/FREENECT_COUNTS_PER_G*GRAVITY;
102 *z = (double)state->accelerometer_z/FREENECT_COUNTS_PER_G*GRAVITY;
103 }
104