void *freenect_threadfunc(void *arg)
{
freenect_set_tilt_degs(f_dev,freenect_angle);
freenect_set_led(f_dev,LED_RED);
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_rgb_callback(f_dev, rgb_cb);
freenect_set_rgb_format(f_dev, FREENECT_FORMAT_RGB);
freenect_set_depth_format(f_dev, FREENECT_FORMAT_11_BIT);
freenect_start_depth(f_dev);
freenect_start_rgb(f_dev);
printf("'w'-tilt up, 's'-level, 'x'-tilt down, '0'-'6'-select LED mode\n");
while(!die && freenect_process_events(f_ctx) >= 0 )
{
int16_t ax,ay,az;
freenect_get_raw_accel(f_dev, &ax, &ay, &az);
double dx,dy,dz;
freenect_get_mks_accel(f_dev, &dx, &dy, &dz);
printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f %4f\r", ax, ay, az, dx, dy, dz);
fflush(stdout);
}
printf("\nshutting down streams...\n");
freenect_stop_depth(f_dev);
freenect_stop_rgb(f_dev);
printf("-- done!\n");
return NULL;
}
void *network_data(void *arg)
{
int childlen;
struct sockaddr_in childaddr;
childlen = sizeof(childaddr);
while ( !die )
{
printf("### Wait data client\n");
data_child = accept(s_data, (struct sockaddr *)&childaddr, (unsigned int *)&childlen);
if ( data_child < 0 )
{
fprintf(stderr, "Error on accept() for data, exit data thread.\n");
break;
}
printf("### Got data client\n");
while(!die && freenect_process_events(f_ctx) >= 0 )
{
int n;
int16_t ax,ay,az;
freenect_get_raw_accel(f_dev, &ax, &ay, &az);
double dx,dy,dz;
freenect_get_mks_accel(f_dev, &dx, &dy, &dz);
char buffer_send[3*2+3*8];
memcpy(&buffer_send,&ax, sizeof(int16_t));
memcpy(&buffer_send[2],&ay, sizeof(int16_t));
memcpy(&buffer_send[4],&az, sizeof(int16_t));
memcpy(&buffer_send[6],&dx, sizeof(double));
memcpy(&buffer_send[14],&dy, sizeof(double));
memcpy(&buffer_send[22],&dz, sizeof(double));
n = write(data_child, buffer_send, 3*2+3*8);
}
}
return NULL;
}
void *data_out(void *arg) {
int n;
int16_t ax,ay,az;
double dx,dy,dz;
while(!die && freenect_process_events(f_ctx) >= 0 )
{
if(data_connected == 1){
freenect_get_raw_accel(f_dev, &ax, &ay, &az);
freenect_get_mks_accel(f_dev, &dx, &dy, &dz);
//printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f %4f\r", ax, ay, az, dx, dy, dz);
//fflush(stdout);
char buffer_send[3*2+3*8];
memcpy(&buffer_send,&ax, sizeof(int16_t));
memcpy(&buffer_send[2],&ay, sizeof(int16_t));
memcpy(&buffer_send[4],&az, sizeof(int16_t));
memcpy(&buffer_send[6],&dx, sizeof(double));
memcpy(&buffer_send[14],&dy, sizeof(double));
memcpy(&buffer_send[22],&dz, sizeof(double));
n = write(data_child, buffer_send, 3*2+3*8);
}
}
return NULL;
}
void getAccelerometers(int16_t &_x, int16_t &_y, int16_t &_z) {
if(freenect_get_raw_accel(m_dev,&_x, &_y, &_z) != 0) throw std::runtime_error("Cannot get raw accemerometers");
}
int main(int argc, char **argv)
{
int res;
freenect_context *f_ctx;
printf("Kinect camera test\n");
int i;
for (i=0; i<2048; i++) {
float v = i/2048.0;
v = powf(v, 3)* 6;
t_gamma[i] = v*6*256;
}
g_argc = argc;
g_argv = argv;
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
return 1;
}
freenect_set_log_level(f_ctx, FREENECT_LOG_DEBUG);
int nr_devices = freenect_num_devices (f_ctx);
printf ("Number of devices found: %d\n", nr_devices);
int user_device_number = 0;
if (argc > 1)
user_device_number = atoi(argv[1]);
if (nr_devices < 1)
return 1;
if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
printf("Could not open device\n");
return 1;
}
freenect_set_tilt_degs(f_dev,freenect_angle);
freenect_set_led(f_dev,LED_RED);
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_rgb_callback(f_dev, rgb_cb);
freenect_set_rgb_format(f_dev, FREENECT_FORMAT_RGB);
freenect_set_depth_format(f_dev, FREENECT_FORMAT_11_BIT);
res = pthread_create(&gl_thread, NULL, gl_threadfunc, NULL);
if (res) {
printf("pthread_create failed\n");
return 1;
}
freenect_start_depth(f_dev);
freenect_start_rgb(f_dev);
printf("'w'-tilt up, 's'-level, 'x'-tilt down, '0'-'6'-select LED mode\n");
while(!die && freenect_process_events(f_ctx) >= 0 )
{
int16_t ax,ay,az;
freenect_get_raw_accel(f_dev, &ax, &ay, &az);
double dx,dy,dz;
freenect_get_mks_accel(f_dev, &dx, &dy, &dz);
printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f %4f\r", ax, ay, az, dx, dy, dz);
fflush(stdout);
}
printf("-- done!\n");
pthread_exit(NULL);
}