void device::work()
{
load_options();
create_client();
delegate_all();
initialize_device();
}
KinectHelper::KinectHelper(QObject *parent) : QObject(parent){
/// Initialize and open device, create depth stream
/// NOTE: by doing this in the constructor it remains in the main thread,
/// so the plugin will lock until Kinect is initialized
bool success = initialize_device();
if(!success)
qDebug()<<"Failed to initialize Kinect";
/// GUI Elements
color_label = NULL;
depth_label = NULL;
/// Instantiate listeners
depthListener = new ModeKinectListener(this);
colorListener = new ModeKinectListener(this);
depth.addNewFrameListener(depthListener);
color.addNewFrameListener(colorListener);
/// Buffer pointers always valid
points_front_buffer = &points_buffer_1;
points_back_buffer = &points_buffer_2;
color_front_buffer = &color_buffer_1;
color_back_buffer = &color_buffer_2;
depth_front_buffer = &depth_buffer_1;
depth_back_buffer = &depth_buffer_2;
/// Avoid displaying stuff when data is not ready
has_consumed_first_frame = false;
///Initialize the data sizes
{
VideoFrameRef frame;
Status status = depth.readFrame(&frame);
if(!status == STATUS_OK)
throw StarlabException("Failed to initialize Kinect");
/// Allocate memory
points_buffer_1.resize(frame.getHeight(), frame.getWidth());
points_buffer_2.resize(frame.getHeight(), frame.getWidth());
color_buffer_1 = QImage(frame.getWidth(),frame.getHeight(),QImage::Format_RGB888);
color_buffer_2 = QImage(frame.getWidth(),frame.getHeight(),QImage::Format_RGB888);
depth_buffer_1 = QImage(frame.getWidth(),frame.getHeight(),QImage::Format_RGB888);
depth_buffer_2 = QImage(frame.getWidth(),frame.getHeight(),QImage::Format_RGB888);
/// Communicate the viewport to the GUI
compute_frame_bbox(frame);
emit scene_bbox_updated(_bbox);
}
/// Allow passing VideoFrameRefs
qRegisterMetaType<VideoFrameRef>("VideoFrameRef");///< Allow pass reference frame as signals
}
static int open_tinyalsa(out123_handle *ao)
{
debug("open_tinyalsa()");
mpg123_tinyalsa_t* ta = (mpg123_tinyalsa_t*)ao->userptr;
if (ao->format != -1)
{
/* we're going to play: initalize sample format */
return initialize_device(ao);
}
else
{
/* query mode; sample format will be set for each query */
return 0;
}
}
struct upnp_device *upnp_device_init(struct upnp_device_descriptor *device_def,
const char *ip_address,
unsigned short port)
{
int rc;
char *buf;
struct service *srv;
struct icon *icon_entry;
assert(device_def != NULL);
if (device_def->init_function) {
rc = device_def->init_function();
if (rc != 0) {
return NULL;
}
}
struct upnp_device *result_device = malloc(sizeof(*result_device));
result_device->upnp_device_descriptor = device_def;
ithread_mutex_init(&(result_device->device_mutex), NULL);
/* register icons in web server */
for (int i = 0; (icon_entry = device_def->icons[i]); i++) {
webserver_register_file(icon_entry->url, "image/png");
}
/* generate and register service schemas in web server */
for (int i = 0; (srv = device_def->services[i]); i++) {
buf = upnp_get_scpd(srv);
assert(buf != NULL);
webserver_register_buf(srv->scpd_url, buf, "text/xml");
}
if (!initialize_device(device_def, result_device, ip_address, port)) {
UpnpFinish();
free(result_device);
return NULL;
}
return result_device;
}
int audio_open(struct audio_info_struct *ai)
{
const char *pcm_name;
snd_pcm_t *pcm;
pcm_name = ai->device ? ai->device : "default";
if (snd_pcm_open(&pcm, pcm_name, SND_PCM_STREAM_PLAYBACK, 0) < 0) {
fprintf(stderr, "audio_open(): cannot open device %s\n", pcm_name);
return -1;
}
ai->handle = pcm;
if (ai->format != -1) {
/* we're going to play: initalize sample format */
initialize_mixer_device(ai, NULL);
return initialize_device(ai);
} else {
/* query mode; sample format will be set for each query */
return 0;
}
}
static int open_alsa(audio_output_t *ao)
{
const char *pcm_name;
snd_pcm_t *pcm=NULL;
printf("open_alsa with %p", ao->userptr);
#ifndef DEBUG
if(0) snd_lib_error_set_handler(error_ignorer);
#endif
pcm_name = ao->device ? ao->device : "default";
if (snd_pcm_open(&pcm, pcm_name, SND_PCM_STREAM_PLAYBACK, 0) < 0) {
if(!0) printf("cannot open device %s", pcm_name);
return -1;
}
ao->userptr = pcm;
if (ao->format != -1) {
/* we're going to play: initalize sample format */
return initialize_device(ao);
} else {
/* query mode; sample format will be set for each query */
return 0;
}
}
static
void* update_thread(void* data)
{
struct timespec ts;
int ret;
unsigned int diff, ntot, seed;
struct gtec_device* gtdev = data;
pthread_mutex_t* lock = &(gtdev->updatelock);
struct random_data rdata = {.rand_type = 0};
int32_t randnum = 0;
char state[128] = {0};
// Initialize random generator
clock_gettime(CLOCK_REALTIME, &ts);
seed = ts.tv_nsec;
initstate_r(seed, state, sizeof(state), &rdata);
// Wait for acquisition start
pthread_mutex_lock(&acqlock);
while (!acquiring) {
pthread_cond_wait(&acqcond, &acqlock);
}
pthread_mutex_unlock(&acqlock);
memcpy(&ts, &org, sizeof(ts));
random_r(&rdata, &randnum);
addtime(&ts, 0, 7000000 + randnum/2500);
pthread_mutex_lock(lock);
while (gtdev->running) {
ret = pthread_cond_timedwait(>dev->cond, lock, &ts);
if (ret == ETIMEDOUT) {
diff = difftime_ms(&ts, &org);
ntot = (diff*gtdev->conf.sample_rate)/1000;
gtdev->nsample = ntot;
pthread_mutex_unlock(lock);
gtdev->callback(gtdev->callback_data);
random_r(&rdata, &randnum);
addtime(&ts, 0, 7000000 + randnum/2500);
pthread_mutex_lock(lock);
}
}
pthread_mutex_unlock(lock);
return NULL;
}
API_EXPORTED
void GT_ShowDebugInformation( gt_bool show )
{
(void)show;
}
API_EXPORTED
gt_bool GT_UpdateDevices()
{
static int isinit = 0;
int i;
if (isinit)
return GT_TRUE;
for (i=0; i<NUMDEV; i++)
initialize_device(>devices[i]);
isinit = 1;
return GT_TRUE;
}
API_EXPORTED
gt_size GT_GetDeviceListSize()
{
return NUMDEV;
}
API_EXPORTED
char** GT_GetDeviceList()
{
int i;
char** devlist;
devlist = malloc(NUMDEV*sizeof(devlist[0]));
for (i=0; i<NUMDEV; i++) {
devlist[i] = malloc(strlen(devname[i])+1);
strcpy(devlist[i], devname[i]);
}
return devlist;
}
int main(int argc, char *argv[]) {
mems_t dev;
coord_t begin, end;
double spring_factor = 0.0;
double accel;
double length_nm;
double velocity_nm_s;
double seek_time;
double seek_time_zero = 0.0;
int bit_step = 10;
int i;
int verbose = 0;
int three_dee = 0;
int mathematica = 0;
int transpose = 0;
int hong = 0;
double time_constants = 0.0;
int x_pos, y_pos;
begin.x_pos = 1000;
begin.y_pos = 1000;
//start_nm = -1;
//end_nm = -1;
spring_factor = 0.0;
for (i = 0; i < argc; i++) {
if (strcmp(argv[i], "-v") == 0)
verbose = 1;
if (strcmp(argv[i], "-spring") == 0)
spring_factor = atof(argv[i+1]);
if (strcmp(argv[i], "-step") == 0)
bit_step = atoi(argv[i+1]);
if (strcmp(argv[i], "-start") == 0)
//start_nm = atof(argv[i+1]);
if (strcmp(argv[i], "-end") == 0)
//end_nm = atof(argv[i+1]);
if (strcmp(argv[i], "-3d") == 0)
three_dee = 1;
if (strcmp(argv[i], "-num") == 0)
time_constants = atof(argv[i+1]);
if (strcmp(argv[i], "-x") == 0)
begin.x_pos = atoi(argv[i+1]);
if (strcmp(argv[i], "-y") == 0)
begin.y_pos = atoi(argv[i+1]);
if (strcmp(argv[i], "-math") == 0)
mathematica = atoi(argv[i+1]);
if (strcmp(argv[i], "-transpose") == 0)
transpose = 1;
if (strcmp(argv[i], "-hong") == 0)
hong = 1;
}
accel = 746.2 * 1000000000.0;
length_nm = 2000.0 * 50.0;
//velocity_nm_s = 200000.0 * 50.0;
velocity_nm_s = 0.0;
dev.sled = malloc(sizeof(mems_sled_t));
initialize_device(&dev,
spring_factor,
time_constants,
hong);
begin.y_vel = 0;
end.x_pos = 0;
end.y_pos = 0;
end.y_vel = 0;
//fprintf(stdout, "spring_factor = %f\n", spring_factor);
for(x_pos = -1000; x_pos <= 1000; x_pos += bit_step) {
for(y_pos = -1000; y_pos <= 1000; y_pos += bit_step) {
/*
for(x_pos = 0; x_pos <= 1000; x_pos += bit_step) {
for(y_pos = 0; y_pos <= 1000; y_pos += bit_step) {
*/
//x_pos = 1000;
//y_pos = 500;
if (!transpose) {
end.x_pos = x_pos;
end.y_pos = y_pos;
} else {
end.x_pos = y_pos;
end.y_pos = x_pos;
}
if (verbose) fprintf(stdout, "main: starting a new seek (monkey)\n");
seek_time = mems_seek_time(&(dev.sled[0]),
&begin, &end,
NULL, NULL,
NULL, NULL);
spring_factor = dev.sled[0].spring_factor;
dev.sled[0].spring_factor = 0.0;
seek_time_zero = mems_seek_time(&(dev.sled[0]),
&begin, &end,
NULL, NULL,
NULL, NULL);
dev.sled[0].spring_factor = spring_factor;
if (verbose) fprintf(stdout, "goose\n");
if (mathematica == 1) {
fprintf(stdout, "%f ", seek_time);
} else if (mathematica == 2) {
fprintf(stdout, "%f ", (seek_time - seek_time_zero));
} else {
if (!transpose) {
fprintf(stdout, "%d %d %f %f %f\n",
x_pos, y_pos, seek_time, seek_time_zero, (seek_time - seek_time_zero));
} else {
fprintf(stdout, "%d %d %f %f %f\n",
y_pos, x_pos, seek_time, seek_time_zero, (seek_time - seek_time_zero));
}
/*
fprintf(stdout, "%f %f %f %f\n",
x_velocity_diff, x_time_diff, y_velocity_diff, y_time_diff);
*/
}
}
if (mathematica != 0) fprintf(stdout, "\n");
}
free(dev.sled);
return 0;
}
API_EXPORTED int usb_find_devices(void)
{
struct usb_bus *bus;
libusb_device **dev_list;
int dev_list_len;
int r;
int changes = 0;
/* libusb-1.0 initialization might have failed, but we can't indicate
* this with libusb-0.1, so trap that situation here */
if (!ctx)
return 0;
usbi_dbg("");
dev_list_len = libusb_get_device_list(ctx, &dev_list);
if (dev_list_len < 0)
return compat_err(dev_list_len);
for (bus = usb_busses; bus; bus = bus->next) {
struct usb_device *new_devices = NULL;
struct usb_device *dev;
r = find_devices(dev_list, dev_list_len, bus, &new_devices);
if (r < 0) {
libusb_free_device_list(dev_list, 1);
return r;
}
/* walk through the devices we already know about, removing duplicates
* from the new list. if we do not find it in the new list, the device
* has been removed. */
dev = bus->devices;
while (dev) {
int found = 0;
struct usb_device *tdev = dev->next;
struct usb_device *ndev = new_devices;
while (ndev) {
if (ndev->devnum == dev->devnum) {
LIST_DEL(new_devices, ndev);
free(ndev);
found = 1;
break;
}
ndev = ndev->next;
}
if (!found) {
usbi_dbg("device %d.%d removed",
dev->bus->location, dev->devnum);
LIST_DEL(bus->devices, dev);
free_device(dev);
changes++;
}
dev = tdev;
}
/* anything left in new_devices is a new device */
dev = new_devices;
while (dev) {
struct usb_device *tdev = dev->next;
r = initialize_device(dev);
if (r < 0) {
usbi_err("couldn't initialize device %d.%d (error %d)",
dev->bus->location, dev->devnum, r);
dev = tdev;
continue;
}
usbi_dbg("device %d.%d added", dev->bus->location, dev->devnum);
LIST_DEL(new_devices, dev);
LIST_ADD(bus->devices, dev);
changes++;
dev = tdev;
}
}
libusb_free_device_list(dev_list, 1);
return changes;
}
