tilt.c
Go to the documentation of this file.
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 
42 // Structs for 1473 and K4W communication
43 typedef struct {
50 
51 typedef struct {
56 
57 static int tag_seq = 0;
58 static int tag_next_ack = 0;
59 
61  unsigned char buffer[512];
62  memset(buffer, 0, 512);
63  int transferred = 0;
64  int res = 0;
65  res = libusb_bulk_transfer(dev, 0x81, buffer, 512, &transferred, 100);
66  if (res != 0) {
67  FN_ERROR("get_reply(): libusb_bulk_transfer failed: %d (transferred = %d)\n", res, transferred);
68  } else if (transferred != 12) {
69  FN_ERROR("get_reply(): weird - got %d bytes (expected 12)\n", transferred);
70  } else {
71  fn_alt_motor_reply reply;
72  memcpy(&reply, buffer, sizeof(reply));
73  if (reply.magic != 0x0a6fe000) {
74  FN_ERROR("Bad magic: %08X (expected 0A6FE000\n", reply.magic);
75  res = -1;
76  }
77 
78  //can't really do this as tag_seq is static. with two cameras this is not going to match. could put in as part of libusb_device but I don't think it really matters.
79 // if (reply.tag != tag_next_ack) {
80 // FN_ERROR("Reply tag out of order: expected %d, got %d\n", tag_next_ack, reply.tag);
81 // res = -1;
82 // }
83 
84  if (reply.status != 0) {
85  FN_ERROR("reply status != 0: failure?\n");
86  res = -1;
87  }
88 
89  tag_next_ack++;
90  }
91  return res;
92 }
93 
94 
96 {
97  return &dev->raw_state;
98 }
99 
101  freenect_context *ctx = dev->parent;
102 
103  int transferred = 0;
104  int res = 0;
106  cmd.magic = fn_le32(0x06022009);
107  cmd.tag = fn_le32(tag_seq++);
108  cmd.arg1 = fn_le32(0x68); // 104. Incidentally, the number of bytes that we expect in the reply.
109  cmd.cmd = fn_le32(0x8032);
110 
111  unsigned char buffer[256];
112  memcpy(buffer, &cmd, 16);
113 
114  res = libusb_bulk_transfer(dev->usb_audio.dev, 0x01, buffer, 16, &transferred, 250);
115  if (res != 0) {
116  return res;
117  }
118 
119  res = libusb_bulk_transfer(dev->usb_audio.dev, 0x81, buffer, 256, &transferred, 250); // 104 bytes
120  if (res != 0) {
121  return res;
122  } else {
123 // int i;
124 // for(i = 0 ; i < transferred ; i += 4) {
125 // int32_t j;
126 // memcpy(&j, buffer + i, 4);
127 // printf("\t%d\n", j);
128 // }
129 // printf("\n");
130  struct {
131  int32_t x;
132  int32_t y;
133  int32_t z;
134  int32_t tilt;
135  } accel_and_tilt;
136 
137  memcpy(&accel_and_tilt, buffer + 16, sizeof(accel_and_tilt));
138  //printf("\tX: %d Y: %d Z:%d - tilt is %d\n", accel_and_tilt.x, accel_and_tilt.y, accel_and_tilt.z, accel_and_tilt.tilt);
139 
140  dev->raw_state.accelerometer_x = (int16_t)accel_and_tilt.x;
141  dev->raw_state.accelerometer_y = (int16_t)accel_and_tilt.y;
142  dev->raw_state.accelerometer_z = (int16_t)accel_and_tilt.z;
143 
144  //this is multiplied by 2 as the older 1414 device reports angles doubled and freenect takes this into account
145  dev->raw_state.tilt_angle = (int8_t)accel_and_tilt.tilt * 2;
146 
147  }
148  // Reply: skip four uint32_t, then you have three int32_t that give you acceleration in that direction, it seems.
149  // Units still to be worked out.
150  return get_reply(dev->usb_audio.dev, ctx);
151 }
152 
154 {
155  freenect_context *ctx = dev->parent;
156 
157  //if we have motor control via audio and fw is uploaded - call the alt function
159  return update_tilt_state_alt(dev);
160  }
161 
163  return 0;
164 
165 
166  uint8_t buf[10];
167  uint16_t ux, uy, uz;
168  int ret = fnusb_control(&dev->usb_motor, 0xC0, 0x32, 0x0, 0x0, buf, 10);
169  if (ret != 10) {
170  FN_ERROR("Error in accelerometer reading, libusb_control_transfer returned %d\n", ret);
171  return ret < 0 ? ret : -1;
172  }
173 
174  ux = ((uint16_t)buf[2] << 8) | buf[3];
175  uy = ((uint16_t)buf[4] << 8) | buf[5];
176  uz = ((uint16_t)buf[6] << 8) | buf[7];
177 
178  dev->raw_state.accelerometer_x = (int16_t)ux;
179  dev->raw_state.accelerometer_y = (int16_t)uy;
180  dev->raw_state.accelerometer_z = (int16_t)uz;
181  dev->raw_state.tilt_angle = (int8_t)buf[8];
183 
184  return ret;
185 }
186 
187 int freenect_set_tilt_degs_alt(freenect_device *dev, int tilt_degrees)
188 {
189  freenect_context *ctx = dev->parent;
190 
191  if (tilt_degrees > 31 || tilt_degrees < -31) {
192  FN_WARNING("set_tilt(): degrees %d out of safe range [-31, 31]\n", tilt_degrees);
193  return -1;
194  }
195 
197  cmd.magic = fn_le32(0x06022009);
198  cmd.tag = fn_le32(tag_seq++);
199  cmd.arg1 = fn_le32(0);
200  cmd.cmd = fn_le32(0x803b);
201  cmd.arg2 = (uint32_t)(fn_le32((int32_t)tilt_degrees));
202  int transferred = 0;
203  int res = 0;
204  unsigned char buffer[20];
205  memcpy(buffer, &cmd, 20);
206 
207  res = libusb_bulk_transfer(dev->usb_audio.dev, 0x01, buffer, 20, &transferred, 250);
208  if (res != 0) {
209  FN_ERROR("freenect_set_tilt_alt(): libusb_bulk_transfer failed: %d (transferred = %d)\n", res, transferred);
210  return res;
211  }
212 
213  return get_reply(dev->usb_audio.dev, ctx);
214 }
215 
217 {
218  freenect_context *ctx = dev->parent;
219 
220  //if we have motor control via audio and fw is uploaded - call the alt function
222  return freenect_set_tilt_degs_alt(dev, angle);
223  }
224 
226  return 0;
227 
228  int ret;
229  uint8_t empty[0x1];
230 
231  angle = (angle<MIN_TILT_ANGLE) ? MIN_TILT_ANGLE : ((angle>MAX_TILT_ANGLE) ? MAX_TILT_ANGLE : angle);
232  angle = angle * 2;
233 
234  ret = fnusb_control(&dev->usb_motor, 0x40, 0x31, (int16_t)angle, 0x0, empty, 0x0);
235  return ret;
236 }
237 
238 
240 {
241  typedef enum {
242  LED_ALT_OFF = 1,
243  LED_ALT_BLINK_GREEN = 2,
244  LED_ALT_SOLID_GREEN = 3,
245  LED_ALT_SOLID_RED = 4,
246  }led_alt_state;
247 
248  int transferred = 0;
249  int res = 0;
250 
251  //The LED states are different between K4W/1473 and older 1414
252  if( state == LED_GREEN ){
253  state = (freenect_led_options)LED_ALT_SOLID_GREEN;
254  }else if( state == LED_RED ){
255  state = (freenect_led_options)LED_ALT_SOLID_RED;
256  }else if( state == LED_YELLOW ){
257  state = (freenect_led_options)LED_ALT_SOLID_GREEN;
258  }else if( state == LED_OFF ){
259  state = (freenect_led_options)LED_ALT_OFF;
260  }else if( state == LED_BLINK_GREEN ){
261  state = (freenect_led_options)LED_ALT_BLINK_GREEN;
262  }else{
263  state = LED_GREEN;
264  }
265 
267  cmd.magic = fn_le32(0x06022009);
268  cmd.tag = fn_le32(tag_seq++);
269  cmd.arg1 = fn_le32(0);
270  cmd.cmd = fn_le32(0x10);
271  cmd.arg2 = (uint32_t)(fn_le32((int32_t)state));
272 
273  unsigned char buffer[20];
274  memcpy(buffer, &cmd, 20);
275 
276  res = libusb_bulk_transfer(dev, 0x01, buffer, 20, &transferred, 100);
277  if (res != 0) {
278  FN_WARNING("fnusb_set_led_alt(): libusb_bulk_transfer failed: %d (transferred = %d)\n", res, transferred);
279  return res;
280  }
281  return get_reply(dev, ctx);
282 }
283 
285 {
286  freenect_context *ctx = dev->parent;
287  return fnusb_set_led_alt(dev->usb_audio.dev, ctx, state);
288 }
289 
291 {
292  freenect_context *ctx = dev->parent;
293 
294  //if we have motor control via audio and fw is uploaded - call the alt function
296  return freenect_set_led_alt(dev, option);
297  }
298 
300  return 0;
301 
302  int ret;
303  uint8_t empty[0x1];
304  ret = fnusb_control(&dev->usb_motor, 0x40, 0x06, (uint16_t)option, 0x0, empty, 0x0);
305  return ret;
306 }
307 
309 {
310  return ((double)state->tilt_angle) / 2.;
311 }
312 
314 {
315  return state->tilt_status;
316 }
317 
318 void freenect_get_mks_accel(freenect_raw_tilt_state *state, double* x, double* y, double* z)
319 {
320  //the documentation for the accelerometer (http://www.kionix.com/Product%20Sheets/KXSD9%20Product%20Brief.pdf)
321  //states there are 819 counts/g
322  *x = (double)state->accelerometer_x/FREENECT_COUNTS_PER_G*GRAVITY;
323  *y = (double)state->accelerometer_y/FREENECT_COUNTS_PER_G*GRAVITY;
324  *z = (double)state->accelerometer_z/FREENECT_COUNTS_PER_G*GRAVITY;
325 }
freenect_device_flags enabled_subdevices
uint32_t tag
Definition: tilt.c:45
int get_reply(libusb_device_handle *dev, freenect_context *ctx)
Definition: tilt.c:60
uint32_t arg1
Definition: tilt.c:46
uint32_t magic
Definition: tilt.c:44
freenect_context * parent
short int16_t
unsigned short uint16_t
FN_INTERNAL int fnusb_set_led_alt(libusb_device_handle *dev, freenect_context *ctx, freenect_led_options state)
Definition: tilt.c:239
#define FN_ERROR(...)
freenect_tilt_status_code tilt_status
Definition: libfreenect.h:172
freenect_tilt_status_code freenect_get_tilt_status(freenect_raw_tilt_state *state)
Definition: tilt.c:313
double freenect_get_tilt_degs(freenect_raw_tilt_state *state)
Definition: tilt.c:308
signed char int8_t
int freenect_set_led(freenect_device *dev, freenect_led_options option)
Definition: tilt.c:290
unsigned char uint8_t
#define FREENECT_COUNTS_PER_G
Definition: libfreenect.h:42
int freenect_update_tilt_state(freenect_device *dev)
Definition: tilt.c:153
void freenect_get_mks_accel(freenect_raw_tilt_state *state, double *x, double *y, double *z)
Definition: tilt.c:318
uint32_t magic
Definition: tilt.c:52
libusb_device_handle * dev
Definition: usb_libusb10.h:70
uint32_t status
Definition: tilt.c:54
#define FN_WARNING(...)
static int tag_seq
Definition: tilt.c:57
int int32_t
freenect_raw_tilt_state raw_state
FN_INTERNAL int fnusb_control(fnusb_dev *dev, uint8_t bmRequestType, uint8_t bRequest, uint16_t wValue, uint16_t wIndex, uint8_t *data, uint16_t wLength)
Definition: usb_libusb10.c:823
static int tag_next_ack
Definition: tilt.c:58
#define MAX_TILT_ANGLE
Definition: tilt.c:37
freenect_led_options
Definition: libfreenect.h:148
#define GRAVITY
Definition: tilt.c:40
int update_tilt_state_alt(freenect_device *dev)
Definition: tilt.c:100
static freenect_context * ctx
int freenect_set_tilt_degs(freenect_device *dev, double angle)
Definition: tilt.c:216
Data from the tilt motor and accelerometer.
Definition: libfreenect.h:167
uint32_t tag
Definition: tilt.c:53
#define FN_INTERNAL
unsigned int uint32_t
int freenect_set_tilt_degs_alt(freenect_device *dev, int tilt_degrees)
Definition: tilt.c:187
capture state
Definition: micview.c:53
uint32_t arg2
Definition: tilt.c:48
int libusb_bulk_transfer(libusb_device_handle *dev_handle, unsigned char endpoint, unsigned char *data, int length, int *actual_length, unsigned int timeout)
Definition: libusbemu.cpp:412
#define fn_le32(x)
freenect_raw_tilt_state * freenect_get_tilt_state(freenect_device *dev)
Definition: tilt.c:95
int freenect_set_led_alt(freenect_device *dev, freenect_led_options state)
Definition: tilt.c:284
freenect_tilt_status_code
Enumeration of tilt motor status.
Definition: libfreenect.h:160
uint32_t cmd
Definition: tilt.c:47


libfreenect
Author(s): Hector Martin, Josh Blake, Kyle Machulis, OpenKinect community
autogenerated on Mon Jun 10 2019 13:46:42