void ltr_int_gui_lock_clean()
{
if(pfSem != NULL) {
ltr_int_unlockSemaphore(pfSem);
ltr_int_closeSemaphore(pfSem);
pfSem= NULL;
}
}
static bool start_master(int *l_master_uplink, int *l_master_downlink)
{
bool is_child;
//master inherits fds!
int fifo = ltr_int_open_fifo_exclusive(ltr_int_master_fifo_name(), &master_lock);
if(fifo > 0){
//no master there yet, so lets start one
close(fifo);
ltr_int_unlockSemaphore(master_lock);
ltr_int_closeSemaphore(master_lock);
close_master_comms(l_master_uplink, l_master_downlink);
ltr_int_log_message("Master is not running, start it\n");
char *args[] = {"srv", NULL};
args[0] = ltr_int_get_app_path("/ltr_server1");
ltr_int_fork_child(args, &is_child);
int status;
//Disable the wait when not daemonizing master!!!
wait(&status);
//At this point master is either running or exited (depending on the state of fifo)
free(args[0]);
}
//At this point either master runs already, or we just started one
return true;
}
static void *ltr_int_slave_reader_thread(void *param)
{
ltr_int_log_message("Slave reader thread function entered!\n");
(void) param;
int received_frames = 0;
master_works = false;
while(1){
if(!ltr_int_try_start_master(&master_uplink, &master_downlink)){
break;
}
ltr_int_log_message("Master Uplink %d, Downlink %d\n", master_uplink, master_downlink);
int poll_errs = 0;
struct pollfd downlink_poll= {
.fd = master_downlink,
.events = POLLIN,
.revents = 0
};
while(1){
downlink_poll.events = POLLIN;
int fds = poll(&downlink_poll, 1, 1000);
if(fds < 0){
++poll_errs;
ltr_int_my_perror("poll");
if(poll_errs > 3){break;}else{continue;}
}else if(fds == 0){
if(!parent_alive()){
//printf("Parent %lu died! (1)\n", (unsigned long)ppid);
return NULL;
}
continue;
}
if(downlink_poll.revents & POLLHUP){
break;
}
//We have a new message
if(ltr_int_process_message(master_downlink)){
++received_frames;
if(received_frames > 20){
master_works = true;
}
}
if(!parent_alive()){
//printf("Parent %lu died! (2)\n", (unsigned long)ppid);
return NULL;
}
}
}
return NULL;
}
static void ltr_int_slave_main_loop()
{
//Prepare to process client requests
struct ltr_comm *com = mmm.data;
ltr_cmd cmd = NOP_CMD;
bool recenter = false;
bool quit_flag = false;
while(!quit_flag){
if((com->cmd != NOP_CMD) || com->recenter || com->notify){
ltr_int_lockSemaphore(mmm.sem);
cmd = (ltr_cmd)com->cmd;
com->cmd = NOP_CMD;
recenter = com->recenter;
notify = com->notify;
com->recenter = false;
ltr_int_unlockSemaphore(mmm.sem);
}
int res = 0;
do{
switch(cmd){
case PAUSE_CMD:
res = ltr_int_send_message(master_uplink, CMD_PAUSE, 0);
break;
case RUN_CMD:
res = ltr_int_send_message(master_uplink, CMD_WAKEUP, 0);
break;
case STOP_CMD:
quit_flag = true;
break;
case FRAMES_CMD:
res = ltr_int_send_message(master_uplink, CMD_FRAMES, 0);
break;
default:
break;
}
if(res < 0){
usleep(100000);
}
if(!parent_alive()){
//printf("Parent %lu died! (3)\n", (unsigned long)ppid);
break;
}
}while((res < 0) && (!quit_flag));
cmd = NOP_CMD;
if(recenter){
ltr_int_log_message("Slave sending master recenter request!\n");
if(ltr_int_send_message(master_uplink, CMD_RECENTER, 0) >= 0){
//clear request only on successfull transmission
recenter = false;
}
}
if(!parent_alive()){
//printf("Parent %lu died! (3)\n", (unsigned long)ppid);
break;
}
usleep(100000);
}
}
//main slave function
bool ltr_int_slave(const char *c_profile, const char *c_com_file, const char *ppid_str,
const char *close_pipe_str, const char *notify_pipe_str)
{
unsigned long tmp_ppid;
profile_name = ltr_int_my_strdup(c_profile);
sscanf(ppid_str, "%lu", &tmp_ppid);
int tmp_pipe = -1;
sscanf(close_pipe_str, "%d", &tmp_pipe);
if(tmp_pipe > 0){
close(tmp_pipe);
}
sscanf(notify_pipe_str, "%d", ¬ify_pipe);
if(notify_pipe > 0){
fcntl(notify_pipe, F_SETFL, fcntl(notify_pipe, F_GETFL) | O_NONBLOCK);
}
//printf("Going to monitor parent %lu!\n", tmp_ppid);
ppid = (pid_t)tmp_ppid;
if(!ltr_int_read_prefs(NULL, false)){
ltr_int_log_message("Couldn't load preferences!\n");
return false;
}
ltr_int_init_axes(&axes, profile_name);
//Prepare client comm channel
char *com_file = ltr_int_my_strdup(c_com_file);
if(!ltr_int_mmap_file(com_file, sizeof(struct ltr_comm), &mmm)){
ltr_int_log_message("Couldn't mmap file!!!\n");
return false;
}
free(com_file);
if(pthread_create(&reader_tid, NULL, ltr_int_slave_reader_thread, NULL) == 0){
ltr_int_slave_main_loop();
pthread_join(reader_tid, NULL);
}
close_master_comms(&master_uplink, &master_downlink);
ltr_int_unmap_file(&mmm);
//finish prefs
ltr_int_close_axes(&axes);
ltr_int_free_prefs();
free(profile_name);
ltr_int_gui_lock_clean();
close(notify_pipe);
return true;
}
static bool ltr_int_process_message(int l_master_downlink)
{
message_t msg;
struct ltr_comm *com;
linuxtrack_pose_t unfiltered;
ssize_t bytesRead = ltr_int_fifo_receive(l_master_downlink, &msg, sizeof(message_t));
if(bytesRead < 0){
ltr_int_log_message("Slave reader problem!\n");
ltr_int_my_perror("fifo_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;
}
