vrpn 07.35
Virtual Reality Peripheral Network
Loading...
Searching...
No Matches
vrpn_JoyFly.C
Go to the documentation of this file.
1
2
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{
17 int i;
18
19 joy_remote = new vrpn_Analog_Remote(source, sourceConnection);
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 }
36 }
37 initMatrix[0][0] = initMatrix[2][2] =
38 initMatrix[1][1] = initMatrix[3][3] = 1.0;
39 } else {
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");
43 fclose(fp);
44 return;
45 }
46 fprintf(stderr, "Chan[%d] = (%lf %d)\n", i, chanAccel[i], chanPower[i]);
47 }
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");
52 fclose(fp);
53 return;
54 }
55 }
56 }
57 fclose(fp);
58 }
59
60 q_matrix_copy(currentMatrix, initMatrix);
61}
62
64{
65 delete joy_remote;
66}
67
69{
71
72 if (joy_remote != NULL)
73 joy_remote->mainloop();
75 // pack and deliver tracker report;
76 fprintf(stderr, "Sending a report\n");
77
78 char msgbuf[1000];
79 vrpn_int32 len = encode_to(msgbuf);
83 fprintf(stderr,
84 "\nvrpn_Tracker_Flock: cannot write message ... tossing");
85 } else {
86 fprintf(stderr,"\nvrpn_Tracker_Flock: No valid connection");
87 }
89 }
90}
91
92
93#define ONE_SEC (1000000l)
94// static
96 (void * userdata, const vrpn_ANALOGCB b)
97{
98 double tx, ty, tz, rx, ry, rz;
99 double temp[7];
100 int i,j;
101 q_matrix_type newM;
102 double deltaT;
103
105
106 printf("Joy total = %d,Chan[0] = %lf\n",
107 b.num_channel,b.channel[0]);
108
109 if (pts->prevtime.tv_sec == -1) {
110 deltaT = 1.0 ; // one milisecond;
111 } else {
112 deltaT = (pts->prevtime.tv_sec* ONE_SEC + pts->prevtime.tv_usec) -
113 (b.msg_time.tv_sec *ONE_SEC + b.msg_time.tv_usec);
114 deltaT /= 1000.0 ; // convert to millsecond;
115 }
116
117 memcpy(&(pts->prevtime), &b.msg_time, sizeof(struct timeval));
118
119 for (i=0; i< 7; i++) {
120 temp[i] = 1.0;
121 //printf("chan[%d] = %lf\n", i, b.channel[i]);
122 if (!(b.channel[i] >=-0.5 && b.channel[i] <= 0.5)) return;
123 for (j=0; j< pts->chanPower[i]; j++)
124 temp[i] *= b.channel[i];
125 if (b.channel[i] > 0) {
126 temp[i] *= pts->chanAccel[i];
127 } else {
128 temp[i] = -fabs(temp[i]) * pts->chanAccel[i];
129 }
130 temp[i] *= deltaT;
131 }
132
133 /* roll chan[3] */
134 rz = temp[3];
135
136 /* pitch Chan[4]*/
137 rx = temp[4];
138
139 /* yaw Chan[5]*/
140 ry = temp[5];
141
142
143 /* translation, x chan[0], y: chane[2], z: chan[1] */
144 tx = -temp[0]; // tx is NEGTIVE of power !!! ;
145 ty = temp[2];
146 tz = temp[1];
147
148 q_euler_to_col_matrix(newM, rz, ry, rx);
149 newM[3][0] = tx; newM[3][1] = ty; newM[3][2] = tz;
150 pts->update(newM);
151}
152
153void vrpn_Tracker_JoyFly::update(q_matrix_type & newM) {
154
155 q_matrix_type final;
156 q_xyz_quat_type xq;
157 int i;
158
159 q_matrix_mult(final, newM, currentMatrix);
160 q_matrix_copy(currentMatrix, final);
161 q_row_matrix_to_xyz_quat( & xq, currentMatrix);
162
163
165 for (i=0; i< 3; i++) {
166 pos[i] = xq.xyz[i]; // position;
167 }
168 printf("(x, y, z)= %lf %lf %lf\n", pos[0],pos[1], pos[2]);
169 for (i=0; i< 4; i++) {
170 d_quat[i] = xq.quat[i]; // orientation.
171 }
172}
173
174
175// static
177 (void * userdata, vrpn_HANDLERPARAM) {
178
179 printf("Get a new connection, reset virtual_Tracker\n");
180 ((vrpn_Tracker_JoyFly *) userdata)->reset();
181 return 0;
182}
183
185 q_matrix_copy(currentMatrix, initMatrix);
186 prevtime.tv_sec = -1;
187 prevtime.tv_usec = -1;
188}
189
190
191
192
193
194
195
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
Definition: vrpn_Analog.C:327
virtual int register_change_handler(void *userdata, vrpn_ANALOGCHANGEHANDLER handler)
Definition: vrpn_Analog.h:192
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...
Generic connection class not specific to the transport mechanism.
virtual vrpn_int32 register_message_type(const char *name)
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...
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.
Definition: vrpn_JoyFly.C:12
virtual ~vrpn_Tracker_JoyFly(void)
Definition: vrpn_JoyFly.C:63
virtual void reset(void)
Definition: vrpn_JoyFly.C:184
virtual void mainloop(void)
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
Definition: vrpn_JoyFly.C:68
static void VRPN_CALLBACK handle_joystick(void *, const vrpn_ANALOGCB)
Definition: vrpn_JoyFly.C:96
void update(q_matrix_type &)
Definition: vrpn_JoyFly.C:153
static int VRPN_CALLBACK handle_newConnection(void *, vrpn_HANDLERPARAM)
Definition: vrpn_JoyFly.C:177
virtual int encode_to(char *buf)
Definition: vrpn_Tracker.C:552
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
struct timeval timestamp
Definition: vrpn_Tracker.h:100
vrpn_int32 position_m_id
Definition: vrpn_Tracker.h:80
vrpn_int32 num_channel
Definition: vrpn_Analog.h:170
struct timeval msg_time
Definition: vrpn_Analog.h:169
vrpn_float64 channel[vrpn_CHANNEL_MAX]
Definition: vrpn_Analog.h:171
This structure is what is passed to a vrpn_Connection message callback.
const char * vrpn_got_connection
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
#define ONE_SEC
Definition: vrpn_JoyFly.C:93
const int vrpn_TRACKER_SYNCING
Definition: vrpn_Tracker.h:35
const int vrpn_TRACKER_REPORT_READY
Definition: vrpn_Tracker.h:37