t_jit_err jit_freenect_grab_get_tilt(t_jit_freenect_grab *x, void *attr, long *ac, t_atom **av){
double tilt=0;
if ((*ac)&&(*av)) {
} else {
*ac = 1;
if (!(*av = jit_getbytes(sizeof(t_atom)*(*ac)))) {
*ac = 0;
return JIT_ERR_OUT_OF_MEM;
}
}
if(x->device){
freenect_update_tilt_state(x->device);
x->state = freenect_get_tilt_state(x->device);
if (x->state)
tilt = freenect_get_tilt_degs(x->state);
}
jit_atom_setfloat(*av, tilt);
return JIT_ERR_NONE;
}
//---------------------------------------------------------------------------
void ofxKinect::threadedFunction(){
if(currentLed < 0) {
freenect_set_led(kinectDevice, (freenect_led_options)ofxKinect::LED_GREEN);
}
freenect_frame_mode videoMode = freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, bIsVideoInfrared?FREENECT_VIDEO_IR_8BIT:FREENECT_VIDEO_RGB);
freenect_set_video_mode(kinectDevice, videoMode);
freenect_frame_mode depthMode = freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, bUseRegistration?FREENECT_DEPTH_REGISTERED:FREENECT_DEPTH_MM);
freenect_set_depth_mode(kinectDevice, depthMode);
ofLogVerbose("ofxKinect") << "device " << deviceId << " " << serial << " connection opened";
freenect_start_depth(kinectDevice);
if(bGrabVideo) {
freenect_start_video(kinectDevice);
}
while(isThreadRunning() && freenect_process_events(kinectContext.getContext()) >= 0) {
if(bTiltNeedsApplying) {
freenect_set_tilt_degs(kinectDevice, targetTiltAngleDeg);
bTiltNeedsApplying = false;
}
if(bLedNeedsApplying) {
if(currentLed == ofxKinect::LED_DEFAULT) {
freenect_set_led(kinectDevice, (freenect_led_options)ofxKinect::LED_GREEN);
}
else {
freenect_set_led(kinectDevice, (freenect_led_options)currentLed);
}
bLedNeedsApplying = false;
}
freenect_update_tilt_state(kinectDevice);
freenect_raw_tilt_state * tilt = freenect_get_tilt_state(kinectDevice);
currentTiltAngleDeg = freenect_get_tilt_degs(tilt);
rawAccel.set(tilt->accelerometer_x, tilt->accelerometer_y, tilt->accelerometer_z);
double dx,dy,dz;
freenect_get_mks_accel(tilt, &dx, &dy, &dz);
mksAccel.set(dx, dy, dz);
}
// finish up a tilt on exit
if(bTiltNeedsApplying) {
freenect_set_tilt_degs(kinectDevice, targetTiltAngleDeg);
bTiltNeedsApplying = false;
}
freenect_stop_depth(kinectDevice);
freenect_stop_video(kinectDevice);
if(currentLed < 0) {
freenect_set_led(kinectDevice, (freenect_led_options)ofxKinect::LED_RED);
}
kinectContext.close(*this);
ofLogVerbose("ofxKinect") << "device " << deviceId << " connection closed";
}
t_jit_err jit_freenect_grab_get_accel(t_jit_freenect_grab *x, void *attr, long *ac, t_atom **av){
double ax=0,ay=0,az=0;
if ((*ac)&&(*av)) {
} else {
*ac = 3;
if (!(*av = jit_getbytes(sizeof(t_atom)*(*ac)))) {
*ac = 0;
return JIT_ERR_OUT_OF_MEM;
}
}
if(x->device){
freenect_update_tilt_state(x->device);
x->state = freenect_get_tilt_state(x->device);
if (x->state)
freenect_get_mks_accel(x->state, &ax, &ay, &az);
}
jit_atom_setfloat(*av, ax);
jit_atom_setfloat(*av +1, ay);
jit_atom_setfloat(*av +2, az);
return JIT_ERR_NONE;
}
void snapshot_accel(freenect_device *dev) {
if (!last_timestamp)
return;
freenect_raw_tilt_state* state;
freenect_update_tilt_state(dev);
state = freenect_get_tilt_state(dev);
dump('a', last_timestamp, state, sizeof *state);
}
int freenect_sync_get_tilt_state(freenect_raw_tilt_state **state, int index)
{
if (runloop_enter(index)) return -1;
freenect_update_tilt_state(kinects[index]->dev);
*state = freenect_get_tilt_state(kinects[index]->dev);
runloop_exit();
return 0;
}
//---------------------------------------------------------------------------
void ofxKinect::threadedFunction(){
freenect_set_led(kinectDevice, LED_GREEN);
freenect_set_video_format(kinectDevice, bInfrared?FREENECT_VIDEO_IR_8BIT:FREENECT_VIDEO_YUV_RGB);
freenect_set_depth_format(kinectDevice, FREENECT_DEPTH_11BIT);
ofLog(OF_LOG_VERBOSE, "ofxKinect: Connection opened");
freenect_start_depth(kinectDevice);
if(bGrabVideo) {
freenect_start_video(kinectDevice);
}
// call platform specific processors (needed for Win)
if(freenect_process_events(kinectContext) != 0){
ofLog(OF_LOG_ERROR, "ofxKinect: freenect_process_events failed!");
return;
}
while(isThreadRunning()){
if(bTiltNeedsApplying){
freenect_set_tilt_degs(kinectDevice, targetTiltAngleDeg);
bTiltNeedsApplying = false;
}
if(bLedNeedsApplying){
freenect_set_led(kinectDevice, targetLedStatus);
bLedNeedsApplying = false;
}
freenect_update_tilt_state(kinectDevice);
freenect_raw_tilt_state * tilt = freenect_get_tilt_state(kinectDevice);
rawAccel.set(tilt->accelerometer_x, tilt->accelerometer_y, tilt->accelerometer_z);
double dx,dy,dz;
freenect_get_mks_accel(tilt, &dx, &dy, &dz);
mksAccel.set(dx, dy, dz);
ofSleepMillis(10);
// printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f %4f", ax, ay, az, dx, dy, dz);
}
// finish up a tilt on exit
if(bTiltNeedsApplying){
freenect_set_tilt_degs(kinectDevice, targetTiltAngleDeg);
bTiltNeedsApplying = false;
}
freenect_stop_depth(kinectDevice);
freenect_stop_video(kinectDevice);
freenect_set_led(kinectDevice, LED_YELLOW);
freenect_close_device(kinectDevice);
ofLog(OF_LOG_VERBOSE, "ofxKinect: Connection closed");
}
/*
* Class: org_ensor_robots_sensors_kinect_Device
* Method: nativeGetState
* Signature: (ILorg/ensor/robots/sensors/kinect/ITiltListener;)V
*/
JNIEXPORT void JNICALL Java_org_ensor_robots_sensors_kinect_Device_nativeGetState
(JNIEnv *aJNIEnv, jobject aThisObject, jint aDeviceId, jobject aTiltListener)
{
freenect_raw_tilt_state* state;
freenect_update_tilt_state(f_devices[aDeviceId].f_dev);
state = freenect_get_tilt_state(f_devices[aDeviceId].f_dev);
// TODO: Call the tilt listener interface.
}
int main(int argc, char **argv) {
freenect_context *fc;
freenect_device *fd;
if (freenect_init(&fc, 0) < 0) {
printf("failed to init freenect\n");
exit(1);
}
if (freenect_num_devices(fc) < 1) {
printf("no kinect found\n");
freenect_shutdown(fc);
exit(1);
}
if (freenect_open_device(fc, &fd, 0) < 0) {
printf("unable to open device 0\n");
freenect_shutdown(fc);
exit(1);
}
double want;
while (1) {
want = 30.0;
freenect_set_tilt_degs(fd, want);
freenect_set_led(fd, LED_RED);
while (fabs(freenect_get_tilt_degs(freenect_get_tilt_state(fd)) - want) > 1) {
freenect_update_tilt_state(fd);
usleep(1000);
}
want = -30.0;
freenect_set_tilt_degs(fd, want);
freenect_set_led(fd, LED_GREEN);
while (fabs(freenect_get_tilt_degs(freenect_get_tilt_state(fd)) - want) > 1) {
freenect_update_tilt_state(fd);
usleep(1000);
}
}
freenect_close_device(fd);
freenect_shutdown(fc);
exit(0);
}
//---------------------------------------------------------------------------
void ofxKinect::threadedFunction(){
freenect_set_led(kinectDevice, LED_GREEN);
freenect_frame_mode videoMode = freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, bIsVideoInfrared?FREENECT_VIDEO_IR_8BIT:FREENECT_VIDEO_RGB);
freenect_set_video_mode(kinectDevice, videoMode);
freenect_frame_mode depthMode = freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, bUseRegistration?FREENECT_DEPTH_REGISTERED:FREENECT_DEPTH_MM);
freenect_set_depth_mode(kinectDevice, depthMode);
ofLog(OF_LOG_VERBOSE, "ofxKinect: Device %d %s connection opened", deviceId, serial.c_str());
freenect_start_depth(kinectDevice);
if(bGrabVideo) {
freenect_start_video(kinectDevice);
}
// call platform specific processors (needed for Win)
if(freenect_process_events(kinectContext.getContext()) != 0) {
ofLog(OF_LOG_ERROR, "ofxKinect: Device %d freenect_process_events failed!", deviceId);
return;
}
while(isThreadRunning()) {
if(bTiltNeedsApplying) {
freenect_set_tilt_degs(kinectDevice, targetTiltAngleDeg);
bTiltNeedsApplying = false;
}
freenect_update_tilt_state(kinectDevice);
freenect_raw_tilt_state * tilt = freenect_get_tilt_state(kinectDevice);
currentTiltAngleDeg = freenect_get_tilt_degs(tilt);
rawAccel.set(tilt->accelerometer_x, tilt->accelerometer_y, tilt->accelerometer_z);
double dx,dy,dz;
freenect_get_mks_accel(tilt, &dx, &dy, &dz);
mksAccel.set(dx, dy, dz);
// ... and $0.02 for the scheduler
ofSleepMillis(10);
}
// finish up a tilt on exit
if(bTiltNeedsApplying) {
freenect_set_tilt_degs(kinectDevice, targetTiltAngleDeg);
bTiltNeedsApplying = false;
}
freenect_stop_depth(kinectDevice);
freenect_stop_video(kinectDevice);
freenect_set_led(kinectDevice, LED_YELLOW);
kinectContext.close(*this);
ofLog(OF_LOG_VERBOSE, "ofxKinect: Device %d connection closed", deviceId);
}
void *freenect_threadfunc(void *arg)
{
int accelCount = 0;
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_video_callback(f_dev, rgb_cb);
freenect_set_video_mode(f_dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, current_format));
freenect_set_depth_mode(f_dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
freenect_set_video_buffer(f_dev, rgb_back);
freenect_start_depth(f_dev);
freenect_start_video(f_dev);
printf("'w' - tilt up, 's' - level, 'x' - tilt down, '0'-'6' - select LED mode, '+' & '-' - change IR intensity \n");
printf("'f' - change video format, 'm' - mirror video, 'o' - rotate video with accelerometer \n");
printf("'e' - auto exposure, 'b' - white balance, 'r' - raw color, 'n' - near mode (K4W only) \n");
while (!die && freenect_process_events(f_ctx) >= 0) {
//Throttle the text output
if (accelCount++ >= 2000)
{
accelCount = 0;
freenect_raw_tilt_state* state;
freenect_update_tilt_state(f_dev);
state = freenect_get_tilt_state(f_dev);
double dx,dy,dz;
freenect_get_mks_accel(state, &dx, &dy, &dz);
printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f %4f", state->accelerometer_x, state->accelerometer_y, state->accelerometer_z, dx, dy, dz);
fflush(stdout);
}
if (requested_format != current_format) {
freenect_stop_video(f_dev);
freenect_set_video_mode(f_dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, requested_format));
freenect_start_video(f_dev);
current_format = requested_format;
}
}
printf("\nshutting down streams...\n");
freenect_stop_depth(f_dev);
freenect_stop_video(f_dev);
freenect_close_device(f_dev);
freenect_shutdown(f_ctx);
printf("-- done!\n");
return NULL;
}
void *freenect_threadfunc(void *arg)
{
int accelCount = 0;
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_video_callback(f_dev, rgb_cb);
freenect_set_video_format(f_dev, current_format);
freenect_set_depth_format(f_dev, FREENECT_DEPTH_11BIT);
freenect_set_video_buffer(f_dev, rgb_back);
freenect_start_depth(f_dev);
freenect_start_video(f_dev);
printf("'w'-tilt up, 's'-level, 'x'-tilt down, '0'-'6'-select LED mode, 'f'-video format\n");
while (!die && freenect_process_events(f_ctx) >= 0) {
//Throttle the text output
if (accelCount++ >= 2000)
{
accelCount = 0;
freenect_raw_tilt_state* state;
freenect_update_tilt_state(f_dev);
state = freenect_get_tilt_state(f_dev);
double dx,dy,dz;
freenect_get_mks_accel(state, &dx, &dy, &dz);
printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f %4f", state->accelerometer_x, state->accelerometer_y, state->accelerometer_z, dx, dy, dz);
fflush(stdout);
}
if (requested_format != current_format) {
freenect_stop_video(f_dev);
freenect_set_video_format(f_dev, requested_format);
freenect_start_video(f_dev);
current_format = requested_format;
}
}
printf("\nshutting down streams...\n");
freenect_stop_depth(f_dev);
freenect_stop_video(f_dev);
freenect_close_device(f_dev);
freenect_shutdown(f_ctx);
printf("-- done!\n");
return NULL;
}
//---------------------------------------------------------------------------
void ofxKinect::threadedFunction() {
freenect_set_led(kinectDevice, LED_GREEN);
freenect_set_video_format(kinectDevice, bInfrared?FREENECT_VIDEO_IR_8BIT:FREENECT_VIDEO_RGB);
freenect_set_depth_format(kinectDevice, FREENECT_DEPTH_11BIT);
freenect_set_depth_callback(kinectDevice, &grabDepthFrame);
freenect_set_video_callback(kinectDevice, &grabRgbFrame);
ofLog(OF_LOG_VERBOSE, "ofxKinect: Connection opened");
freenect_start_depth(kinectDevice);
freenect_start_video(kinectDevice);
while (isThreadRunning()) {
if( bTiltNeedsApplying ) {
freenect_set_tilt_degs(kinectDevice, targetTiltAngleDeg);
bTiltNeedsApplying = false;
}
freenect_update_tilt_state(kinectDevice);
freenect_raw_tilt_state * tilt = freenect_get_tilt_state(kinectDevice);
rawAccel.set(tilt->accelerometer_x, tilt->accelerometer_y, tilt->accelerometer_z);
double dx,dy,dz;
freenect_get_mks_accel(tilt, &dx, &dy, &dz);
mksAccel.set(dx, dy, dz);
ofSleepMillis(10);
// printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f %4f", ax, ay, az, dx, dy, dz);
}
//TODO: uncomment these when they are implemented in freenect
freenect_set_tilt_degs(kinectDevice, 0);
freenect_update_tilt_state(kinectDevice);
freenect_stop_depth(kinectDevice);
freenect_stop_video(kinectDevice);
freenect_set_led(kinectDevice, LED_YELLOW);
freenect_close_device(kinectDevice);
freenect_shutdown(kinectContext);
ofLog(OF_LOG_VERBOSE, "ofxKinect: Connection closed");
}
// the freenect thread.
void *freenect_threadfunc(void *arg) {
int accelCount = 0;
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_video_callback(f_dev, rgb_cb);
freenect_set_video_mode(f_dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, current_format));
freenect_set_depth_mode(f_dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
freenect_set_video_buffer(f_dev, rgb_back);
freenect_start_depth(f_dev);
freenect_start_video(f_dev);
while (!die && freenect_process_events(f_ctx) >= 0) {
if (accelCount++ >= 2000)
{
accelCount = 0;
freenect_raw_tilt_state* state;
freenect_update_tilt_state(f_dev);
state = freenect_get_tilt_state(f_dev);
double dx,dy,dz;
freenect_get_mks_accel(state, &dx, &dy, &dz);
}
if (requested_format != current_format) {
freenect_stop_video(f_dev);
freenect_set_video_mode(f_dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, requested_format));
freenect_start_video(f_dev);
current_format = requested_format;
}
}
freenect_stop_depth(f_dev);
freenect_stop_video(f_dev);
freenect_close_device(f_dev);
freenect_shutdown(f_ctx);
return NULL;
}
void DrawGLScene()
{
pthread_mutex_lock(&gl_backbuf_mutex);
// When using YUV_RGB mode, RGB frames only arrive at 15Hz, so we shouldn't force them to draw in lock-step.
// However, this is CPU/GPU intensive when we are receiving frames in lockstep.
if (current_format == FREENECT_VIDEO_YUV_RGB) {
while (!got_depth && !got_rgb) {
pthread_cond_wait(&gl_frame_cond, &gl_backbuf_mutex);
}
} else {
while ((!got_depth || !got_rgb) && requested_format != current_format) {
pthread_cond_wait(&gl_frame_cond, &gl_backbuf_mutex);
}
}
if (requested_format != current_format) {
pthread_mutex_unlock(&gl_backbuf_mutex);
return;
}
uint8_t *tmp;
if (got_depth) {
tmp = depth_front;
depth_front = depth_mid;
depth_mid = tmp;
got_depth = 0;
}
if (got_rgb) {
tmp = rgb_front;
rgb_front = rgb_mid;
rgb_mid = tmp;
got_rgb = 0;
}
pthread_mutex_unlock(&gl_backbuf_mutex);
glBindTexture(GL_TEXTURE_2D, gl_depth_tex);
glTexImage2D(GL_TEXTURE_2D, 0, 3, 640, 480, 0, GL_RGB, GL_UNSIGNED_BYTE, depth_front);
if (camera_rotate) {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
freenect_raw_tilt_state* state;
freenect_update_tilt_state(f_dev);
state = freenect_get_tilt_state(f_dev);
GLfloat x_accel_raw, x_accel,y_accel_raw,y_accel;
x_accel_raw = (GLfloat)state->accelerometer_x/819.0;
y_accel_raw = (GLfloat)state->accelerometer_y/819.0;
// sloppy acceleration vector cleanup
GLfloat accel_length = sqrt(x_accel_raw * x_accel_raw + y_accel_raw * y_accel_raw);
x_accel = x_accel_raw/accel_length;
y_accel = y_accel_raw/accel_length;
camera_angle = atan2(y_accel,x_accel)*180/M_PI -90.0;
}
else {
camera_angle = 0;
}
glLoadIdentity();
glPushMatrix();
glTranslatef((640.0/2.0),(480.0/2.0) ,0.0);
glRotatef(camera_angle, 0.0, 0.0, 1.0);
glTranslatef(-(640.0/2.0),-(480.0/2.0) ,0.0);
glBegin(GL_TRIANGLE_FAN);
glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
glTexCoord2f(0, 1); glVertex3f(0,0,1.0);
glTexCoord2f(1, 1); glVertex3f(640,0,1.0);
glTexCoord2f(1, 0); glVertex3f(640,480,1.0);
glTexCoord2f(0, 0); glVertex3f(0,480,1.0);
glEnd();
glPopMatrix();
glBindTexture(GL_TEXTURE_2D, gl_rgb_tex);
if (current_format == FREENECT_VIDEO_RGB || current_format == FREENECT_VIDEO_YUV_RGB)
glTexImage2D(GL_TEXTURE_2D, 0, 3, 640, 480, 0, GL_RGB, GL_UNSIGNED_BYTE, rgb_front);
else
glTexImage2D(GL_TEXTURE_2D, 0, 1, 640, 480, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE, rgb_front+640*4);
glPushMatrix();
glTranslatef(640+(640.0/2.0),(480.0/2.0) ,0.0);
glRotatef(camera_angle, 0.0, 0.0, 1.0);
glTranslatef(-(640+(640.0/2.0)),-(480.0/2.0) ,0.0);
glBegin(GL_TRIANGLE_FAN);
glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
glTexCoord2f(0, 1); glVertex3f(640,0,0);
glTexCoord2f(1, 1); glVertex3f(1280,0,0);
glTexCoord2f(1, 0); glVertex3f(1280,480,0);
glTexCoord2f(0, 0); glVertex3f(640,480,0);
glEnd();
glPopMatrix();
glutSwapBuffers();
}
FreenectTiltState getState() const {
return FreenectTiltState(freenect_get_tilt_state(m_dev));
}
/*!
* \brief store acceleration components in a, accel strut
* \param [out] a where acceleration components is saved as accel struct
*/
void Apikinect::getAccel(accel &a)
{
freenect_update_tilt_state(myself);
freenect_get_mks_accel(freenect_get_tilt_state(myself), &a.accel_x, &a.accel_y, &a.accel_z);
}
/*!
* \brief store angle in a
* \param [out] a where angle is saved as int
*/
void Apikinect::getAngle(int &a)
{
freenect_raw_tilt_state *state;
state = freenect_get_tilt_state(myself);
a = state->tilt_angle;
}
