void CameraNUI::setAngleFromProperty() { CLOG(LNOTICE) << "Setting angle: " << angle; freenect_sync_set_tilt_degs(angle, index); freenect_raw_tilt_state * state; freenect_sync_get_tilt_state(&state, index); CLOG(LNOTICE) << "Angle: " << state->tilt_angle << ", state: " << state->tilt_status; }
void *data_out(void *arg) { int n; int16_t ax,ay,az; double dx,dy,dz; while(data_connected){ #ifdef WIN32 Sleep(1000/30); #else usleep(1000000 / 30); // EMULATE 30 FPS #endif char buffer_send[3*2+3*8]; freenect_raw_tilt_state *state; freenect_sync_get_tilt_state(&state, 0); ax = state->accelerometer_x; ay = state->accelerometer_y; az = state->accelerometer_z; freenect_get_mks_accel(state, &dx, &dy, &dz); 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)); #ifdef WIN32 int n = send(data_client_socket, (char*)buffer_send, 3*2+3*8, 0); #else n = write(data_child, buffer_send, 3*2+3*8); #endif } return NULL; }
int main(int argc, char *argv[]) { srand(time(0)); while (1) { // Pick a random tilt and a random LED state freenect_led_options led = (freenect_led_options) (rand() % 6); // explicit cast int tilt = (rand() % 30)-15; freenect_raw_tilt_state *state = 0; double dx, dy, dz; // Set the LEDs to one of the possible states if (freenect_sync_set_led(led, 0)) no_kinect_quit(); // Set the tilt angle (in degrees) if (freenect_sync_set_tilt_degs(tilt, 0)) no_kinect_quit(); // Get the raw accelerometer values and tilt data if (freenect_sync_get_tilt_state(&state, 0)) no_kinect_quit(); // Get the processed accelerometer values (calibrated to gravity) freenect_get_mks_accel(state, &dx, &dy, &dz); printf("led[%d] tilt[%d] accel[%lf,%lf,%lf]\n", led, tilt, dx,dy,dz); sleep(3); } }
void getKinectAngle(void) { int i; double totalX=0, totalY=0, totalZ=0; double tempPoint1[3]; double tempPoint2[3]; for (i=0; i<10; i++) { // Get the raw accelerometer values and tilt data if (freenect_sync_get_tilt_state(&tiltState, 0)) exit(1); // Get the processed accelerometer values (calibrated to gravity) freenect_get_mks_accel(tiltState, &dx, &dy, &dz); totalX+=dx; totalY+=dy; totalZ+=dz; } dx=(totalX/10)/98; dy=(totalY/10)/98; dz=(totalZ/10)/98; //dy-=0.1; //printf("length: %lf\n", sqrt(dx*dx+dy*dy+dz*dz)); kinectAngleX = 0;//atan(dx/(sqrt(dy*dy+dz*dz))); kinectAngleZ = 0;//atan(dy/(sqrt(dx*dx+dz*dz)))-3.14159265/2; kinectAngleY = 0;//atan(dz/(sqrt(dx*dx+dz*dz))); printf("pitch: %lf ", kinectAngleX*(180.0/3.14159265)); printf("yaw: %lf ", kinectAngleY*(180.0/3.14159265)); printf("roll: %lf \n", kinectAngleZ*(180.0/3.14159265)); kinectRotMat[0] = cos(kinectAngleZ)*cos(kinectAngleY); kinectRotMat[1] = cos(kinectAngleZ)*sin(kinectAngleY)*sin(kinectAngleX) - sin(kinectAngleZ)*cos(kinectAngleX); kinectRotMat[2] = cos(kinectAngleZ)*sin(kinectAngleY)*cos(kinectAngleX) + sin(kinectAngleZ)*sin(kinectAngleX); kinectRotMat[3] = sin(kinectAngleZ)*cos(kinectAngleY); kinectRotMat[4] = sin(kinectAngleZ)*sin(kinectAngleY)*sin(kinectAngleX) + cos(kinectAngleZ)*cos(kinectAngleX); kinectRotMat[5] = sin(kinectAngleZ)*sin(kinectAngleY)*cos(kinectAngleX) - cos(kinectAngleZ)*sin(kinectAngleX); kinectRotMat[6] = -sin(kinectAngleY); kinectRotMat[7] = cos(kinectAngleY)*sin(kinectAngleX); kinectRotMat[8] = cos(kinectAngleY)*cos(kinectAngleX); tempPoint1[0]=dx; tempPoint1[1]=dy; tempPoint1[2]=dz; matMul(kinectRotMat,3,3,tempPoint1,3,1,tempPoint2); dx=tempPoint2[0]; dy=tempPoint2[1]; dz=tempPoint2[2]; //printf("accel[%lf,%lf,%lf]\n", dx,dy,dz); }
//send accelerometer data to client void sendAccelerometers() { int16_t ax,ay,az; double dx,dy,dz; unsigned char buffer_send[3*2+3*8]; freenect_raw_tilt_state *state; freenect_sync_get_tilt_state(&state, 0); ax = state->accelerometer_x; ay = state->accelerometer_y; az = state->accelerometer_z; freenect_get_mks_accel(state, &dx, &dy, &dz); 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)); int n = freenect_network_sendMessage(1, 2, buffer_send, 3*2+3*8); if ( n < 0) { printf("Error sending Accelerometers\n"); client_connected = 0; } }