Changeset 20358 for juggler/branches/2.2/modules/gadgeteer/drivers/ART

juggler/branches/2.2/modules/gadgeteer/drivers/ART/DTrack/DTrack.cpp

r18827	r20358
1	1	/* Gadgeteer Driver for 'A.R.T. DTrack' Tracker
2		* Copyright (C) 2005, Advanced Realtime Tracking GmbH
	2	* Copyright (C) 2005-2007, Advanced Realtime Tracking GmbH
3	3	*
4	4	* This library is free software; you can redistribute it and/or
…	…
22	22	* Kurt Achatz, Advanced Realtime Tracking GmbH (http://www.ar-tracking.de)
23	23	*
24		* Last modified: 200~~5/06/16~~
	24	* Last modified: 2007/06/20
25	25	*
26		* DTrack.cpp,v 1.~~2 2005/06/16 14:43:31~~ kurt Exp
	26	* DTrack.cpp,v 1.5 2007/06/20 15:12:58 kurt Exp
27	27	*/
28	28
…	…
43	43	// some constants:
44	44
45		#define VERSION_STRING "DTrack Driver v0.~~1.0; (C) 2005~~, Advanced Realtime Tracking GmbH"
	45	#define VERSION_STRING "DTrack Driver v0.2.0; (C) 2005-2007, Advanced Realtime Tracking GmbH"
46	46
47	47	#define BUTTONS_PER_FLYSTICK 8 // number of buttons per 'Flystick' (fixed)
48	48	#define VALUATORS_PER_FLYSTICK 2 // number of valuators per 'Flystick' (fixed)
	49	#define BUTTONS_PER_MEATOOL 2 // number of buttons per 'Measurement Tool' (fixed)
