vrpn  07.33
Virtual Reality Peripheral Network
vrpn_JoyFly.C
Go to the documentation of this file.
1 
3 #include <math.h> // for fabs
4 #include <string.h> // for memcpy
5 
6 #include "quat.h" // for q_matrix_copy, etc
7 #include "vrpn_Connection.h" // for vrpn_Connection, etc
8 #include "vrpn_JoyFly.h"
9 #include "vrpn_Types.h" // for vrpn_float64, vrpn_int32
10 
12  (const char * name, vrpn_Connection * c,
13  const char * source, const char * set_config,
14  vrpn_Connection * sourceConnection) :
15  vrpn_Tracker (name, c) {
16  int i;
17 
18 
19  joy_remote = new vrpn_Analog_Remote (source, sourceConnection);
20  joy_remote->register_change_handler(this, handle_joystick);
22  handle_newConnection, this);
23 
24  FILE * fp = fopen(set_config, "r");
25  if (fp == NULL) {
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++) {
29  chanAccel[i] = 1.0;
30  chanPower[i] = 1;
31  }
32  for ( i =0; i< 4; i++)
33  for (int j=0; j< 4; j++)
34  initMatrix[i][j] = 0;
35  initMatrix[0][0] = initMatrix[2][2] =
36  initMatrix[1][1] = initMatrix[3][3] = 1.0;
37  } else {
38  for (i=0; i< 7; i++) {
39  if (fscanf(fp, "%lf %d", &chanAccel[i], &chanPower[i]) != 2) {
40  fprintf(stderr,"Cannot read acceleration and power from file\n");
41  return;
42  }
43  fprintf(stderr, "Chan[%d] = (%lf %d)\n", i, chanAccel[i], chanPower[i]);
44  }
45  for (i =0; i< 4; i++)
46  for (int j=0; j< 4; j++) {
47  if (fscanf(fp, "%lf", &initMatrix[i][j]) < 0) {
48  perror("vrpn_Tracker_JoyFly::vrpn_Tracker_JoyFly(): Could not read matrix value");
49  return;
50  }
51  }
52  fclose(fp);
53  }
54 
55  q_matrix_copy(currentMatrix, initMatrix);
56 
57 }
58 
60 {
61  delete joy_remote;
62 }
63 
65 {
67 
68  if (joy_remote != NULL)
69  joy_remote->mainloop();
71  // pack and deliver tracker report;
72  fprintf(stderr, "Sending a report\n");
73 
74  char msgbuf[1000];
75  vrpn_int32 len = encode_to(msgbuf);
77  position_m_id, d_sender_id, msgbuf,
79  fprintf(stderr,
80  "\nvrpn_Tracker_Flock: cannot write message ... tossing");
81  } else {
82  fprintf(stderr,"\nvrpn_Tracker_Flock: No valid connection");
83  }
85  }
86 }
87 
88 
89 #define ONE_SEC (1000000l)
90 // static
92  (void * userdata, const vrpn_ANALOGCB b)
93 {
94  double tx, ty, tz, rx, ry, rz;
95  double temp[7];
96  int i,j;
97  q_matrix_type newM;
98  double deltaT;
99 
101 
102  printf("Joy total = %d,Chan[0] = %lf\n",
103  b.num_channel,b.channel[0]);
104 
105  if (pts->prevtime.tv_sec == -1) {
106  deltaT = 1.0 ; // one milisecond;
107  } else {
108  deltaT = (pts->prevtime.tv_sec* ONE_SEC + pts->prevtime.tv_usec) -
109  (b.msg_time.tv_sec *ONE_SEC + b.msg_time.tv_usec);
110  deltaT /= 1000.0 ; // convert to millsecond;
111  }
112 
113  memcpy(&(pts->prevtime), &b.msg_time, sizeof(struct timeval));
114 
115  for (i=0; i< 7; i++) {
116  temp[i] = 1.0;
117  //printf("chan[%d] = %lf\n", i, b.channel[i]);
118  if (!(b.channel[i] >=-0.5 && b.channel[i] <= 0.5)) return;
119  for (j=0; j< pts->chanPower[i]; j++)
120  temp[i] *= b.channel[i];
121  if (b.channel[i] > 0) {
122  temp[i] *= pts->chanAccel[i];
123  } else {
124  temp[i] = -fabs(temp[i]) * pts->chanAccel[i];
125  }
126  temp[i] *= deltaT;
127  }
128 
129  /* roll chan[3] */
130  rz = temp[3];
131 
132  /* pitch Chan[4]*/
133  rx = temp[4];
134 
135  /* yaw Chan[5]*/
136  ry = temp[5];
137 
138 
139  /* translation, x chan[0], y: chane[2], z: chan[1] */
140  tx = -temp[0]; // tx is NEGTIVE of power !!! ;
141  ty = temp[2];
142  tz = temp[1];
143 
144  q_euler_to_col_matrix(newM, rz, ry, rx);
145  newM[3][0] = tx; newM[3][1] = ty; newM[3][2] = tz;
146  pts->update(newM);
147 }
148 
149 void vrpn_Tracker_JoyFly::update(q_matrix_type & newM) {
150 
151  q_matrix_type final;
152  q_xyz_quat_type xq;
153  int i;
154 
155  q_matrix_mult(final, newM, currentMatrix);
156  q_matrix_copy(currentMatrix, final);
157  q_row_matrix_to_xyz_quat( & xq, currentMatrix);
158 
159 
161  for (i=0; i< 3; i++) {
162  pos[i] = xq.xyz[i]; // position;
163  }
164  printf("(x, y, z)= %lf %lf %lf\n", pos[0],pos[1], pos[2]);
165  for (i=0; i< 4; i++) {
166  d_quat[i] = xq.quat[i]; // orientation.
167  }
168 }
169 
170 
171 // static
173  (void * userdata, vrpn_HANDLERPARAM) {
174 
175  printf("Get a new connection, reset virtual_Tracker\n");
176  ((vrpn_Tracker_JoyFly *) userdata)->reset();
177  return 0;
178 }
179 
181  q_matrix_copy(currentMatrix, initMatrix);
182  prevtime.tv_sec = -1;
183  prevtime.tv_usec = -1;
184 }
185 
186 
187 
188 
189 
190 
191 
vrpn_Connection::pack_message
virtual int pack_message(vrpn_uint32 len, struct timeval time, vrpn_int32 type, vrpn_int32 sender, const char *buffer, vrpn_uint32 class_of_service)
Pack a message that will be sent the next time mainloop() is called. Turn off the RELIABLE flag if yo...
Definition: vrpn_Connection.C:4632
vrpn_Tracker
Definition: vrpn_Tracker.h:49
vrpn_got_connection
const char * vrpn_got_connection
Definition: vrpn_Connection.C:185
vrpn_Tracker::encode_to
virtual int encode_to(char *buf)
Definition: vrpn_Tracker.C:533
vrpn_Tracker_JoyFly::mainloop
virtual void mainloop(void)
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
Definition: vrpn_JoyFly.C:64
vrpn_Types.h
vrpn_Tracker_JoyFly::reset
virtual void reset(void)
Definition: vrpn_JoyFly.C:180
vrpn_Tracker_JoyFly::update
void update(q_matrix_type &)
Definition: vrpn_JoyFly.C:149
vrpn_Analog_Remote
Definition: vrpn_Analog.h:181
vrpn_TRACKER_REPORT_READY
const int vrpn_TRACKER_REPORT_READY
Definition: vrpn_Tracker.h:37
vrpn_BaseClassUnique::userdata
void * userdata
Definition: vrpn_BaseClass.h:287
vrpn_Tracker::d_quat
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
vrpn_Tracker::timestamp
struct timeval timestamp
Definition: vrpn_Tracker.h:100
vrpn_CONNECTION_LOW_LATENCY
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
Definition: vrpn_Connection.h:122
vrpn_BaseClassUnique::d_connection
vrpn_Connection * d_connection
Connection that this object talks to.
Definition: vrpn_BaseClass.h:224
vrpn_Connection::register_message_type
virtual vrpn_int32 register_message_type(const char *name)
Definition: vrpn_Connection.C:5074
vrpn_Tracker_JoyFly::vrpn_Tracker_JoyFly
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.
Definition: vrpn_JoyFly.C:12
vrpn_ANALOGCB
Definition: vrpn_Analog.h:168
vrpn_HANDLERPARAM
This structure is what is passed to a vrpn_Connection message callback.
Definition: vrpn_Connection.h:44
vrpn_Tracker_JoyFly::handle_newConnection
static int VRPN_CALLBACK handle_newConnection(void *, vrpn_HANDLERPARAM)
Definition: vrpn_JoyFly.C:173
vrpn_BaseClassUnique::d_sender_id
vrpn_int32 d_sender_id
Sender ID registered with the connection.
Definition: vrpn_BaseClass.h:228
vrpn_Tracker_JoyFly::~vrpn_Tracker_JoyFly
virtual ~vrpn_Tracker_JoyFly(void)
Definition: vrpn_JoyFly.C:59
vrpn_Tracker_JoyFly::handle_joystick
static void VRPN_CALLBACK handle_joystick(void *, const vrpn_ANALOGCB)
Definition: vrpn_JoyFly.C:92
vrpn_ANALOGCB::msg_time
struct timeval msg_time
Definition: vrpn_Analog.h:169
vrpn_ANALOGCB::channel
vrpn_float64 channel[vrpn_CHANNEL_MAX]
Definition: vrpn_Analog.h:171
vrpn_Analog_Remote::mainloop
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
Definition: vrpn_Analog.C:328
vrpn_ANALOGCB::num_channel
vrpn_int32 num_channel
Definition: vrpn_Analog.h:170
vrpn_Tracker::position_m_id
vrpn_int32 position_m_id
Definition: vrpn_Tracker.h:80
vrpn_Connection
Generic connection class not specific to the transport mechanism.
Definition: vrpn_Connection.h:510
vrpn_Connection.h
vrpn_Tracker::status
int status
Definition: vrpn_Tracker.h:129
vrpn_TRACKER_SYNCING
const int vrpn_TRACKER_SYNCING
Definition: vrpn_Tracker.h:35
vrpn_JoyFly.h
vrpn_Tracker::pos
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
vrpn_Connection::register_handler
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...
Definition: vrpn_Connection.C:5199
vrpn_Tracker_JoyFly
Definition: vrpn_JoyFly.h:17
vrpn_Analog_Remote::register_change_handler
virtual int register_change_handler(void *userdata, vrpn_ANALOGCHANGEHANDLER handler)
Definition: vrpn_Analog.h:192
vrpn_BaseClassUnique::server_mainloop
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
Definition: vrpn_BaseClass.C:603
ONE_SEC
#define ONE_SEC
Definition: vrpn_JoyFly.C:89