void vrpn_Tracker_ButtonFly::mainloop()
{
int i;
struct timeval now;
double interval; // How long since the last report, in secs
// Call generic server mainloop, since we are a server
server_mainloop();
// Mainloop() all of the buttons that are defined and the analog
// scale values if they are defined so that we will get all of
// the values fresh.
for (i = 0; i < d_num_axes; i++) {
d_axes[i].btn->mainloop();
}
if (d_vel_scale != NULL) { d_vel_scale->mainloop(); };
if (d_rot_scale != NULL) { d_rot_scale->mainloop(); };
// See if it has been long enough since our last report.
// If so, generate a new one.
vrpn_gettimeofday(&now, NULL);
interval = duration(now, d_prevtime);
if (shouldReport(interval)) {
// Figure out the new matrix based on the current values and
// the length of the interval since the last report
update_matrix_based_on_values(interval);
d_prevtime = now;
// Set the time on the report to now.
vrpn_Tracker::timestamp = now;
// pack and deliver tracker report;
if (d_connection) {
char msgbuf[1000];
int len = encode_to(msgbuf);
if (d_connection->pack_message(len, vrpn_Tracker::timestamp,
position_m_id, d_sender_id, msgbuf,
vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr,"Tracker ButtonFly: cannot write message: tossing\n");
}
} else {
fprintf(stderr,"Tracker ButtonFly: No valid connection\n");
}
// We're not always sending reports, but we still want to
// update the matrix so that we don't integrate over
// too long a timespan when we do finally report a change.
} else if (interval >= d_update_interval) {
// Figure out the new matrix based on the current values and
// the length of the interval since the last report
update_matrix_based_on_values(interval);
d_prevtime = now;
}
}
void vrpn_Tracker_MotionNode::send_report()
{
// Send the message on the connection
if (NULL != vrpn_Tracker::d_connection) {
char buffer[1024];
int len = encode_to(buffer);
if (vrpn_Tracker::d_connection->pack_message(len, timestamp, position_m_id, d_sender_id, buffer, vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr, "MotionNode: cannot write message: tossing\n");
}
}
}
void vrpn_Tracker_NDI_Polaris::send_report(void) // called from get_report()
{
if (d_connection)
{
char msgbuf[VRPN_MSGBUFSIZE];
int len = encode_to(msgbuf);
if (d_connection->pack_message(len, timestamp, position_m_id, d_sender_id, msgbuf,
vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr,"vrpn_Tracker_NDI_Polaris: cannot write message: tossing\n");
}
}
}
void vrpn_Tracker_PhaseSpace::send_report(void)
{
if(d_connection)
{
char msgbuf[VRPN_PHASESPACE_MSGBUFSIZE];
int len = encode_to(msgbuf);
if(d_connection->pack_message(len, timestamp, position_m_id, d_sender_id, msgbuf,
vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr,"PhaseSpace: cannot write message: tossing\n");
}
}
}
void vrpn_IMU_SimpleCombiner::mainloop()
{
// Call generic server mainloop, since we are a server
server_mainloop();
// Mainloop() all of the analogs that are defined and the button
// so that we will get all of the values fresh.
if (d_acceleration.ana != NULL) { d_acceleration.ana->mainloop(); }
if (d_rotational_vel.ana != NULL) { d_rotational_vel.ana->mainloop(); }
if (d_magnetometer.ana != NULL) { d_magnetometer.ana->mainloop(); }
// Update the matrix based on the change in time since the last
// update and the current values.
struct timeval now;
vrpn_gettimeofday(&now, NULL);
double delta_t = vrpn_TimevalDurationSeconds(now, d_prev_update_time);
update_matrix_based_on_values(delta_t);
d_prev_update_time = now;
// See if it has been long enough since our last report.
// If so, generate a new one.
double interval = vrpn_TimevalDurationSeconds(now, d_prevtime);
if (interval >= d_update_interval) {
// pack and deliver tracker report with info from the current matrix;
if (d_connection) {
char msgbuf[1000];
int len = encode_to(msgbuf);
if (d_connection->pack_message(len, vrpn_Tracker::timestamp,
position_m_id, d_sender_id, msgbuf,
vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr, "vrpn_IMU_SimpleCombiner: "
"cannot write pose message: tossing\n");
}
len = encode_vel_to(msgbuf);
if (d_connection->pack_message(len, vrpn_Tracker::timestamp,
velocity_m_id, d_sender_id, msgbuf,
vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr, "vrpn_IMU_SimpleCombiner: "
"cannot write velocity message: tossing\n");
}
} else {
fprintf(stderr, "vrpn_IMU_SimpleCombiner: "
"No valid connection\n");
}
// We just sent a report, so reset the time
d_prevtime = now;
}
}
void vrpn_Tracker_InterSense::send_report(void)
{
#ifdef VRPN_INCLUDE_INTERSENSE
// Send the message on the connection
if (d_connection)
{
char msgbuf[1000];
int len = encode_to(msgbuf);
if (d_connection->pack_message(len, timestamp, position_m_id, d_sender_id, msgbuf,
vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr,"InterSense: cannot write message: tossing\n");
}
}
#endif
}
int vrpn_Poser_Remote::client_send_pose_relative()
{
char msgbuf[1000];
vrpn_int32 len;
// Pack pose delta.
len = encode_to(msgbuf);
if (d_connection->pack_message(len, p_timestamp,
req_position_relative_m_id, d_sender_id, msgbuf,
vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr, "vrpn_Poser_Remote: can't write a message: tossing\n");
return -1;
}
return 0;
}
// This will report the dials whether they have changed or not; this
// will result in some 0 dial values being sent. Normally, code will
// use the report_changes method to avoid flooding the system with
// un-needed messages.
// It then clears the dials back to 0.
void vrpn_Dial::report(void)
{
char msgbuf[1000];
vrpn_int32 i;
vrpn_int32 len;
if (d_connection) {
for (i = 0; i < num_dials; i++) {
len = encode_to(msgbuf, sizeof(msgbuf), i, dials[i]);
if (d_connection->pack_message(len, timestamp, change_m_id,
d_sender_id, msgbuf,
vrpn_CONNECTION_RELIABLE)) {
fprintf(stderr, "vrpn_Dial: can't write message: tossing\n");
}
dials[i] = 0; // We've reported it the change.
}
}
}
void reportState()
{
if (d_connection)
{
for (int i = 0; i < num_buttons; i++)
{
char msgbuf[1000];
vrpn_int32 len = encode_to(msgbuf,i, buttons[i]);
if (d_connection->pack_message(len, timestamp, change_message_id, d_sender_id, msgbuf, vrpn_CONNECTION_RELIABLE))
{
qCritical("myButtonServer: can't write message: tossing\n");
}
}
}
else
{
qWarning("No valid connection");
}
}
void vrpn_Tracker_WiimoteHead::report() {
// pack and deliver tracker report;
if (d_connection) {
char msgbuf[1000];
int len = encode_to(msgbuf);
if (d_connection->pack_message(len, vrpn_Tracker::timestamp,
position_m_id, d_sender_id, msgbuf,
vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr, "vrpn_Tracker_WiimoteHead: "
"cannot write message: tossing\n");
}
} else {
fprintf(stderr, "vrpn_Tracker_WiimoteHead: "
"No valid connection\n");
}
// We just sent a report, so reset the time
vrpn_gettimeofday(&d_prevtime, NULL);
d_updated = false;
}
void vrpn_Tracker_Crossbow::send_report(void) {
// Send the message on the connection
if (d_connection) {
char msgbuf[1000];
int len = encode_to(msgbuf);
if (d_connection->pack_message(len, timestamp, position_m_id, d_sender_id, msgbuf, vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr,"Tracker: cannot write message: tossing\n");
}
len = encode_acc_to(msgbuf);
if (d_connection->pack_message(len, timestamp, accel_m_id, d_sender_id, msgbuf, vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr,"Tracker: cannot write message: tossing\n");
}
len = encode_vel_to(msgbuf);
if (d_connection->pack_message(len, timestamp, velocity_m_id, d_sender_id, msgbuf, vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr,"Tracker: cannot write message: tossing\n");
}
} else {
fprintf(stderr,"Tracker: No valid connection\n");
}
}
void vrpn_Tracker_JoyFly::mainloop(void)
{
server_mainloop();
if (joy_remote != NULL)
joy_remote->mainloop();
if (status == vrpn_TRACKER_REPORT_READY) {
// pack and deliver tracker report;
fprintf(stderr, "Sending a report\n");
char msgbuf[1000];
vrpn_int32 len = encode_to(msgbuf);
if (d_connection->pack_message(len, timestamp,
position_m_id, d_sender_id, msgbuf,
vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr,
"\nvrpn_Tracker_Flock: cannot write message ... tossing");
} else {
fprintf(stderr,"\nvrpn_Tracker_Flock: No valid connection");
}
status = vrpn_TRACKER_SYNCING;
}
}
void vrpn_Tracker_AnalogFly::mainloop()
{
struct timeval now;
double interval; // How long since the last report, in secs
// Call generic server mainloop, since we are a server
server_mainloop();
// Mainloop() all of the analogs that are defined and the button
// so that we will get all of the values fresh.
if (d_x.ana != NULL) { d_x.ana->mainloop(); };
if (d_y.ana != NULL) { d_y.ana->mainloop(); };
if (d_z.ana != NULL) { d_z.ana->mainloop(); };
if (d_sx.ana != NULL) { d_sx.ana->mainloop(); };
if (d_sy.ana != NULL) { d_sy.ana->mainloop(); };
if (d_sz.ana != NULL) { d_sz.ana->mainloop(); };
if (d_reset_button != NULL) { d_reset_button->mainloop(); };
if (d_clutch_button != NULL) { d_clutch_button->mainloop(); };
// See if it has been long enough since our last report.
// If so, generate a new one.
vrpn_gettimeofday(&now, NULL);
interval = vrpn_TimevalDurationSeconds(now, d_prevtime);
if (shouldReport(interval)) {
// Set the time on the report to now, if not an absolute
// tracker. Absolute trackers have their time values set
// to match the time at which their analog devices gave the
// last report.
if (!d_absolute) {
vrpn_Tracker::timestamp = now;
}
// Figure out the new matrix based on the current values and
// the length of the interval since the last report
update_matrix_based_on_values(interval);
// pack and deliver tracker report;
if (d_connection) {
char msgbuf[1000];
int len = encode_to(msgbuf);
if (d_connection->pack_message(len, vrpn_Tracker::timestamp,
position_m_id, d_sender_id, msgbuf,
vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr,"Tracker AnalogFly: "
"cannot write message: tossing\n");
}
} else {
fprintf(stderr,"Tracker AnalogFly: "
"No valid connection\n");
}
// We just sent a report, so reset the time
d_prevtime = now;
}
// We're not always sending reports, but we still want to
// update the interval clock so that we don't integrate over
// too long a timespan when we do finally report a change.
if (interval >= d_update_interval) {
d_prevtime = now;
}
}
void vrpn_Tracker_GPS::mainloop()
{
//char temp[256];
fprintf(stderr,"calling server main\n");
// Call the generic server mainloop, since we are a server
server_mainloop();
//-eb adding get report and removing switch statement
//get_report();
fprintf(stderr,"status in mainloop is %d\n\n",status);
switch (status) {
case vrpn_TRACKER_REPORT_READY:
{
printf("tracker report ready\n",status);
gettimeofday(×tamp, NULL); // Set watchdog now
// Send the message on the connection
if (d_connection) {
char msgbuf[1000];
//sprintf(temp, "position id = %d, sender id = %d", position_m_id, d_sender_id);
//MessageBox(NULL, temp,"GPS Testing",0);
// Pack position report
int len = encode_to(msgbuf);
if (d_connection->pack_message(len, timestamp,
position_m_id, d_sender_id, msgbuf,
vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr,"Fastrak: cannot write message: tossing\n");
}
// Pack velocity report
// len = encode_vel_to(msgbuf);
// if (d_connection->pack_message(len, timestamp,
// velocity_m_id, d_sender_id, msgbuf,
// vrpn_CONNECTION_LOW_LATENCY)){
// fprintf(stderr,"Fastrak: cannot write message: tossing\n");
// }
} else {
fprintf(stderr,"Tracker Fastrak: No valid connection\n");
}
// Ready for another report
status = vrpn_TRACKER_SYNCING;
}
break;
case vrpn_TRACKER_SYNCING:
case vrpn_TRACKER_AWAITING_STATION:
case vrpn_TRACKER_PARTIAL:
{
// It turns out to be important to get the report before checking
// to see if it has been too long since the last report. This is
// because there is the possibility that some other device running
// in the same server may have taken a long time on its last pass
// through mainloop(). Trackers that are resetting do this. When
// this happens, you can get an infinite loop -- where one tracker
// resets and causes the other to timeout, and then it returns the
// favor. By checking for the report here, we reset the timestamp
// if there is a report ready (ie, if THIS device is still operating).
get_report();
//
// struct timeval current_time;
// gettimeofday(¤t_time, NULL);
// if ( duration(current_time,timestamp) > MAX_TIME_INTERVAL) {
// sprintf(errmsg,"Timeout... current_time=%ld:%ld, timestamp=%ld:%ld",current_time.tv_sec, current_time.tv_usec, timestamp.tv_sec, timestamp.tv_usec);
// FT_ERROR(errmsg);
// MessageBox(NULL,"Timeout","GPS Testing",0);
// status = vrpn_TRACKER_FAIL;
// }
//
}
break;
case vrpn_TRACKER_RESETTING:
reset();
break;
case vrpn_TRACKER_FAIL:
FT_WARNING("Tracking failed, trying to reset (try power cycle if more than 4 attempts made)");
//vrpn_close_commport(serial_fd);
//serial_fd = vrpn_open_commport(portname, baudrate);
status = vrpn_TRACKER_RESETTING;
break;
}//switch
}