49	50
50	51
…	…
98	99	{
99	100
100		return ~~"dtrack"~~;
	101	return std::string("dtrack");
101	102	}
102	103
…	…
166	167
167	168	if(use_commands){
168		standalone->send(DTRACKLIB_CMD_CAMERAS_AND_CALC_ON);
169		standalone->send(DTRACKLIB_CMD_SEND_DATA);
	169	standalone->cmd_cameras(true);
170	170	}
171	171
…	…
212	212
213	213	if(use_commands){
214		standalone->send(DTRACKLIB_CMD_STOP_DATA);
215		standalone->send(DTRACKLIB_CMD_CAMERAS_OFF);
	214	standalone->cmd_cameras(false);
216	215	}
217	216
…	…
246	245	{
247	246	bool stat;
248		unsigned long i;
249		unsigned long nbody, nflystick;
	247	int i, j, id;
	248	int nbt, nvt;
	249	int num_body, num_flystick, num_meatool;
250	250
251	251	if(!thrRunning){
…	…
259	259	}
260	260
261		nbody = (unsigned long )standalone->get_nbody();
262		nflystick = (unsigned long )standalone->get_nflystick();
263
264		if(standalone->get_nbodycal() > -1){ // number of calibrated bodies available
265		unsigned long nbodycal = (unsigned long )standalone->get_nbodycal();
266		nbodycal -= standalone->get_nmeatool(); // ignore measurement tools
	261	num_body = standalone->get_num_body();
	262	num_flystick = standalone->get_num_flystick();
	263	num_meatool = standalone->get_num_meatool();
267	264
268		resize_curPosition(nbodycal);
269		}
	265	resize_curPosition(num_flystick + num_meatool + num_body);
	266	resize_curDigital(num_flystick * BUTTONS_PER_FLYSTICK + num_meatool * BUTTONS_PER_MEATOOL);
	267	resize_curAnalog(num_flystick * VALUATORS_PER_FLYSTICK);
270	268
271	269	// get 'Flystick' data:
272	270
273		resize_curPosition(nflystick);
274
275		if(nflystick * BUTTONS_PER_FLYSTICK > curDigital.size()){
276		curDigital.resize(nflystick * BUTTONS_PER_FLYSTICK); // all elements will be set later
277		}
278
279		if(nflystick * VALUATORS_PER_FLYSTICK > curAnalog.size()){
280		curAnalog.resize(nflystick * VALUATORS_PER_FLYSTICK); // all elements will be set later
281		}
282
283		for(i=0; i<nflystick; i++){
284		dtracklib_flystick_type dat = standalone->get_flystick(i);
	271	for(i=0; i<num_flystick; i++){
	272	dtrack_flystick_type dat = standalone->get_flystick(i);
285	273
286	274	if(dat.quality >= 0){ // check if Flystick position is tracked
…	…
291	279	// Flystick buttons:
292	280
293		unsigned long bt = dat.bt;
294		for(int j=0; j<BUTTONS_PER_FLYSTICK; j++){
295		curDigital[i*BUTTONS_PER_FLYSTICK + j] = bt & 0x0001;
296		curDigital[i*BUTTONS_PER_FLYSTICK + j].setTime();
	281	nbt = dat.num_button;
	282
	283	if(nbt > BUTTONS_PER_FLYSTICK){
	284	nbt = BUTTONS_PER_FLYSTICK;
	285	}
	286
	287	for(j=0; j<nbt; j++){
	288	id = j + i * BUTTONS_PER_FLYSTICK; // VRJuggler id number
297	289
298		bt >>= 1;
299		}
300
301		// Flystick 'HAT switch' (valuators simulated using four buttons):
302
303		for(int j=0; j<VALUATORS_PER_FLYSTICK; j++){
304		curAnalog[i*VALUATORS_PER_FLYSTICK + j] = 0;
305		curAnalog[i*VALUATORS_PER_FLYSTICK + j].setTime();
306		}
307
308		float f, fn;
309
310		f = 0;
311		if(dat.bt & 0x0020){
312		f = -1;
313		}else if(dat.bt & 0x0080){
314		f = 1;
315		}
316
317		if(f != 0){
318		this->normalizeMinToMax(f, fn);
319		curAnalog[i*VALUATORS_PER_FLYSTICK + 0] = fn;
320		}
321
322		f = 0;
323		if(dat.bt & 0x0010){
324		f = -1;
325		}else if(dat.bt & 0x0040){
326		f = 1;
327		}
328
329		if(f != 0){
330		this->normalizeMinToMax(f, fn);
331		curAnalog[i*VALUATORS_PER_FLYSTICK + 1] = fn;
	290	curDigital[id] = dat.button[j];
	291	curDigital[id].setTime();
	292	}
	293
	294	// Flystick valuators ('HAT switch' or 'joystick'):
	295
	296	nvt = dat.num_joystick;
	297
	298	if(nvt > VALUATORS_PER_FLYSTICK){
	299	nvt = VALUATORS_PER_FLYSTICK;
	300	}
	301
	302	for(j=0; j<nvt; j++){
	303	id = j + i * VALUATORS_PER_FLYSTICK; // VRJuggler id number
	304
	305	curAnalog[id] = dat.joystick[j] / 2.0 + 0.5; // normalizing
	306	curAnalog[id].setTime();
	307	}
	308	}
	309
	310	// get 'measurement tool' data:
	311
	312	for(i=0; i<num_meatool; i++){
	313	dtrack_meatool_type dat = standalone->get_meatool(i);
	314
	315	if(dat.quality >= 0){ // check if position is tracked
	316	id = i + num_flystick; // VRJuggler id number
	317
	318	curPosition[id].mPosData = getpos(dat);
	319	curPosition[id].setTime();
	320	} // otherwise keep last valid position
	321
	322	// measurement tool buttons:
	323
	324	nbt = dat.num_button;
	325
	326	if(nbt > BUTTONS_PER_MEATOOL){
	327	nbt = BUTTONS_PER_MEATOOL;
	328	}
	329
	330	for(j=0; j<nbt; j++){
	331	id = j + i * BUTTONS_PER_MEATOOL + num_flystick * BUTTONS_PER_FLYSTICK; // VRJuggler id number
	332
	333	curDigital[id] = dat.button[j];
	334	curDigital[id].setTime();
332	335	}
333	336	}
…	…
335	338	// get 'standard body' data:
336	339
337		for(i=0; i<nbody; i++){
338		dtracklib_body_type dat = standalone->get_body(i);
339
340		unsigned long id = dat.id + nflystick; // VRJuggler id number of this 'Standard Body'
341		resize_curPosition(id+1);
342
343		curPosition[id].mPosData = getpos(dat);
344		curPosition[id].setTime();
345		} // bodies not in this list are not tracked: keep their last valid position
	340	for(i=0; i<num_body; i++){
	341	dtrack_body_type dat = standalone->get_body(i);
	342
	343	if(dat.quality >= 0){ // check if position is tracked
	344	id = i + num_flystick + num_meatool; // VRJuggler id number
	345
	346	curPosition[id].mPosData = getpos(dat);
	347	curPosition[id].setTime();
	348	} // otherwise keep last valid position
	349	}
346	350
347	351	// update buffers:
…	…
373	377	// n (i): new size
374	378
375		void DTrack::resize_curPosition(~~unsigned long~~ n)
376		{
377		~~unsigned long nsize =~~ curPosition.size();
	379	void DTrack::resize_curPosition(int n)
	380	{
	381	int nsize = (int )curPosition.size();
378	382
379	383	if(n > nsize){
380	384	curPosition.resize(n);
381	385
382		for(~~unsigned long~~ i=nsize; i<n; i++){ // set default for new elements
	386	for(int i=nsize; i<n; i++){ // set default for new elements
383	387	curPosition[i].mPosData = getpos_default();
384	388	curPosition[i].setTime();
…	…
387	391	}
388	392
	393	// Resize curDigital vector:
	394	// n (i): new size
	395
	396	void DTrack::resize_curDigital(int n)
	397	{
	398	int nsize = (int )curDigital.size();
	399
	400	if(n > nsize){
	401	curDigital.resize(n);
	402
	403	for(int i=nsize; i<n; i++){ // set default for new elements
	404	curDigital[i] = 0;
	405	curDigital[i].setTime();
	406	}
	407	}
	408	}
	409
	410	// Resize curAnalog vector:
	411	// n (i): new size
	412
	413	void DTrack::resize_curAnalog(int n)
	414	{
	415	int nsize = (int )curAnalog.size();
	416	float fn;
	417
	418	if(n > nsize){
	419	curAnalog.resize(n);
	420
	421	for(int i=nsize; i<n; i++){ // set default for new elements
	422	this->normalizeMinToMax(0, fn);
	423	curAnalog[i] = fn;
	424	curAnalog[i].setTime();
	425	}
	426	}
	427	}
	428
389	429
390	430	// Transfer DTracklib pose into gmtl pose:
…	…
393	433	// return value (o): gmtl pose
394	434
395		gmtl::Matrix44f DTrack::getpos(dtrack~~lib~~_body_type& bod)
	435	gmtl::Matrix44f DTrack::getpos(dtrack_body_type& bod)
396	436	{
397	437	gmtl::Matrix44f ret_val;
…	…
411	451	}
412	452
413		gmtl::Matrix44f DTrack::getpos(dtrack~~lib~~_flystick_type& bod)
	453	gmtl::Matrix44f DTrack::getpos(dtrack_flystick_type& bod)
414	454	{
415	455	gmtl::Matrix44f ret_val;
…	…
429	469	}
430	470
	471	gmtl::Matrix44f DTrack::getpos(dtrack_meatool_type& bod)
	472	{
	473	gmtl::Matrix44f ret_val;
	474
	475	for(int i=0; i<3; i++){
	476	for(int j=0; j<3; j++){
	477	ret_val[i][j] = bod.rot[i + 3*j];
	478	}
	479
	480	ret_val[3][i] = 0;
	481	ret_val[i][3] = bod.loc[i] / 1000; // convert to meters
	482	}
	483
	484	ret_val[3][3] = 1;
	485
	486	return ret_val;
	487	}
	488
431	489	gmtl::Matrix44f DTrack::getpos_default(void)
432	490	{

juggler/branches/2.2/modules/gadgeteer/drivers/ART/DTrack/DTrack.h

r20309	r20358
1	1	/* Gadgeteer Driver for 'A.R.T. DTrack' Tracker
2		* Copyright (C) 2005, Advanced Realtime Tracking GmbH
	2	* Copyright (C) 2005-2007, Advanced Realtime Tracking GmbH
3	3	*
4	4	* This library is free software; you can redistribute it and/or
…	…
22	22	* Kurt Achatz, Advanced Realtime Tracking GmbH (http://www.ar-tracking.de)
23	23	*
24		* Last modified: 200~~5/05~~/28
	24	* Last modified: 2007/03/28
25	25	*
26		* DTrack.h,v 1.~~2 2005/06/16 14:43:31~~ kurt Exp
	26	* DTrack.h,v 1.3 2007/06/20 15:10:55 kurt Exp
27	27	*/
28	28
…	…
96	96
97	97	DTrackStandalone* standalone;
98		~~unsigned shor~~t receive_port;
	98	int receive_port;
99	99	bool use_commands;
100	100	std::string server_name;
101		~~unsigned shor~~t command_port;
	101	int command_port;
102	102
103	103	std::vector<PositionData> curPosition;
…	…
105	105	std::vector<AnalogData> curAnalog;
106	106
107		void resize_curPosition(unsigned long n);
	107	void resize_curPosition(int n);
	108	void resize_curDigital(int n);
	109	void resize_curAnalog(int n);
108	110
109		gmtl::Matrix44f getpos(dtracklib_body_type& bod);
110		gmtl::Matrix44f getpos(dtracklib_flystick_type& bod);
	111	gmtl::Matrix44f getpos(dtrack_body_type& bod);
	112	gmtl::Matrix44f getpos(dtrack_flystick_type& bod);
	113	gmtl::Matrix44f getpos(dtrack_meatool_type& bod);
111	114	gmtl::Matrix44f getpos_default(void);
112	115	};

juggler/branches/2.2/modules/gadgeteer/drivers/ART/DTrack/DTrackStandalone.cpp

r19847	r20358
1	1	/* Gadgeteer Driver for 'A.R.T. DTrack' Tracker
2		* Copyright (C) 2005, Advanced Realtime Tracking GmbH
	2	* Copyright (C) 2005-2007, Advanced Realtime Tracking GmbH
3	3	*
4	4	* This library is free software; you can redistribute it and/or
…	…
18	18	*
19	19	* Purpose: standalone driver class; derived from the A.R.T. sample source
20		* code 'DTrack~~lib' (version v1.2~~.1), just modified for use with VPR
	20	* code 'DTrackSDK' (version v1.3.1), just modified for use with VPR
21	21	*
22	22	* Authors: Kurt Achatz, Advanced Realtime Tracking GmbH (http://www.ar-tracking.de)
23	23	*
24		* Last modified: 200~~5/06/14~~
	24	* Last modified: 2007/03/27
25	25	*
26		* DTrackStandalone.cpp,v 1.~~2 2005/06/16 14:44:01~~ kurt Exp
	26	* DTrackStandalone.cpp,v 1.4 2007/06/20 15:15:12 kurt Exp
27	27	*/
28	28
…	…
45	45	// Local error codes:
46	46
47		#define DTRACKLIB_ERR_NONE 0
48		#define DTRACKLIB_ERR_TIMEOUT 1 // timeout while receiving data
49		#define DTRACKLIB_ERR_UDP 2 // UDP receive error
50		#define DTRACKLIB_ERR_PARSE 3 // error in UDP packet
	47	#define DTRACK_ERR_NONE 0 // no error
	48	#define DTRACK_ERR_TIMEOUT 1 // timeout while receiving data
	49	#define DTRACK_ERR_UDP 2 // UDP receive error
	50	#define DTRACK_ERR_PARSE 3 // error in UDP packet
	51
	52	// DTrack remote commands:
	53
	54	#define DTRACK_CMD_CAMERAS_OFF 1
	55	#define DTRACK_CMD_CAMERAS_ON 2
	56	#define DTRACK_CMD_CAMERAS_AND_CALC_ON 3
	57
	58	#define DTRACK_CMD_SEND_DATA 11
	59	#define DTRACK_CMD_STOP_DATA 12
	60	#define DTRACK_CMD_SEND_N_DATA 13
51	61
52	62
53	63	// Local prototypes:
54	64
55		static char* string_nextline(char* str, char* start, unsigned long len);
56		static char* string_get_ul(char* str, unsigned long* ul);
	65	static char* string_nextline(char* str, char* start, int len);
	66	static char* string_get_i(char* str, int* i);
	67	static char* string_get_ui(char* str, unsigned int* ui);
57	68	static char* string_get_d(char* str, double* d);
58	69	static char* string_get_f(char* str, float* f);
59		static char* string_get_block(char* str, char* fmt, ~~unsigned long* ul~~dat, float* fdat);
	70	static char* string_get_block(char* str, char* fmt, int* idat, float* fdat);
60	71
61	72
…	…
67	78	// udpport (i): UDP port number to receive data from DTrack
68	79	//
69		// remote_~~ip (i): DTrack remote control: ip~~ address of DTrack PC (NULL if not used)
	80	// remote_host (i): DTrack remote control: hostname or IP address of DTrack PC (NULL if not used)
70	81	// remote_port (i): port number of DTrack remote control (0 if not used)
71	82	//
72	83	// udpbufsize (i): size of buffer for UDP packets (in bytes)
73		// udptimeout_us (i): UDP timeout (receiving and sending) in us (micro sec)
	84	// udptimeout_us (i): UDP timeout (receiving and sending) in us (micro second)
74	85
75	86	DTrackStandalone::DTrackStandalone(
76		~~unsigned short udpport, const char* remote_ip, unsigned shor~~t remote_port,
77		int udpbufsize, ~~unsigned long~~ udptimeout_us
	87	int udpport, const char* remote_host, int remote_port,
	88	int udpbufsize, int udptimeout_us
78	89	)
79	90	{
80	91	vpr::InetAddr addr;
81	92
	93	d_udpsock = NULL;
82	94	d_udpbuf = NULL;
83	95	set_noerror();
…	…
85	97	// creat UDP socket:
86	98
87		if(remote_ip != NULL && remote_port != 0){
88		try
89		{
90		d_remote.setAddress(remote_ip, remote_port);
91		d_use_remote = true;
92		}
93		catch (vpr::UnknownHostException& ex)
94		{
95		vprDEBUG(vprDBG_ALL, vprDBG_CRITICAL_LVL)
96		<< "[DTrackStandalone] invalid remote address "
97		<< remote_ip << ":" << remote_port
98		<< std::endl << vprDEBUG_FLUSH;
99		vprDEBUG_NEXT(vprDBG_ALL, vprDBG_CRITICAL_LVL)
100		<< ex.what() << std::endl;
101		d_udpsock = NULL;
102		return;
103		}
104		}else{
105		d_use_remote = false;
	99	if(udpport <= 0 \|\| udpport > 65535){
	100	return;
106	101	}
107	102
108	103	addr = vpr::InetAddr::AnyAddr;
109		addr.setPort(udpport);
	104	addr.setPort((unsigned short )udpport);
110	105
111	106	d_udpsock = new vpr::SocketDatagram(addr, vpr::InetAddr::AnyAddr);
…	…
130	125	}
131	126
	127	// DTrack remote control parameters:
	128
	129	d_use_remote = false;
	130
	131	if(remote_host != NULL && remote_port > 0 && remote_port <= 65535){
	132	try
	133	{
	134	d_remote.setAddress(remote_host, remote_port);
	135	d_use_remote = true;
	136	}
	137	catch (vpr::UnknownHostException& ex)
	138	{
	139	vprDEBUG(vprDBG_ALL, vprDBG_CRITICAL_LVL)
	140	<< "[DTrackStandalone] invalid remote address "
	141	<< remote_host << ":" << remote_port
	142	<< std::endl << vprDEBUG_FLUSH;
	143	vprDEBUG_NEXT(vprDBG_ALL, vprDBG_CRITICAL_LVL)
	144	<< ex.what() << std::endl;
	145	free(d_udpbuf);
	146	d_udpsock->close();
	147	delete d_udpsock;
	148	d_udpsock = NULL;
	149	return;
	150	}
	151	}
	152
	153	d_remote_cameras = false;
	154	d_remote_tracking = true;
	155	d_remote_sending = true;
	156
132	157	// reset actual DTrack data:
133	158
134		act_framenr = 0;
	159	act_framecounter = 0;
135	160	act_timestamp = -1;
136	161
137		act_n~~bodycal = -1~~;
138		act_n~~body = act_nflystick = act_nmeatool = act_nmarker = act_nglove~~ = 0;
	162	act_num_body = act_num_flystick = act_num_meatool = act_num_hand = 0;
	163	act_num_marker = 0;
139	164	}
140	165	catch (vpr::Exception& ex)
…	…
164	189	// release UDP socket:
165	190
166		if(d_udpsock ~~!= NULL~~){
	191	if(d_udpsock){
167	192	d_udpsock->close();
168	193	delete d_udpsock;
	194	d_udpsock = NULL;
169	195	}
170	196	}
…	…
187	213	bool DTrackStandalone::timeout(void) // 'timeout'
188	214	{
189		return (d_lasterror == DTRACK~~LIB~~_ERR_TIMEOUT);
	215	return (d_lasterror == DTRACK_ERR_TIMEOUT);
190	216	}
191	217
192	218	bool DTrackStandalone::udperror(void) // 'udp error'
193	219	{
194		return (d_lasterror == DTRACK~~LIB~~_ERR_UDP);
	220	return (d_lasterror == DTRACK_ERR_UDP);
195	221	}
196	222
197	223	bool DTrackStandalone::parseerror(void) // 'parse error'
198	224	{
199		return (d_lasterror == DTRACK~~LIB~~_ERR_PARSE);
	225	return (d_lasterror == DTRACK_ERR_PARSE);
200	226	}
201	227
…	…
204	230	void DTrackStandalone::set_noerror(void) // 'no error'
205	231	{
206		d_lasterror = DTRACK~~LIB~~_ERR_NONE;
	232	d_lasterror = DTRACK_ERR_NONE;
207	233	}
208	234
209	235	void DTrackStandalone::set_timeout(void) // 'timeout'
210	236	{
211		d_lasterror = DTRACK~~LIB~~_ERR_TIMEOUT;
	237	d_lasterror = DTRACK_ERR_TIMEOUT;
212	238	}
213	239
214	240	void DTrackStandalone::set_udperror(void) // 'udp error'
215	241	{
216		d_lasterror = DTRACK~~LIB~~_ERR_UDP;
	242	d_lasterror = DTRACK_ERR_UDP;
217	243	}
218	244
219	245	void DTrackStandalone::set_parseerror(void) // 'parse error'
220	246	{
221		d_lasterror = DTRACK~~LIB~~_ERR_PARSE;
	247	d_lasterror = DTRACK_ERR_PARSE;
222	248	}
223	249
…	…
234	260
235	261	char* s;
236		int i, j;
237		unsigned long ul, ularr[2];
238		float farr[6];
	262	int i, j, k, l, n, id;
	263	char sfmt[20];
	264	int iarr[3];
	265	float f, farr[6];
	266	int loc_num_bodycal, loc_num_handcal, loc_num_flystick1, loc_num_meatool;
239	267
240	268	if(!valid()){
…	…
242	270	}
243	271
244		// Defaults:
245
246		act_framenr = 0;
	272	// defaults:
	273
	274	act_framecounter = 0;
247	275	act_timestamp = -1; // i.e. not available
248		act_nbodycal = -1; // i.e. not available
249		act_nbody = act_nflystick = act_nmeatool = act_nmarker = act_nglove = 0;
250
251		// Receive UDP packet:
	276
	277	loc_num_bodycal = loc_num_handcal = -1; // i.e. not available
	278	loc_num_flystick1 = loc_num_meatool = 0;
	279
	280	// receive UDP packet:
252	281
253	282	do{
…	…
272	301	s[len] = '\0';
273	302
274		// Process lines:
	303	// process lines:
275	304
276	305	set_parseerror();
277	306
278	307	do{
279		// Line for frame counter:
	308	// line for frame counter:
280	309
281	310	if(!strncmp(s, "fr ", 3)){
282	311	s += 3;
283	312
284		if(!(s = string_get_ul(s, &ul))){ // get frame counter
	313	if(!(s = string_get_ui(s, &act_framecounter))){ // get frame counter
	314	act_framecounter = 0;
285	315	return false;
286	316	}
287	317
288		~~act_framenr = ul;~~
289	318	continue;
290	319	}
291	320
292		// Line for timestamp:
	321	// line for timestamp:
293	322
294	323	if(!strncmp(s, "ts ", 3)){
295	324	s += 3;
296	325
297		if(!(s = string_get_d(s, &act_timestamp))){ // get timestamp
	326	if(!(s = string_get_d(s, &act_timestamp))){ // get timestamp
298	327	act_timestamp = -1;
299	328	return false;
300	329	}
	330
301	331	continue;
302	332	}
303	333
304		// Line for additional information about number of calibrated bodies:
	334	// line for additional information about number of calibrated bodies:
305	335
306	336	if(!strncmp(s, "6dcal ", 6)){
307	337	s += 6;
308	338
309		if(!(s = string_get_~~ul(s, &ul))){ // get number of~~ bodies
	339	if(!(s = string_get_i(s, &loc_num_bodycal))){ // get number of calibrated bodies
310	340	return false;
311	341	}
312	342
313		~~act_nbodycal = (int )ul;~~
314	343	continue;
315	344	}
316	345
317		// ~~Line for 6d~~ data:
	346	// line for standard body data:
318	347
319	348	if(!strncmp(s, "6d ", 3)){
320	349	s += 3;
321	350
322		if(!(s = string_get_ul(s, &ul))){ // get number of bodies
	351	for(i=0; i<act_num_body; i++){ // disable all existing data
	352	memset(&act_body[i], 0, sizeof(dtrack_body_type));
	353	act_body[i].id = i;
	354	act_body[i].quality = -1;
	355	}
	356
	357	if(!(s = string_get_i(s, &n))){ // get number of standard bodies (in line)
323	358	return false;
324	359	}
325	360
326		act_nbody = (int )ul;
327		if(act_nbody > (int )act_body.size()){
328		act_body.resize(act_nbody);
329		}
330
331		for(i=0; i<act_nbody; i++){ // get data of body
332		if(!(s = string_get_block(s, "uf", &act_body[i].id, &act_body[i].quality))){
333		return false;
334		}
335
336		if(!(s = string_get_block(s, "ffffff", NULL, farr))){
337		return false;
338		}
339		for(j=0; j<3; j++){
340		act_body[i].loc[j] = farr[j];
341		act_body[i].ang[j] = farr[j+3];
342		}
343
344		if(!(s = string_get_block(s, "fffffffff", NULL, act_body[i].rot))){
	361	for(i=0; i<n; i++){ // get data of standard bodies
	362	if(!(s = string_get_block(s, "if", &id, &f))){
	363	return false;
	364	}
	365
	366	if(id >= act_num_body){ // adjust length of vector
	367	act_body.resize(id + 1);
	368
	369	for(j=act_num_body; j<=id; j++){
	370	memset(&act_body[j], 0, sizeof(dtrack_body_type));
	371	act_body[j].id = j;
	372	act_body[j].quality = -1;
	373	}
	374
	375	act_num_body = id + 1;
	376	}
	377
	378	act_body[id].id = id;
	379	act_body[id].quality = f;
	380
	381	if(!(s = string_get_block(s, "fff", NULL, act_body[id].loc))){
	382	return false;
	383	}
	384
	385	if(!(s = string_get_block(s, "fffffffff", NULL, act_body[id].rot))){
345	386	return false;
346	387	}
…	…
350	391	}
351	392
352		// ~~Line for flystick data~~:
	393	// line for Flystick data (older format):
353	394
354	395	if(!strncmp(s, "6df ", 4)){
355	396	s += 4;
356	397
357		if(!(s = string_get_~~ul(s, &ul))){ // get number of f~~lysticks
	398	if(!(s = string_get_i(s, &n))){ // get number of calibrated Flysticks
358	399	return false;
359	400	}
360	401
361		act_nflystick = (int )ul;
362		if(act_nflystick > (int )act_flystick.size()){
363		act_flystick.resize(act_nflystick);
364		}
365
366		for(i=0; i<act_nflystick; i++){ // get data of body
367		if(!(s = string_get_block(s, "ufu", ularr, &act_flystick[i].quality))){
	402	loc_num_flystick1 = n;
	403
	404	if(n != act_num_flystick){ // adjust length of vector
	405	act_flystick.resize(n);
	406
	407	act_num_flystick = n;
	408	}
	409
	410	for(i=0; i<n; i++){ // get data of Flysticks
	411	if(!(s = string_get_block(s, "ifi", iarr, &f))){
368	412	return false;
369	413	}
370	414
371		act_flystick[i].id = ularr[0];
372		act_flystick[i].bt = ularr[1];
373
374		if(!(s = string_get_block(s, "ffffff", NULL, farr))){
375		return false;
376		}
377		for(j=0; j<3; j++){
378		act_flystick[i].loc[j] = farr[j];
379		act_flystick[i].ang[j] = farr[j+3];
	415	if(iarr[0] != i){ // not expected
	416	return false;
	417	}
	418
	419	act_flystick[i].id = iarr[0];
	420	act_flystick[i].quality = f;
	421
	422	act_flystick[i].num_button = 8;
	423	k = iarr[1];
	424	for(j=0; j<8; j++){
	425	act_flystick[i].button[j] = k & 0x01;
	426	k >>= 1;
	427	}
	428
	429	act_flystick[i].num_joystick = 2; // additionally to buttons 5-8
	430	if(iarr[1] & 0x20){
	431	act_flystick[i].joystick[0] = -1;
	432	}else if(iarr[1] & 0x80){
	433	act_flystick[i].joystick[0] = 1;
	434	}else{
	435	act_flystick[i].joystick[0] = 0;
	436	}
	437	if(iarr[1] & 0x10){
	438	act_flystick[i].joystick[1] = -1;
	439	}else if(iarr[1] & 0x40){
	440	act_flystick[i].joystick[1] = 1;
	441	}else{
	442	act_flystick[i].joystick[1] = 0;
	443	}
	444
	445	if(!(s = string_get_block(s, "fff", NULL, act_flystick[i].loc))){
	446	return false;
380	447	}
381	448	&nbs

Navigation

Legend:

juggler/branches/2.2/modules/gadgeteer/drivers/ART/DTrack/DTrack.cpp

juggler/branches/2.2/modules/gadgeteer/drivers/ART/DTrack/DTrack.h

juggler/branches/2.2/modules/gadgeteer/drivers/ART/DTrack/DTrackStandalone.cpp