int ltr_int_master_main_loop(int socket)
{
int res;
int new_fd;
bool close_conn;
int heartbeat = 0;
nfds_t i;
no_slaves = false;
struct sockaddr_un address;
socklen_t address_len = sizeof(address);
add_poll_desc(socket);
while(1) {
if(gui_shutdown_request || (!ltr_int_gui_lock(false)) ||
no_slaves || (ltr_int_get_tracking_state() < LINUXTRACK_OK)) {
break;
}
close_conn = false;
for(i = 0; i < current_len; ++i) {
descs[i].revents = 0;
}
res = poll(descs, current_len, 2000);
if(res < 0) {
ltr_int_my_perror("poll");
continue;
} else if(res == 0) {
if(ltr_int_get_tracking_state() == PAUSED) {
++heartbeat;
if(heartbeat > 5) {
linuxtrack_full_pose_t dummy;
dummy.pose.pitch = 0.0;
dummy.pose.yaw = 0.0;
dummy.pose.roll = 0.0;
dummy.pose.tx = 0.0;
dummy.pose.ty = 0.0;
dummy.pose.tz = 0.0;
dummy.pose.counter = 0;
dummy.pose.status = PAUSED;
dummy.blobs = 0;
ltr_int_broadcast_pose(dummy);
heartbeat = 0;
}
} else {
heartbeat = 0;
}
continue;
}
for(i = 0; i < current_len; ++i) {
if(descs[i].revents == 0) {
continue;
}
if(descs[i].revents) {
if(descs[i].revents & POLLIN) {
if(descs[i].fd == socket) {
do {
new_fd = accept(socket, (struct sockaddr*)&address, &address_len);
if(new_fd < 0) {
if(errno != EWOULDBLOCK) {
ltr_int_my_perror("accept");
}
//No more connection requests
break;
} else {
add_poll_desc(new_fd);
}
} while(new_fd >= 0);
} else {
message_t msg;
msg.cmd = CMD_NOP;
ssize_t x = ltr_int_socket_receive(descs[i].fd, &msg, sizeof(message_t));
//ltr_int_log_message("Read %d bytes from fd %d.\n", (int)x, descs[i].fd);
if(x < 0) {
if(x != -EWOULDBLOCK) {
ltr_int_log_message("Unexpected error %d reading from fd %d.\n", x, descs[i].fd);
close_conn = true;
descs[i].fd = -1;
}
} else if(x == 0) {
//close_conn = true;
//descs[i].fd = -1;
} else {
//ltr_int_log_message("Received a message from slave (%d)!!!\n", msg.cmd);
switch(msg.cmd) {
case CMD_PAUSE:
ltr_int_suspend_cmd();
break;
case CMD_WAKEUP:
ltr_int_wakeup_cmd();
break;
case CMD_RECENTER:
ltr_int_recenter_cmd();
break;
case CMD_NEW_SOCKET:
//ltr_int_log_message("Cmd to register new slave...\n");
ltr_int_register_slave(descs[i].fd, msg);
break;
case CMD_FRAMES:
ltr_int_publish_frames_cmd();
break;
}
}
}
}
if(descs[i].revents & POLLHUP) {
ltr_int_log_message("Hangup at fd %d\n", descs[i].fd);
close_conn = true;
}
}
}
if(close_conn) {
remove_poll_desc();
}
}
return 0;
}
static bool ltr_int_process_message(int l_master_uplink)
{
message_t msg;
struct ltr_comm *com;
linuxtrack_pose_t unfiltered;
ssize_t bytesRead = ltr_int_socket_receive(l_master_uplink, &msg, sizeof(message_t));
if(bytesRead < 0){
ltr_int_log_message("Slave reader problem!\n");
ltr_int_my_perror("socket_receive");
return false;
}else if(bytesRead == 0){
return true;
}
switch(msg.cmd){
case CMD_NOP:
break;
case CMD_POSE:
//printf("Have new pose!\n");
//printf(">>>>%f %f %f\n", msg.pose.raw_yaw, msg.pose.raw_pitch, msg.pose.raw_tz);
ltr_int_postprocess_axes(axes, &(msg.pose.pose), &unfiltered);
//printf(">>>>%f %f %f\n", msg.pose.yaw, msg.pose.pitch, msg.pose.tz);
//printf("Raw center: %f %f %f\n", msg.pose.pose.raw_tx, msg.pose.pose.raw_ty, msg.pose.pose.raw_tz);
//printf("Raw angles: %f %f %f\n", msg.pose.pose.raw_pitch, msg.pose.pose.raw_yaw, msg.pose.pose.raw_roll);
com = mmm.data;
ltr_int_lockSemaphore(mmm.sem);
//printf("STATUS: %d\n", msg.pose.status);
if(msg.pose.pose.status == RUNNING){
//printf("PASSING TO SHM: %f %f %f\n", msg.pose.yaw, msg.pose.pitch, msg.pose.tz);
com->full_pose = msg.pose;
}
com->state = msg.pose.pose.status;
com->preparing_start = false;
ltr_int_unlockSemaphore(mmm.sem);
if(notify && (notify_pipe > 0)){
uint8_t tmp = 0;
if(write(notify_pipe, &tmp, 1) < 0){
//Don't report, it would overfill logs
}
}
break;
case CMD_PARAM:
//printf("Changing %s of %s to %f!!!\n", ltr_int_axis_param_get_desc(msg.param.param_id),
// ltr_int_axis_get_desc(msg.param.axis_id), msg.param.flt_val);
if(msg.param.axis_id == MISC){
switch(msg.param.param_id){
case MISC_ALTER:
ltr_int_set_use_alter(msg.param.flt_val > 0.5f);
break;
case MISC_ALIGN:
ltr_int_set_tr_align(msg.param.flt_val > 0.5f);
break;
case MISC_LEGR:
ltr_int_set_use_oldrot(msg.param.flt_val > 0.5f);
break;
case MISC_FOCAL_LENGTH:
ltr_int_set_focal_length(msg.param.flt_val);
break;
default:
ltr_int_log_message("Wrong misc param: %d\n", msg.param.param_id);
return false;
break;
}
}else if(msg.param.param_id == AXIS_ENABLED){
ltr_int_set_axis_bool_param(axes, msg.param.axis_id, msg.param.param_id, msg.param.flt_val > 0.5f);
}else{
ltr_int_set_axis_param(axes, msg.param.axis_id, msg.param.param_id, msg.param.flt_val);
}
break;
default:
ltr_int_log_message("Slave received unexpected message %d!\n", msg.cmd);
return false;
break;
}
//printf("Received: '%s'\n", msg.str);
return true;
}
