//=============================================================================
void
tracker::start_logging(char* filename)
{
if (get_logging())
return;
char logfilename[128];
FILE* file;
timer t;
unsigned long time;
double x,y,z,ya,pi,ro;
if (filename == NULL)
if (get_default_log_filename(logfilename, 128) == -1)
{
#if TraceErrors
printf("tracker: start_logging: could not get default log filename\n");
#endif
return;
}
child_pid_ = fork();
if (child_pid_ == -1)
{
#if TraceErrors
printf("tracker: start_logging: error creating child process\n");
#endif
return;
}
set_logging(1);
if (child_pid_ == 0)
{
file = fopen(logfilename, "a+");
if (file == NULL)
{
#if TraceErrors
printf("tracker: start_logging: error opening file\n");
#endif
}
t.timer_reset();
while(get_logging())
{
get_pose(&x,&y,&z,&ya,&pi,&ro);
time = t.timer_read();
fprintf(file, "%lu %.2lf %.2lf %.2lf %.2lf %.2lf %.2lf\n", time, x, y, z, ya, pi, ro);
}
fclose(file);
exit(0);
}
}
//=============================================================================
int
tracker::get_default_xml_filename(char* filename, int max_length)
{
if (get_logging())
return -1;
int length;
char temp_filename[128];
struct tm* date;
time_t current_time;
//Get time, use to make default log filename
current_time = time(NULL);
date = localtime(¤t_time);
sprintf(temp_filename, "%02d%02d%02d-%02d_%02d_%02d.xml",
date->tm_year-100, date->tm_mon+1, date->tm_mday, date->tm_hour,
date->tm_min, date->tm_sec);
length = strlen(temp_filename);
if (length > max_length)
{
#if TraceErrors
printf("FusionMonitor: get_default_log_file_name: default log file name too long\n");
#endif
return -1;
}
strcpy(filename, temp_filename);
return 1;
}
//=============================================================================
void
tracker::set_boresight(double yaw, double pitch, double roll)
{
if (get_logging())
return;
//Wouldn't need to bother with by,bp,br if boresight was working on tracker
char buf[MAX_LENGTH];
if (get_angle_units() == ANGLE_UNIT_RAD)
{
by = DEG(yaw);
bp = DEG(pitch);
br = DEG(roll);
}
else
{
by = yaw;
bp = pitch;
br = roll;
}
sprintf(buf, "G%d,%.2lf,%.2lf,%.2lf\r", STATION, by, bp, br);
port->send_comport(buf);
/*
by = NORM_DEG(by);
bp = NORM_DEG(bp);
br = NORM_DEG(br);
*/
}
//=============================================================================
void
tracker::setup_boresight(int op)
{
if (get_logging())
return;
//One step calibration procedure
switch(op)
{
case INTERACTIVE:
clear_boresight();
printf("Place probe at desired origin orientation, then hit enter...");
getchar();
set_boresight_to_current();
printf("Done!\n");
break;
case STEP_0:
clear_boresight();
break;
case STEP_1: //Place probe at origin
set_boresight_to_current();
break;
default:
#if TraceErrors
printf("tracker: invalid op given to setup_angles_origin\n");
#endif
}
}
//=============================================================================
int
tracker::get_origin(double* x, double* y, double* z, double* px_x, double* px_y, double* px_z, double* py_x, double* py_y, double* py_z)
{
if (get_logging())
return -1;
double tx, ty, tz, tpx_x, tpx_y, tpx_z, tpy_x, tpy_y, tpy_z;
//Read hemisphere of operation
char buf[MAX_LENGTH];
sprintf(buf, "A%d\r", STATION);
port->send_comport(buf);
port->recv_comport(buf, MAX_LENGTH);
if (sscanf(buf, "%*s %lf %lf %lf", &tx, &ty, &tz, &tpx_x, &tpx_y, &tpx_z, &tpy_x, &tpy_y, &tpy_z) != 9)
return -1;
*x = tx;
*y = ty;
*z = tz;
*px_x = tpx_x;
*px_y = tpx_y;
*px_z = tpx_z;
*py_x = tpy_x;
*py_y = tpy_y;
*py_z = tpy_z;
return 1;
}
//=============================================================================
void
tracker::set_default_hemisphere(void)
{
if (get_logging())
return;
//Set hemisphere of operation with vector
set_hemisphere(DEF_HEM_X, DEF_HEM_Y, DEF_HEM_Z);
}
//=============================================================================
void
tracker::restore_system_defaults(void)
{
if (get_logging())
return;
port->send_comport("W");
set_boresight(0.0, 0.0, 0.0); //Need to do this manually since boresight not working
}
//=============================================================================
void
tracker::set_default_units(void)
{
if (get_logging())
return;
set_dist_units(DEFAULT_DIST_UNIT);
set_angle_units(DEFAULT_ANGLE_UNIT);
}
//=============================================================================
void
tracker::clear_origin(void)
{
if (get_logging())
return;
char buf[MAX_LENGTH];
sprintf(buf, "R%d\r", STATION);
port->send_comport(buf);
}
void DatabaseIO::verify_and_log(const GroupingEntity *ge, const Field &field, int in_out) const
{
if (ge != nullptr) {
assert(!is_parallel_consistent() ||
internal_parallel_consistent(singleProcOnly, ge, field, in_out, util_));
}
if (get_logging()) {
log_field(in_out == 1 ? ">" : "<", ge, field, singleProcOnly, util_);
}
}
//=============================================================================
void
tracker::store_current_to_eeprom(void)
{
if (get_logging())
return;
char buf[MAX_LENGTH];
sprintf(buf, "%c", 0x0b);
port->send_comport(buf);
}
//=============================================================================
void
tracker::set_hemisphere(double x, double y, double z)
{
if (get_logging())
return;
//Set hemisphere of operation with vector
char buf[MAX_LENGTH];
sprintf(buf, "H%d,%lf,%lf,%lf\r", STATION, x, y, z);
port->send_comport(buf);
}
//=============================================================================
void
tracker::stop_logging(void)
{
//Parent process gets this call
if (!get_logging())
return;
set_logging(0);
//Send interrupt to child
kill(child_pid_, SIGALRM);
}
//=============================================================================
void
tracker::set_origin(double x, double y, double z, double px_x, double px_y, double px_z, double py_x, double py_y, double py_z)
{
if (get_logging())
return;
char buf[MAX_LENGTH];
sprintf(buf, "A%d,%.2lf,%.2lf,%.2lf,%.2lf,%.2lf,%.2lf,%.2lf,%.2lf,%.2lf\r", STATION, x, y, z, px_x, px_y, px_z, py_x, py_y, py_z);
port->send_comport(buf);
}
//=============================================================================
void
tracker::restart_tracker(void)
{
if (get_logging())
return;
timer t;
char buf[MAX_LENGTH];
sprintf(buf, "%c", 0x19);
port->send_comport(buf);
t.timer_sleep(RESTART_TIME);//alow time seconds for system restart
}
//=============================================================================
void
tracker::clear_boresight(void)
{
if (get_logging())
return;
//char buf[MAX_LENGTH];
//sprintf(buf, "b%d\r", STATION);
//port->send_comport(buf);
//Boresight not working, do it manually
set_boresight(0.0, 0.0, 0.0);
}
//=============================================================================
void
tracker::set_boresight_to_current(void)
{
if (get_logging())
return;
//char buf[MAX_LENGTH];
//sprintf(buf, "B%d\r", STATION);
//port->send_comport(buf);
//Boresight not working, do it manually
double yaw, pitch, roll;
get_pose(NULL, NULL, NULL, &yaw, &pitch, &roll);
set_boresight(yaw, pitch, roll);
}
//=============================================================================
void
tracker::set_angle_units(int angle_units)
{
if (get_logging())
return;
if (ANGLE_UNIT_VALID(angle_units))
angle_units_ = angle_units;
else
{
#if TraceErrors
printf("tracker: invalid distance unit, current unit not changed\n");
#endif
}
}
//=============================================================================
void
tracker::set_dist_units(int dist_units)
{
if (get_logging())
return;
//if valid assign, otherwise print error
if (DIST_UNIT_VALID(dist_units))
dist_units_ = dist_units;
else
{
#if TraceErrors
printf("tracker: invalid distance unit, current unit not changed\n");
#endif
}
}
//=============================================================================
void
tracker::setup_origin(int op)
{
if (get_logging())
return;
//Three step calibration procedure
char buf[MAX_LENGTH];
switch(op)
{
case INTERACTIVE:
clear_origin();
port->send_comport("A0\r");
port->recv_comport(buf, MAX_LENGTH);
printf("Place probe at desired xyz origin, then hit enter...\n");
getchar();
port->send_comport("P");
port->recv_comport(buf, MAX_LENGTH);
printf("Place probe 20-40cm along the positive X axis, then hit enter...\n");
getchar();
port->send_comport("P");
port->recv_comport(buf, MAX_LENGTH);
printf("Place probe 20-40cm along the positive Y axis, then hit enter...\n");
getchar();
port->send_comport("P");
port->recv_comport(buf, MAX_LENGTH);
printf("Done!\n");
break;
case STEP_0:
clear_origin();
port->send_comport("A0\r");
port->recv_comport(buf, MAX_LENGTH);
break;
case STEP_1: //Place probe at origin
case STEP_2: //Place probe on X
case STEP_3: //Place probe on Y
port->send_comport("P");
port->recv_comport(buf, MAX_LENGTH);
break;
default:
#if TraceErrors
printf("tracker: invalid op given to setup_xyz_origin\n");
#endif
}
}
//=============================================================================
int
tracker::get_hemisphere(double* x, double* y, double* z)
{
if (get_logging())
return -1;
//double tx, ty, tz;
double values[3];
//Read hemisphere of operation
char buf[MAX_LENGTH];
sprintf(buf, "H%d\r", STATION);
port->send_comport(buf);
port->recv_comport(buf, MAX_LENGTH);
parse_stupid_string(buf, 3, values, 3);
// if (sscanf(buf, "%*s %lf %lf %lf", &tx, &ty, &tz) != 3)
// return -1;
*x = values[0];
*y = values[1];
*z = values[2];
return 1;
}
//=============================================================================
int
tracker::get_system_boresight(double* yaw, double* pitch, double* roll)
{
if (get_logging())
return -1;
//Read system boresight, only used by constructor
//This would be get_boresight(), if boresight was working on tracker
//double y,p,r;
double values[3];
char buf[MAX_LENGTH];
sprintf(buf, "G%d\r", STATION);
port->send_comport(buf);
port->recv_comport(buf, MAX_LENGTH);
parse_stupid_string(buf, 3, values, 3);
*yaw = values[0];
*pitch = values[1];
*roll = values[2];
return 1;
}
//=============================================================================
void
tracker::start_logging_xml(char* filename)
{
if (get_logging())
return;
char logfilename[128];
FILE* file;
timer t;
unsigned long time;
double x,y,z,ya,pi,ro;
//double *x, *y, *z, *ya, *pi, *ro;
//int num = 2;
if (filename == NULL)
if (get_default_xml_filename(logfilename, 128) == -1)
{
#if TraceErrors
printf("tracker: start_logging: could not get default log filename\n");
#endif
return;
}
child_pid_ = fork();
if (child_pid_ == -1)
{
#if TraceErrors
printf("tracker: start_logging: error creating child process\n");
#endif
return;
}
set_logging(1);
if (child_pid_ == 0)
{
file = fopen(logfilename, "a+");
if (file == NULL)
{
#if TraceErrors
printf("tracker: start_logging: error opening file\n");
#endif
}
t.timer_reset();
fprintf(file,"<demo>\n");
while(get_logging())
{
get_pose(&x,&y,&z,&ya,&pi,&ro);
//get_multi_pose(num,&x,&y,&z,&ya,&pi,&ro);
time = t.timer_read();
fprintf(file,"<point>\n");
fprintf(file,"<time>%.3lf</time>\n",((double)time) /1000 );
fprintf(file,"<end-effector>");
fprintf(file, "<position>%.2lf;%.2lf;%.2lf</position><YPR>%.2lf;%.2lf;%.2lf</YPR>", x, y, z, ya, pi, ro);
//fprintf(file, "<position>%.2lf;%.2lf;%.2lf</position><YPR>%.2lf;%.2lf;%.2lf</YPR>", x, y, z, ya, pi, ro);
printf("%lu %.2lf %.2lf %.2lf %.2lf %.2lf %.2lf\n",time, x, y, z, ya, pi, ro);
/*for(int i=0;i<num;i++){
printf("%lu %.2lf %.2lf %.2lf %.2lf %.2lf %.2lf\n",time, x[i], y[i], z[i], ya[i], pi[i], ro[i]);
}*/
fprintf(file,"</end-effector>\n");
fprintf(file,"</point>\n");
}
fprintf(file,"</demo>\n");
fclose(file);
exit(0);
}
}
