13 const char * source,
const char * set_config,
24 FILE * fp = fopen(set_config,
"r");
26 fprintf(stderr,
"Can't open joy config file, using default..\n");
27 fprintf(stderr,
"Channel Accel 1.0, Power 1, Init Identity Matrix. \n");
28 for (i=0; i< 7; i++) {
32 for ( i =0; i< 4; i++) {
33 for (
int j=0; j< 4; j++) {
37 initMatrix[0][0] = initMatrix[2][2] =
38 initMatrix[1][1] = initMatrix[3][3] = 1.0;
40 for (i=0; i< 7; i++) {
41 if (fscanf(fp,
"%lf %d", &chanAccel[i], &chanPower[i]) != 2) {
42 fprintf(stderr,
"Cannot read acceleration and power from file\n");
46 fprintf(stderr,
"Chan[%d] = (%lf %d)\n", i, chanAccel[i], chanPower[i]);
48 for (i =0; i< 4; i++) {
49 for (
int j=0; j< 4; j++) {
50 if (fscanf(fp,
"%lf", &initMatrix[i][j]) < 0) {
51 perror(
"vrpn_Tracker_JoyFly::vrpn_Tracker_JoyFly(): Could not read matrix value");
60 q_matrix_copy(currentMatrix, initMatrix);
72 if (joy_remote != NULL)
73 joy_remote->mainloop();
76 fprintf(stderr,
"Sending a report\n");
84 "\nvrpn_Tracker_Flock: cannot write message ... tossing");
86 fprintf(stderr,
"\nvrpn_Tracker_Flock: No valid connection");
93#define ONE_SEC (1000000l)
98 double tx, ty, tz, rx, ry, rz;
106 printf(
"Joy total = %d,Chan[0] = %lf\n",
109 if (pts->prevtime.tv_sec == -1) {
112 deltaT = (pts->prevtime.tv_sec*
ONE_SEC + pts->prevtime.tv_usec) -
117 memcpy(&(pts->prevtime), &b.
msg_time,
sizeof(
struct timeval));
119 for (i=0; i< 7; i++) {
123 for (j=0; j< pts->chanPower[i]; j++)
126 temp[i] *= pts->chanAccel[i];
128 temp[i] = -fabs(temp[i]) * pts->chanAccel[i];
148 q_euler_to_col_matrix(newM, rz, ry, rx);
149 newM[3][0] = tx; newM[3][1] = ty; newM[3][2] = tz;
159 q_matrix_mult(
final, newM, currentMatrix);
160 q_matrix_copy(currentMatrix,
final);
161 q_row_matrix_to_xyz_quat( & xq, currentMatrix);
165 for (i=0; i< 3; i++) {
168 printf(
"(x, y, z)= %lf %lf %lf\n",
pos[0],
pos[1],
pos[2]);
169 for (i=0; i< 4; i++) {
179 printf(
"Get a new connection, reset virtual_Tracker\n");
185 q_matrix_copy(currentMatrix, initMatrix);
186 prevtime.tv_sec = -1;
187 prevtime.tv_usec = -1;
vrpn_Connection * d_connection
Connection that this object talks to.
vrpn_int32 d_sender_id
Sender ID registered with the connection.
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
virtual vrpn_int32 register_message_type(const char *name)
virtual int register_handler(vrpn_int32 type, vrpn_MESSAGEHANDLER handler, void *userdata, vrpn_int32 sender=vrpn_ANY_SENDER)
Set up (or remove) a handler for a message of a given type. Optionally, specify which sender to handl...
vrpn_Tracker_JoyFly(const char *name, vrpn_Connection *c, const char *source, const char *config_file_name, vrpn_Connection *sourceConnection=NULL)
This object has been superceded by the vrpn_Tracker_AnalogFly object.
virtual ~vrpn_Tracker_JoyFly(void)
virtual void mainloop(void)
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
static void VRPN_CALLBACK handle_joystick(void *, const vrpn_ANALOGCB)
void update(q_matrix_type &)
static int VRPN_CALLBACK handle_newConnection(void *, vrpn_HANDLERPARAM)
virtual int encode_to(char *buf)
vrpn_Tracker(const char *name, vrpn_Connection *c=NULL, const char *tracker_cfg_file_name=NULL)
vrpn_float64 channel[vrpn_CHANNEL_MAX]
This structure is what is passed to a vrpn_Connection message callback.
const char * vrpn_got_connection
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
class VRPN_API vrpn_Connection
This object has been superceded by the vrpn_Tracker_AnalogFly object.
const int vrpn_TRACKER_SYNCING
const int vrpn_TRACKER_REPORT_READY