15#ifdef VRPN_USE_COLIBRIAPI
17vrpn_Tracker_Colibri::vrpn_Tracker_Colibri(
const char* name,
vrpn_Connection* c,
18 const char* path,
int Hz,
bool report_a_w)
19 :
vrpn_Tracker(name, c), nw(NULL), report_a_w(report_a_w)
22 printf(
"ColibriAPI version: %s\n", TC_APIVersion());
24 if (TC_Initialize(colibri_api_strings) != TCR_SUCCESS) {
25 printf(
"Initialize ColibriAPI failed!\n");
30 result = TC_NewFromHardware(&nw, TCO_NEW_WIRED | TCO_NEW_WIRELESS);
33 result = TC_LoadConf(nw, path);
34 if (result == TCR_SUCCESS)
35 result = TC_New(&nw, TCO_NEW_WIRED | TCO_NEW_WIRELESS);
39 for (ok =
false; !ok; ok =
true) {
40 if (result == TCR_FILE) {
41 printf(
"File %s not found or bad format.\n", path);
46 num_sensors = TC_GetNodeInfo(nw, 0, NULL);
47 printf(
"%d Colibri sensors found\n", num_sensors);
49 if (result != TCR_SUCCESS) {
50 printf(
"%s not found (error %d).\n", TC_GetFailedID(nw)[0]?TC_GetFailedID(nw):
"Device", result);
51 if (result == TCR_IO_REMOTE) {
52 printf(
"Dongle will scan for wireless Colibri next 20 seconds...\n"
53 "If sensor is off - just turn it on; if you think it's on another channel: "
54 "put it in scanning mode (push button for 2 seconds, LED will flash fast).\n"
55 "Then restart server.\n");
56 const char (*list)[8];
57 unsigned int nodes_found;
58 int gates = TC_GetGateInfo(nw, 0, NULL);
59 for (
int j = 0; j < gates; ++j)
60 TC_Discover(nw, j, &list, &nodes_found);
65 TC_SaveConf(nw,
"./current.conf");
66 for (
int i = 0; i < num_sensors; ++i) {
68 TC_GetNodeInfo(nw, i, &info);
69 printf(
"%d: %s (FW %d.%.3d)\n", i, info.ID, info.FW_ver/1000, info.FW_ver%1000);
73 result = TC_QuickConfig(nw, TCS_ORIENT | (report_a_w ? TCS_ACC|TCS_GYR : 0));
74 if (result != TCR_SUCCESS) {
75 printf(
"Colibri sensor error.\n");
82 result = TC_Run(nw, Hz);
83 if (result == TCR_SUCCESS) {
84 printf(
"Network started...\n");
86 printf(
"Failed to start network.\n");
97 register_server_handlers();
100vrpn_Tracker_Colibri::~vrpn_Tracker_Colibri()
105 if (TCR_SUCCESS == TC_Stop(nw))
106 printf(
"Network stopped.\n");
108 printf(
"Failed to stop network.\n");
113void vrpn_Tracker_Colibri::mainloop()
122void vrpn_Tracker_Colibri::get_report()
126 const TCSample **data;
127 while (TCR_SUCCESS == TC_GetNextData(nw, &data)) {
128 for (
int i = 0; i < num_sensors; ++i) {
135 tts = data[i]->t / 10000;
136 ttu = (data[i]->t - tts * 10000) * 100;
137 timestamp.tv_sec = tts;
138 timestamp.tv_usec = ttu;
143 d_quat[Q_X] = data[i]->q[1];
144 d_quat[Q_Y] = data[i]->q[2];
145 d_quat[Q_Z] = data[i]->q[3];
146 d_quat[Q_W] = data[i]->q[0];
147 q_conjugate(d_quat, d_quat);
151 acc[0] = data[i]->a[0];
152 acc[1] = data[i]->a[1];
153 acc[2] = data[i]->a[2];
156 const vrpn_float64 x = data[i]->g[0] * vel_quat_dt;
157 const vrpn_float64 y = data[i]->g[1] * vel_quat_dt;
158 const vrpn_float64 z = data[i]->g[2] * vel_quat_dt;
159 const vrpn_float64 angle = sqrt(x*x + y*y + z*z);
161 vel_quat[Q_X] = x*sin(angle/2.0f)/angle;
162 vel_quat[Q_Y] = y*sin(angle/2.0f)/angle;
163 vel_quat[Q_Z] = z*sin(angle/2.0f)/angle;
164 vel_quat[Q_W] = cos(angle/2.0f);
166 vel_quat[Q_X] = vel_quat[Q_Y] = vel_quat[Q_Z] = 0.0f;
167 vel_quat[Q_W] = 1.0f;
178void vrpn_Tracker_Colibri::send_report()
183 int len = encode_to(msgbuf);
185 fprintf(stderr,
"Tracker: cannot write message: tossing\n");
188 len = encode_acc_to(msgbuf);
190 fprintf(stderr,
"Tracker: cannot write message: tossing\n");
192 len = encode_vel_to(msgbuf);
194 fprintf(stderr,
"Tracker: cannot write message: tossing\n");
Generic connection class not specific to the transport mechanism.
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY