vrpn 07.35
Virtual Reality Peripheral Network
Loading...
Searching...
No Matches
vrpn_Tracker_NovintFalcon.C
Go to the documentation of this file.
1// -*- c++ -*-
2// This file provides an interface to a Novint Falcon.
3// http://home.novint.com/products/novint_falcon.php
4// It uses libnifalcon to communicate with the device.
5// http://libnifalcon.nonpolynomial.org/
6//
7// File: vrpn_Tracker_NovintFalcon.C
8// Author: Axel Kohlmeyer akohlmey@gmail.com
9// Date: 2010-08-12
10// Copyright: (C) 2010 Axel Kohlmeyer
11// License: Boost Software License 1.0
12// depends: libnifalcon-1.0.1+, libusb-1.0, boost 1.39, VRPN 07_27
13// tested on: Linux x86_64 w/ gcc 4.4.1
14
16
17#ifdef VRPN_USE_LIBNIFALCON
18#include <vector>
19#include <array>
20#include <memory>
21#include "falcon/core/FalconDevice.h"
22#include "falcon/firmware/FalconFirmwareNovintSDK.h"
23#include "falcon/grip/FalconGripFourButton.h"
24#include "falcon/kinematic/FalconKinematicStamper.h"
25#include "falcon/util/FalconFirmwareBinaryNvent.h"
26
27/**************************************************************************/
28// number of retries for the I/O loop.
29#define FALCON_NUM_RETRIES 10
30
31// define to activate additional messages about
32// what the driver is currently trying to do.
33#undef VERBOSE
34
35// define for detailed status tracking. very verbose.
36#undef VERBOSE2
37/**************************************************************************/
38
39// save us some typing
40typedef std::array<double, 3> d_vector;
41
43static d_vector operator+(const d_vector &a,const d_vector &b)
44{
45 d_vector ret;
46 ret[0] = a[0] + b[0];
47 ret[1] = a[1] + b[1];
48 ret[2] = a[2] + b[2];
49 return ret;
50}
51
53static d_vector operator-(const d_vector &a,const d_vector &b)
54{
55 d_vector ret;
56 ret[0] = a[0] + b[0];
57 ret[1] = a[1] + b[1];
58 ret[2] = a[2] + b[2];
59 return ret;
60}
61
63static double d_length(const d_vector &a)
64{
65 double ret;
66 ret = a[0] * a[0];
67 ret += a[1] * a[1];
68 ret += a[2] * a[2];
69 return sqrt(ret);
70}
71
72/*************************************************************************/
73// compute time difference in microseconds.
74static double timediff(struct timeval t1, struct timeval t2) {
75 return (t1.tv_usec - t2.tv_usec)*1.0 + 1000000.0 * (t1.tv_sec - t2.tv_sec);
76}
77
78/*************************************************************************/
79
80class vrpn_NovintFalcon_Device
81{
82public:
83 vrpn_NovintFalcon_Device(int flags)
84 : m_flags(flags)
85 {
86 if (m_flags < 0) {
87 m_falconDevice = NULL;
88 return;
89 } else {
90 m_falconDevice = new libnifalcon::FalconDevice;
91 m_falconDevice->setFalconFirmware<libnifalcon::FalconFirmwareNovintSDK>();
92 }
93
94 if (m_flags & KINE_STAMPER) {
95 m_falconDevice->setFalconKinematic<libnifalcon::FalconKinematicStamper>();
96 } else {
97 try {
98 delete m_falconDevice;
99 } catch (...) {
100 fprintf(stderr, "vrpn_NovintFalcon_Device::vrpn_NovintFalcon_Device(): delete failed\n");
101 return;
102 }
103 m_falconDevice=NULL;
104 return;
105 }
106
107 if (m_flags & GRIP_FOURBUTTON) {
108 m_falconDevice->setFalconGrip<libnifalcon::FalconGripFourButton>();
109 } else {
110 try {
111 delete m_falconDevice;
112 } catch (...) {
113 fprintf(stderr, "vrpn_NovintFalcon_Device::vrpn_NovintFalcon_Device(): delete failed\n");
114 return;
115 }
116 m_falconDevice=NULL;
117 return;
118 }
119 }
120
121 ~vrpn_NovintFalcon_Device() {
122#ifdef VERBOSE
123 fprintf(stderr, "Closing Falcon device %d.\n", m_flags & MASK_DEVICEIDX);
124#endif
125 if (m_falconDevice) {
126 std::shared_ptr<libnifalcon::FalconFirmware> f;
127 f=m_falconDevice->getFalconFirmware();
128 if(f) {
129 f->setLEDStatus(libnifalcon::FalconFirmware::RED_LED |
130 libnifalcon::FalconFirmware::BLUE_LED |
131 libnifalcon::FalconFirmware::GREEN_LED);
132 for (int i=0; !m_falconDevice->runIOLoop() && i < FALCON_NUM_RETRIES; ++i) continue;
133 }
134 m_falconDevice->close();
135 }
136 try {
137 delete m_falconDevice;
138 } catch (...) {
139 fprintf(stderr, "vrpn_NovintFalcon_Device::~vrpn_NovintFalcon_Device(): delete failed\n");
140 return;
141 }
142 m_flags=-1;
143 };
144
145public:
146 // open device, load firmware and calibrate.
147 bool init() {
148 if (!m_falconDevice)
149 return false;
150
151 unsigned int count;
152 m_falconDevice->getDeviceCount(count);
153 int devidx = m_flags & MASK_DEVICEIDX;
154
155#ifdef VERBOSE
156 fprintf(stderr, "Trying to open Falcon device %d/%d.\n", devidx, count);
157#endif
158 if (devidx < count) {
159 if (!m_falconDevice->open(devidx)) {
160 fprintf(stderr, "Cannot open falcon device %d - Lib Error Code: %d - Device Error Code: %d\n",
161 devidx, m_falconDevice->getErrorCode(), m_falconDevice->getFalconComm()->getDeviceErrorCode());
162 return false;
163 }
164 } else {
165 fprintf(stderr, "Trying to open non-existing Novint Falcon device %d\n", devidx);
166 return false;
167 }
168
169 if (!m_falconDevice->isFirmwareLoaded()) {
170#ifdef VERBOSE
171 fprintf(stderr, "Loading Falcon Firmware\n");
172#endif
173 int i;
174 // 10 chances to load the firmware.
175 for (i=0; i<FALCON_NUM_RETRIES; ++i) {
176 if(!m_falconDevice->getFalconFirmware()->loadFirmware(true, libnifalcon::NOVINT_FALCON_NVENT_FIRMWARE_SIZE, const_cast<uint8_t*>(libnifalcon::NOVINT_FALCON_NVENT_FIRMWARE)))
177 {
178 fprintf(stderr, "Firmware loading attempt %d failed.\n", i);
179 if(i==FALCON_NUM_RETRIES-1){
180 fprintf(stderr, "Cannot load falcon device %d firmware - Device Error Code: %d - Comm Lib Error Code: %d\n",
181 devidx, m_falconDevice->getErrorCode(), m_falconDevice->getFalconComm()->getDeviceErrorCode());
182 return false;
183 }
184 } else {
185#ifdef VERBOSE
186 fprintf(stderr, "Falcon firmware successfully loaded.\n");
187#endif
188 break;
189 }
190 }
191 } else {
192#ifdef VERBOSE
193
194 fprintf(stderr, "Falcon Firmware already loaded.\n");
195#endif
196 }
197
198 int i;
199 bool message = false;
200 std::shared_ptr<libnifalcon::FalconFirmware> f;
201 f=m_falconDevice->getFalconFirmware();
202 for (i=0; !m_falconDevice->runIOLoop() && i < FALCON_NUM_RETRIES; ++i) continue;
203 while(1) { // XXX: add timeout to declare device dead after a while.
204 f->setHomingMode(true);
205 for (i=0; !m_falconDevice->runIOLoop() && i < FALCON_NUM_RETRIES; ++i) continue;
206 if(!f->isHomed()) {
207 f->setLEDStatus(libnifalcon::FalconFirmware::RED_LED);
208 for (i=0; !m_falconDevice->runIOLoop() && i < FALCON_NUM_RETRIES; ++i) continue;
209 if (!message) {
210 fprintf(stderr, "Falcon not currently calibrated. Move control all the way out then push straight all the way in.\n");
211 message = true;
212 }
213 } else {
214 f->setLEDStatus(libnifalcon::FalconFirmware::BLUE_LED);
215 for (i=0; !m_falconDevice->runIOLoop() && i < FALCON_NUM_RETRIES; ++i) continue;
216#ifdef VERBOSE
217 fprintf(stderr, "Falcon calibrated successfully.\n");
218#endif
219 break;
220 }
221 }
222
223 message = false;
224 while(1) { // XXX: add timeout to declare device dead after a while.
225 int i;
226
227 for (i=0; !m_falconDevice->runIOLoop() && i < FALCON_NUM_RETRIES; ++i) continue;
228 d_vector pos = m_falconDevice->getPosition();
229 m_oldpos = pos;
230 vrpn_gettimeofday(&m_oldtime, NULL);
231
232 if (!message) {
233 fprintf(stderr, "Move control all the way out until led turns green to activate device.\n");
234 message = true;
235 }
236 if (pos[2] > 0.170) { // XXX: value taken from libnifalcon test example
237 f->setLEDStatus(libnifalcon::FalconFirmware::GREEN_LED);
238 for (i=0; !m_falconDevice->runIOLoop() && i < FALCON_NUM_RETRIES; ++i) continue;
239#ifdef VERBOSE
240 fprintf(stderr, "Falcon activated successfully.\n");
241#endif
242 break;
243 }
244 }
245 return true;
246 };
247
248 // query status of device.
249 bool get_status(vrpn_float64 *pos, vrpn_float64 *vel,
250 vrpn_float64 *quat, vrpn_float64 *vel_quat,
251 vrpn_float64 *vel_dt, unsigned char *buttons) {
252 if (!m_falconDevice)
253 return false;
254
255 int i;
256 for (i=0; !m_falconDevice->runIOLoop() && i < FALCON_NUM_RETRIES; ++i) continue;
257 if ( i == FALCON_NUM_RETRIES )
258 return false;
259
260 // we have no orientation of the effector.
261 // so we just pick the identity quaternion.
262 if (quat) {
263 quat[0] = 0.0;
264 quat[1] = 0.0;
265 quat[2] = 0.0;
266 quat[3] = 1.0;
267 }
268
269 if (vel_quat) {
270 vel_quat[0] = 0.0;
271 vel_quat[1] = 0.0;
272 vel_quat[2] = 0.0;
273 vel_quat[3] = 0.0;
274 }
275
276 // update position information
277 d_vector my_pos = m_falconDevice->getPosition();
278 const double convert_pos = 1.0; // empirical. need to measure properly.
279 pos[0] = convert_pos * my_pos[0];
280 pos[1] = convert_pos * my_pos[1];
281 pos[2] = convert_pos * (my_pos[2]-0.125); // apply offset to make z axis data centered.
282#ifdef VERBOSE2
283 fprintf(stderr, "position %5.3f %5.3f %5.3f\n", pos[0],pos[1],pos[2]);
284#endif
285 if (vel) {
286 struct timeval current_time;
287 vrpn_gettimeofday(&current_time, NULL);
288 double deltat = timediff(current_time, m_oldtime);
289 *vel_dt= deltat;
290 if (deltat > 0) {
291 vel[0] = convert_pos * (my_pos[0] - m_oldpos[0]) / deltat;
292 vel[1] = convert_pos * (my_pos[1] - m_oldpos[1]) / deltat;
293 vel[2] = convert_pos * (my_pos[2] - m_oldpos[2]) / deltat;
294 } else {
295 vel[0] = 0.0;
296 vel[1] = 0.0;
297 vel[2] = 0.0;
298 }
299#ifdef VERBOSE2
300 fprintf(stderr, "velocity %5.3f %5.3f %5.3f\n", vel[0],vel[1],vel[2]);
301#endif
302 m_oldpos = my_pos;
303 m_oldtime.tv_sec = current_time.tv_sec;
304 m_oldtime.tv_usec = current_time.tv_usec;
305 }
306
307 // update button information
308 unsigned int my_buttons = m_falconDevice->getFalconGrip()->getDigitalInputs();
309 if (m_flags & GRIP_FOURBUTTON) {
310 buttons[0] = (my_buttons & libnifalcon::FalconGripFourButton::CENTER_BUTTON) ? 1 : 0;
311 buttons[1] = (my_buttons & libnifalcon::FalconGripFourButton::PLUS_BUTTON) ? 1 : 0;
312 buttons[2] = (my_buttons & libnifalcon::FalconGripFourButton::MINUS_BUTTON) ? 1 : 0;
313 buttons[3] = (my_buttons & libnifalcon::FalconGripFourButton::FORWARD_BUTTON) ? 1 : 0;
314 }
315 return true;
316 };
317
318 // set feedback force
319 bool set_force(const d_vector &force) {
320 if (!m_falconDevice)
321 return false;
322
323 // update position information
324 m_falconDevice->setForce(force);
325 if(!m_falconDevice->runIOLoop())
326 return false;
327
328 return true;
329 };
330
331protected:
332 int m_flags;
333 libnifalcon::FalconDevice *m_falconDevice;
334 d_vector m_oldpos;
335 struct timeval m_oldtime;
336
337private:
339 vrpn_NovintFalcon_Device() {};
341 vrpn_NovintFalcon_Device(const vrpn_NovintFalcon_Device &dev) {};
342
343public:
345 enum falconflags {
346 NONE = 0x0000, //< empty
347 MASK_DEVICEIDX = 0x000f, //< max. 15 falcons
348 KINE_STAMPER = 0x0010, //< stamper kinematics model
349 GRIP_FOURBUTTON = 0x0100 //< 4-button grip (the default)
350 };
351};
352
354class ForceFieldEffect
355{
356public:
358 ForceFieldEffect() : m_active(false), m_time(0), m_cutoff(0.0), m_damping(0.9)
359 {
360 int i,j;
361 for (i=0; i < 3; i++) {
362 m_origin[i] = 0.0;
363 m_addforce[i] = 0.0;
364 for (j=0; j < 3; j++) {
365 m_jacobian[i][j] = 0.0;
366 }
367 }
368 };
369
371 ~ForceFieldEffect() {}
372
373public:
375 void setForce(vrpn_float32 ori[3], vrpn_float32 frc[3], vrpn_float32 jac[3][3], vrpn_float32 rad) {
376 int i,j;
377 for (i=0; i < 3; i++) {
378 m_neworig[i] = ori[i];
379 m_newadd[i] = frc[i];
380 for (j=0; j < 3; j++) {
381 m_newjacob[i][j] = jac[i][j];
382 }
383 }
384 m_newcut = rad;
385 }
386
388 void setDamping(double damp) {
389 m_damping = damp;
390 }
391
393 virtual bool start() {
394 m_active = true;
395 m_time = 0.0;
396 // this will delay the effect until it
397 // is (re-)initialized with setForce()
398 m_cutoff = 0.0;
399 return m_active;
400 }
401
403 virtual void stop() {
404 m_active = false;
405 }
406
408 virtual bool isActive() const { return m_active; }
409
411 d_vector calcForce(const d_vector &pos) {
412 d_vector force, offset;
413 force.fill(0.0);
414 if (m_active) {
415 // apply damping to effect values
416 const double mix = 1.0 - m_damping;
417 int i,j;
418 for (i=0; i < 3; i++) {
419 m_origin[i] = m_damping*m_origin[i] + mix*m_neworig[i];
420 m_addforce[i] = m_damping*m_addforce[i] + mix*m_newadd[i];
421 for (j=0; j < 3; j++) {
422 m_jacobian[i][j] = m_damping*m_jacobian[i][j] + mix*m_newjacob[i][j];
423 }
424 }
425 m_cutoff = m_damping*m_cutoff + mix*m_newcut;
426
427 offset = pos - m_origin;
428 // no force too far away.
429 if (d_length(offset) > m_cutoff) {
430 return force;
431 }
432 // Compute the force, which is the constant force plus
433 // a force that varies as the effector position deviates
434 // from the origin of the force field. The Jacobian
435 // determines how the force changes in different directions
436 // away from the origin (generalizing spring forces of different
437 // magnitudes that constrain the Phantom to the point of the
438 // origin, to a line containing the origin, or to a plane
439 // containing the origin).
440 force = m_addforce;
441 for (i=0; i<3; ++i) {
442 for (j=0; j<3; ++j) {
443 force[i] += offset[j]*m_jacobian[i][j];
444 }
445 }
446 }
447 return force;
448 }
449
450protected:
451 bool m_active;
452 double m_time;
453 double m_cutoff;
454 d_vector m_origin;
455 d_vector m_addforce;
456 double m_jacobian[3][3];
457 double m_newcut;
458 d_vector m_neworig;
459 d_vector m_newadd;
460 double m_newjacob[3][3];
461 double m_damping;
462};
463
465static int VRPN_CALLBACK handle_forcefield_change_message(void *userdata, vrpn_HANDLERPARAM p)
466{
467 vrpn_Tracker_NovintFalcon *dev = (vrpn_Tracker_NovintFalcon *)userdata;
468 dev->update_forcefield_effect(p);
469 return 0;
470}
471
473class vrpn_NovintFalcon_ForceObjects {
474public:
475 std::vector<ForceFieldEffect*> m_FFEffects;
476
477protected:
478 d_vector m_curforce; //< collected force value
479 d_vector m_curpos; //< position for force calculation
480 d_vector m_curvel; //< velocity for force calculation
481
482public:
484 vrpn_NovintFalcon_ForceObjects() {
485 m_curforce.fill(0.0);
486 m_curpos.fill(0.0);
487 };
489 ~vrpn_NovintFalcon_ForceObjects() {};
490public:
492 d_vector getObjForce(vrpn_float64 *pos, vrpn_float64 *vel) {
493 int i;
494
495 for (i=0; i<3; ++i) {
496 m_curforce[i]=0.0;
497 m_curpos[i]=pos[i];
498 m_curvel[i]=vel[i];
499 }
500
501 // force field objects
502 int nobj = m_FFEffects.size();
503 for (i=0; i<nobj; ++i) {
504 m_curforce = m_curforce + m_FFEffects[i]->calcForce (m_curpos);
505 }
506 return m_curforce;
507 };
508};
509
511vrpn_Tracker_NovintFalcon::vrpn_Tracker_NovintFalcon(const char *name,
513 const int devidx,
514 const char *grip,
515 const char *kine,
516 const char *damp)
517 : vrpn_Tracker(name, c), vrpn_Button_Filter(name, c),
518 vrpn_ForceDevice(name, c), m_dev(NULL), m_obj(NULL), m_update_rate(1000.0), m_damp(0.9)
519{
520 m_devflags=vrpn_NovintFalcon_Device::MASK_DEVICEIDX & devidx;
521 if (grip != NULL) {
522 if (0 == strcmp(grip,"4-button")) {
523 m_devflags |= vrpn_NovintFalcon_Device::GRIP_FOURBUTTON;
525 } else {
526 fprintf(stderr, "WARNING: Unknown grip for Novint Falcon #%d: %s \n", devidx, grip);
527 m_devflags = -1;
528 return;
529 }
530 }
531
532 if (kine != NULL) {
533 if (0 == strcmp(kine,"stamper")) {
534 m_devflags |= vrpn_NovintFalcon_Device::KINE_STAMPER;
535 } else {
536 fprintf(stderr, "WARNING: Unknown kinematic model for Novint Falcon #%d: %s \n", devidx, kine);
537 m_devflags = -1;
538 return;
539 }
540 }
541
542 if (damp != NULL) {
543 vrpn_float64 val= atof(damp);
544 if (val >= 1.0 && val <= 10000.0) {
545 m_damp = 1.0 - 1.0/val;
546 } else {
547 fprintf(stderr, "WARNING: Ignoring illegal force effect damping factor: %g \n", val);
548 }
549 }
550 clear_values();
551
552 if (register_autodeleted_handler(forcefield_message_id,
553 handle_forcefield_change_message, this, vrpn_ForceDevice::d_sender_id)) {
554 fprintf(stderr,"vrpn_Tracker_NovintFalcon:can't register force handler\n");
555 return;
556 }
557 vrpn_gettimeofday(&m_timestamp, NULL);
558 status = vrpn_TRACKER_RESETTING;
559}
560
561void vrpn_Tracker_NovintFalcon::clear_values()
562{
563 // nothing to do
564 if (m_devflags < 0) return;
565
566 int i;
567 for (i=0; i <vrpn_Button::num_buttons; i++)
569
570 if (m_obj) {
571 try {
572 delete m_obj;
573 } catch (...) {
574 fprintf(stderr, "vrpn_Tracker_NovintFalcon::clear_values(): delete failed\n");
575 return;
576 }
577 }
578 try { m_obj = new vrpn_NovintFalcon_ForceObjects; }
579 catch (...) { m_obj = NULL; return; }
580
581 // add dummy effect object
582 ForceFieldEffect *ffe;
583 try { ffe = new ForceFieldEffect; }
584 catch (...) { return; }
585 ffe->setDamping(m_damp);
586 ffe->stop();
587 m_obj->m_FFEffects.push_back(ffe);
588}
589
590vrpn_Tracker_NovintFalcon::~vrpn_Tracker_NovintFalcon()
591{
592 try {
593 if (m_dev) delete m_dev;
594 if (m_obj) delete m_obj;
595 } catch (...) {
596 fprintf(stderr, "vrpn_Tracker_NovintFalcon::~vrpn_Tracker_NovintFalcon(): delete failed\n");
597 return;
598 }
599}
600
601void vrpn_Tracker_NovintFalcon::reset()
602{
603
604 // nothing to do
605 if (m_devflags < 0) return;
606
607 int ret, i;
608 clear_values();
609
610 fprintf(stderr, "Resetting the NovintFalcon #%d\n",
611 vrpn_NovintFalcon_Device::MASK_DEVICEIDX & m_devflags);
612
613 if (m_dev) {
614 try {
615 delete m_dev;
616 } catch (...) {
617 fprintf(stderr, "vrpn_Tracker_NovintFalcon::reset(): delete failed\n");
618 return;
619 }
620 }
621
622 try { m_dev = new vrpn_NovintFalcon_Device(m_devflags); }
623 catch (...) {
624#ifdef VERBOSE
625 fprintf(stderr, "Device constructor failed!\n");
626#endif
627 status = vrpn_TRACKER_FAIL;
628 m_dev = NULL;
629 return;
630 }
631
632 if (!m_dev->init()) {
633#ifdef VERBOSE
634 fprintf(stderr, "Device init failed!\n");
635#endif
636 status = vrpn_TRACKER_FAIL;
637 return;
638 }
639
640 fprintf(stderr, "Reset Completed.\n");
641 status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
642}
643
644int vrpn_Tracker_NovintFalcon::get_report(void)
645{
646 if (!m_dev)
647 return 0;
648
649 if (status == vrpn_TRACKER_SYNCING) {
650 if (m_dev->get_status(pos, vel, d_quat, vel_quat, &vel_quat_dt, buttons)) {
651 // if all buttons are pressed. we force a reset.
652 int i,j;
653 j=0;
654 for (i=0; i < num_buttons; i++)
655 j += buttons[i];
656 // all buttons pressed
657 if (j == num_buttons) {
658 status = vrpn_TRACKER_FAIL;
659 return 0;
660 }
661 } else {
662 status = vrpn_TRACKER_FAIL;
663 return 0;
664 }
665 }
666 status = vrpn_TRACKER_SYNCING;
667
668#ifdef VERBOSE2
669 print_latest_report();
670#endif
671
672 return 1;
673}
674
675void vrpn_Tracker_NovintFalcon::send_report(void)
676{
677 if (d_connection) {
678 char msgbuf[1000];
679 int len = vrpn_Tracker::encode_to(msgbuf);
680 if (d_connection->pack_message(len, m_timestamp, position_m_id, d_sender_id, msgbuf,
682 }
683 len = vrpn_Tracker::encode_vel_to(msgbuf);
684 if (d_connection->pack_message(len, m_timestamp, velocity_m_id, d_sender_id, msgbuf,
686 }
687 }
688}
689
690void vrpn_Tracker_NovintFalcon::handle_forces(void)
691{
692 if (!m_dev) return;
693 if (!m_obj) return;
694
695 // we have just updated our published position and can use that
696 d_vector force= m_obj->getObjForce(pos,vel);
697 m_dev->set_force(force);
698}
699
700
701int vrpn_Tracker_NovintFalcon::update_forcefield_effect(vrpn_HANDLERPARAM p)
702{
703 if (!m_obj) return 1;
704
705 vrpn_float32 center[3];
706 vrpn_float32 force[3];
707 vrpn_float32 jacobian[3][3];
708 vrpn_float32 radius;
709
710 decode_forcefield(p.buffer, p.payload_len, center, force, jacobian, &radius);
711 // XXX: only one force field effect. sufficient for VMD.
712 // we have just updated our published position and can use that
713 m_obj->m_FFEffects[0]->start();
714 m_obj->m_FFEffects[0]->setForce(center, force, jacobian, radius);
715 return 0;
716}
717
718
719void vrpn_Tracker_NovintFalcon::mainloop()
720{
721 struct timeval current_time;
722 server_mainloop();
723
724 // no need to report more often than we can poll the device
725 vrpn_gettimeofday(&current_time, NULL);
726 if ( timediff(current_time, m_timestamp) >= 1000000.0/m_update_rate) {
727
728 // Update the time
729 m_timestamp.tv_sec = current_time.tv_sec;
730 m_timestamp.tv_usec = current_time.tv_usec;
731 switch(status)
732 {
736 if (get_report()) {
737 send_report();
739 handle_forces();
740 }
741 break;
743 reset();
744 break;
745
747 break;
748
749 default:
750 fprintf(stderr, "NovintFalcon #%d , unknown status message: %d)\n",
751 vrpn_NovintFalcon_Device::MASK_DEVICEIDX & m_devflags, status);
752 break;
753 }
754 }
755}
756
757#endif
vrpn_int32 d_sender_id
Sender ID registered with the connection.
All button servers should derive from this class, which provides the ability to turn any of the butto...
Definition: vrpn_Button.h:66
vrpn_int32 num_buttons
Definition: vrpn_Button.h:48
virtual void report_changes(void)
Definition: vrpn_Button.C:423
unsigned char lastbuttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:46
unsigned char buttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:45
Generic connection class not specific to the transport mechanism.
virtual int encode_to(char *buf)
Definition: vrpn_Tracker.C:552
virtual int encode_vel_to(char *buf)
Definition: vrpn_Tracker.C:577
This structure is what is passed to a vrpn_Connection message callback.
const char * buffer
vrpn_int32 payload_len
#define VRPN_CALLBACK
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:99
const int vrpn_TRACKER_FAIL
Definition: vrpn_Tracker.h:40
const int vrpn_TRACKER_RESETTING
Definition: vrpn_Tracker.h:39
const int vrpn_TRACKER_SYNCING
Definition: vrpn_Tracker.h:35
const int vrpn_TRACKER_PARTIAL
Definition: vrpn_Tracker.h:38
const int vrpn_TRACKER_AWAITING_STATION
Definition: vrpn_Tracker.h:36