Rev 48 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 48 | Rev 50 | ||
|---|---|---|---|
| Line 7... | Line 7... | ||
| 7 | #include <sys/socket.h> |
7 | #include <sys/socket.h> |
| 8 | #include <bluetooth/bluetooth.h> |
8 | #include <bluetooth/bluetooth.h> |
| 9 | #include <bluetooth/hci.h> |
9 | #include <bluetooth/hci.h> |
| 10 | #include <bluetooth/hci_lib.h> |
10 | #include <bluetooth/hci_lib.h> |
| 11 | 11 | ||
| - | 12 | #include <sys/time.h> |
|
| - | 13 | ||
| 12 | 14 | ||
| 13 | #include <bluetooth/rfcomm.h> |
15 | #include <bluetooth/rfcomm.h> |
| 14 | 16 | ||
| 15 | 17 | ||
| 16 | #include <linux/joystick.h> |
18 | #include <linux/joystick.h> |
| Line 24... | Line 26... | ||
| 24 | 26 | ||
| 25 | #define BUTTON_SELECT 26 |
27 | #define BUTTON_SELECT 26 |
| 26 | #define BUTTON_DOWN 28 |
28 | #define BUTTON_DOWN 28 |
| 27 | #define BUTTON_UP 27 |
29 | #define BUTTON_UP 27 |
| 28 | 30 | ||
| - | 31 | ||
| - | 32 | #define AXIS_ROLL 0 |
|
| - | 33 | #define AXIS_NICK 1 |
|
| - | 34 | #define AXIS_GIER 5 |
|
| - | 35 | #define AXIS_GAS 2 |
|
| - | 36 | ||
| - | 37 | #define INVERT_ROLL -1 |
|
| - | 38 | #define INVERT_NICK -1 |
|
| - | 39 | #define INVERT_GIER 1 |
|
| - | 40 | #define INVERT_GAS -1 |
|
| - | 41 | ||
| - | 42 | ||
| - | 43 | // time struct for measuring |
|
| - | 44 | struct timeval time_struct1; |
|
| - | 45 | struct timeval time_struct2; |
|
| - | 46 | ||
| - | 47 | ||
| - | 48 | int act_nick=0; |
|
| - | 49 | int act_roll=0; |
|
| - | 50 | int act_gier=0; |
|
| - | 51 | int act_gas=0; |
|
| - | 52 | int act_mode=0; |
|
| - | 53 | ||
| 29 | int bt_device_count=0; |
54 | int bt_device_count=0; |
| 30 | 55 | ||
| 31 | char names[MAX_BT_DEVICES][248]; |
56 | char names[MAX_BT_DEVICES][248]; |
| 32 | char addrs[MAX_BT_DEVICES][19]; |
57 | char addrs[MAX_BT_DEVICES][19]; |
| 33 | 58 | ||
| Line 41... | Line 66... | ||
| 41 | int x52_input_fd, *axis=NULL, num_of_axis=0, num_of_buttons=0, x; |
66 | int x52_input_fd, *axis=NULL, num_of_axis=0, num_of_buttons=0, x; |
| 42 | char *button=NULL,*button_trigger=NULL, name_of_joystick[80]; |
67 | char *button=NULL,*button_trigger=NULL, name_of_joystick[80]; |
| 43 | 68 | ||
| 44 | struct js_event x52_event_struct; |
69 | struct js_event x52_event_struct; |
| 45 | 70 | ||
| 46 | - | ||
| 47 | int engines_on=0; |
71 | int engines_on=0; |
| 48 | int old_engines_on=0; |
72 | int old_engines_on=0; |
| 49 | 73 | ||
| 50 | char in_char; |
74 | char in_char; |
| 51 | 75 | ||
| 52 | struct |
- | |
| 53 | { |
- | |
| 54 | char val[4]; |
- | |
| 55 | } MotortestParam; |
76 | struct ExternControl_s |
| 56 | - | ||
| 57 | struct |
- | |
| 58 | { |
77 | { |
| 59 | unsigned char Digital[2]; // (noch unbenutzt) |
78 | unsigned char Digital[2]; // (noch unbenutzt) |
| 60 | unsigned char RemoteTasten; //(gab es schon für das virtuelle Display) |
79 | unsigned char RemoteTasten; //(gab es schon für das virtuelle Display) |
| 61 | signed char Nick; |
80 | signed char Nick; |
| 62 | signed char Roll; |
81 | signed char Roll; |
| Line 64... | Line 83... | ||
| 64 | unsigned char Gas; //(es wird das Stick-Gas auf diesen Wert begrenzt; --> StickGas ist das Maximum) |
83 | unsigned char Gas; //(es wird das Stick-Gas auf diesen Wert begrenzt; --> StickGas ist das Maximum) |
| 65 | signed char Higt; //(Höhenregler) |
84 | signed char Higt; //(Höhenregler) |
| 66 | unsigned char free; // (unbenutzt) |
85 | unsigned char free; // (unbenutzt) |
| 67 | unsigned char Frame; // (Bestätigung) |
86 | unsigned char Frame; // (Bestätigung) |
| 68 | unsigned char Config; |
87 | unsigned char Config; |
| 69 | } ExternControl; |
88 | }; |
| 70 | 89 | ||
| 71 | 90 | ||
| - | 91 | struct ExternControl_s ExternControl ; |
|
| 72 | 92 | ||
| 73 | int state=STATEID_SCANNING; |
93 | int state=STATEID_SCANNING; |
| 74 | 94 | ||
| 75 | 95 | ||
| 76 | struct x52 *x52_output; |
96 | struct x52 *x52_output; |
| Line 118... | Line 138... | ||
| 118 | free( ii ); |
138 | free( ii ); |
| 119 | close( sock ); |
139 | close( sock ); |
| 120 | } |
140 | } |
| 121 | 141 | ||
| 122 | 142 | ||
| 123 | void connect_joy() |
143 | int connect_joy() |
| 124 | { |
144 | { |
| 125 | 145 | ||
| 126 | 146 | ||
| 127 | if( ( x52_input_fd = open( JOY_DEV, O_RDONLY ) ) < 0 ) |
147 | if( ( x52_input_fd = open( JOY_DEV, O_RDONLY ) ) < 0 ) |
| 128 | { |
148 | { |
| 129 | printf( "Couldn't open joystick device %s\n", JOY_DEV ); |
149 | printf( "Couldn't open joystick device %s\n", JOY_DEV ); |
| - | 150 | printf( "try modprobe joydev\n" ); |
|
| 130 | return ; |
151 | return 0; |
| 131 | } |
152 | } |
| 132 | 153 | ||
| 133 | ioctl( x52_input_fd, JSIOCGAXES, &num_of_axis ); |
154 | ioctl( x52_input_fd, JSIOCGAXES, &num_of_axis ); |
| 134 | ioctl( x52_input_fd, JSIOCGBUTTONS, &num_of_buttons ); |
155 | ioctl( x52_input_fd, JSIOCGBUTTONS, &num_of_buttons ); |
| 135 | ioctl( x52_input_fd, JSIOCGNAME(80), &name_of_joystick ); |
156 | ioctl( x52_input_fd, JSIOCGNAME(80), &name_of_joystick ); |
| Line 143... | Line 164... | ||
| 143 | , num_of_axis |
164 | , num_of_axis |
| 144 | , num_of_buttons ); |
165 | , num_of_buttons ); |
| 145 | 166 | ||
| 146 | fcntl( x52_input_fd, F_SETFL, O_NONBLOCK ); /* use non-blocking mode */ |
167 | fcntl( x52_input_fd, F_SETFL, O_NONBLOCK ); /* use non-blocking mode */ |
| 147 | 168 | ||
| - | 169 | return 1; |
|
| 148 | } |
170 | } |
| 149 | 171 | ||
| 150 | 172 | ||
| 151 | 173 | ||
| 152 | 174 | ||
| Line 164... | Line 186... | ||
| 164 | } |
186 | } |
| 165 | 187 | ||
| 166 | 188 | ||
| 167 | void SendOutData(unsigned char cmd,unsigned char modul, unsigned char *snd, unsigned char len) |
189 | void SendOutData(unsigned char cmd,unsigned char modul, unsigned char *snd, unsigned char len) |
| 168 | { |
190 | { |
| 169 | unsigned int pt = 0; |
191 | unsigned int pt = 0; |
| 170 | unsigned char a,b,c; |
192 | unsigned char a,b,c; |
| 171 | unsigned char ptr = 0; |
193 | unsigned char ptr = 0; |
| 172 | 194 | ||
| 173 | TxBuffer[pt++] = '#'; // Startzeichen |
195 | TxBuffer[pt++] = '#'; // Startzeichen |
| 174 | TxBuffer[pt++] = modul; // Adresse (a=0; b=1,...) |
196 | TxBuffer[pt++] = modul; // Adresse (a=0; b=1,...) |
| 175 | TxBuffer[pt++] = cmd; // Commando |
197 | TxBuffer[pt++] = cmd; // Commando |
| 176 | 198 | ||
| 177 | while(len) |
199 | while(len) |
| 178 | { |
200 | { |
| 179 | if(len) { a = snd[ptr++]; len--;} else a = 0; |
201 | if(len) { a = snd[ptr++]; len--;} else a = 0; |
| 180 | if(len) { b = snd[ptr++]; len--;} else b = 0; |
202 | if(len) { b = snd[ptr++]; len--;} else b = 0; |
| 181 | if(len) { c = snd[ptr++]; len--;} else c = 0; |
203 | if(len) { c = snd[ptr++]; len--;} else c = 0; |
| 182 | TxBuffer[pt++] = '=' + (a >> 2); |
204 | TxBuffer[pt++] = '=' + (a >> 2); |
| 183 | TxBuffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
205 | TxBuffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
| 184 | TxBuffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
206 | TxBuffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
| 185 | TxBuffer[pt++] = '=' + ( c & 0x3f); |
207 | TxBuffer[pt++] = '=' + ( c & 0x3f); |
| 186 | } |
208 | } |
| 187 | 209 | ||
| 188 | 210 | ||
| 189 | 211 | ||
| 190 | AddCRC(pt); |
212 | AddCRC(pt); |
| 191 | printf("Sending to MK %d \n" , pt); |
213 | printf("Sending to MK %d \n" , pt); |
| Line 193... | Line 215... | ||
| 193 | status = send(s,"\r" , 1, 0); |
215 | status = send(s,"\r" , 1, 0); |
| 194 | 216 | ||
| 195 | 217 | ||
| 196 | // for (c=0;c<pt+2;c++) |
218 | // for (c=0;c<pt+2;c++) |
| 197 | // { |
219 | // { |
| 198 | status = write(s,&TxBuffer , pt+3); |
220 | status = write(s,&TxBuffer , pt+3); |
| 199 | // printf("Send to MK %d \n" , TxBuffer[c] ); |
221 | // printf("Send to MK %d \n" , TxBuffer[c] ); |
| 200 | // } |
222 | // } |
| 201 | /*while(TxBuffer[i] !='\r' && i<150) |
223 | /*while(TxBuffer[i] !='\r' && i<150) |
| 202 | { |
224 | { |
| 203 | // TxBuffer[i]='#'; |
225 | // TxBuffer[i]='#'; |
| 204 | status = send(s,&TxBuffer[i] , 1, 0); |
226 | status = send(s,&TxBuffer[i] , 1, 0); |
| 205 | printf(" +%d%c ",i,TxBuffer[i]); |
227 | printf(" +%d%c ",i,TxBuffer[i]); |
| 206 | i++; |
228 | i++; |
| 207 | } |
229 | } |
| 208 | 230 | ||
| 209 | status = send(s,"\r" , 1, 0); |
231 | status = send(s,"\r" , 1, 0); |
| 210 | */ |
232 | */ |
| 211 | // status = send(s,"\r" , 1, 0); |
233 | // status = send(s,"\r" , 1, 0); |
| 212 | printf("\n"); |
234 | printf("\n"); |
| 213 | } |
235 | } |
| 214 | 236 | ||
| Line 242... | Line 264... | ||
| 242 | write_display(i,disp_txt); |
264 | write_display(i,disp_txt); |
| 243 | } |
265 | } |
| 244 | } |
266 | } |
| 245 | } |
267 | } |
| 246 | 268 | ||
| 247 | void connect_mk(char dest[18]) |
269 | int connect_mk(char dest[18]) |
| 248 | { |
270 | { |
| 249 | struct sockaddr_rc addr ; |
271 | struct sockaddr_rc addr ; |
| 250 | 272 | ||
| 251 | // allocate a socket |
273 | // allocate a socket |
| 252 | s = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM); |
274 | s = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM); |
| Line 257... | Line 279... | ||
| 257 | str2ba( dest, &addr.rc_bdaddr ); |
279 | str2ba( dest, &addr.rc_bdaddr ); |
| 258 | 280 | ||
| 259 | // connect to server |
281 | // connect to server |
| 260 | status = connect(s, (struct sockaddr *)&addr, sizeof(addr)); |
282 | status = connect(s, (struct sockaddr *)&addr, sizeof(addr)); |
| 261 | 283 | ||
| - | 284 | return status; |
|
| - | 285 | ||
| 262 | } |
286 | } |
| 263 | 287 | ||
| 264 | 288 | ||
| 265 | 289 | ||
| 266 | 290 | ||
| 267 | int r=0; |
291 | int r=0; |
| 268 | int count=0; |
292 | int count=0; |
| - | 293 | int connected=0; |
|
| 269 | 294 | ||
| 270 | int main(int argc, char**argv) |
295 | int main(int argc, char**argv) |
| 271 | { |
296 | { |
| 272 | printf("Starting Riddim \n"); |
297 | printf("Starting Riddim \n"); |
| 273 | printf("\tRemote Interactive Digital Drone Interface Mashup\n"); |
298 | printf("\tRemote Interactive Digital Drone Interface Mashup\n"); |
| 274 | 299 | ||
| - | 300 | // 1st argument -> Bluetooth adrees to bypass scanning ( takes to long for short testing roundtrips ) |
|
| 275 | //int tmp=2; |
301 | if (argv[1]) |
| - | 302 | { |
|
| 276 | connect_mk("00:0B:CE:01:6B:4F"); |
303 | if (connect_mk(argv[1])==-1) |
| 277 | 304 | { |
|
| - | 305 | printf("cant connect to QC at adress: %s\n",argv[1]); |
|
| 278 | 306 | return 0; |
|
| 279 | 307 | } |
|
| - | 308 | printf("connected to QC at adress: %s\n",argv[1]); |
|
| - | 309 | connected=1; |
|
| 280 | 310 | } |
|
| 281 | int i; |
311 | int i; |
| 282 | 312 | ||
| 283 | printf("\nInitializing X-52 input ..\n"); |
313 | printf("\nInitializing X-52 input ..\n"); |
| 284 | connect_joy(); |
314 | if (!connect_joy()) return 0; |
| 285 | printf(".. done"); |
315 | printf(".. done");// |
| - | 316 | ||
| 286 | 317 | ||
| 287 | printf("\nInitializing X-52 output .."); |
318 | printf("\nInitializing X-52 output .."); |
| 288 | 319 | ||
| 289 | x52_output = x52_init(); |
320 | x52_output = x52_init(); |
| 290 | 321 | ||
| Line 296... | Line 327... | ||
| 296 | if (x52_output) |
327 | if (x52_output) |
| 297 | printf(" done \n"); |
328 | printf(" done \n"); |
| 298 | else |
329 | else |
| 299 | printf(" not found \n"); |
330 | printf(" not found \n"); |
| 300 | 331 | ||
| 301 | printf("Scanning for Bluetooth Devices ..\n"); |
- | |
| 302 | write_display(1,"Bluetooth Scan"); |
- | |
| 303 | - | ||
| 304 | // scan_bt(); |
332 | if (!connected) |
| 305 | printf(" done \n"); |
- | |
| 306 | printf(" %d Devices found \n",bt_device_count); |
- | |
| 307 | - | ||
| 308 | - | ||
| 309 | /* for(i=0;i<bt_device_count;i++) |
- | |
| 310 | { |
333 | { |
| 311 | printf(" %d -> %s (%s) \n",i,names[i],addrs[i]); |
334 | printf("Scanning for Bluetooth Devices ..\n"); |
| 312 | if (i<3) write_display(i,names[i]); |
335 | write_display(1,"Bluetooth Scan"); |
| - | 336 | scan_bt(); |
|
| - | 337 | printf(" done \n"); |
|
| - | 338 | printf(" %d Devices found \n",bt_device_count); |
|
| 313 | } |
339 | } |
| 314 | |
340 | |
| 315 | output_device_list() ; |
- | |
| 316 | */ |
- | |
| 317 | 341 | ||
| 318 | int v_old; |
342 | int v_old; |
| 319 | while( 1 ) |
343 | while( 1 ) |
| 320 | { |
344 | { |
| - | 345 | // poll values from input device |
|
| 321 | int polls=0; |
346 | int polls=0; |
| 322 | for (polls=0;polls<1000;polls++) // FIXME - better Polling |
347 | for (polls=0;polls<100;polls++) // FIXME - better Polling |
| 323 | { |
348 | { |
| 324 | read(x52_input_fd, &x52_event_struct, sizeof(struct js_event)); |
349 | read(x52_input_fd, &x52_event_struct, sizeof(struct js_event)); |
| 325 | 350 | ||
| 326 | 351 | ||
| 327 | /* see what to do with the event */ |
352 | /* see what to do with the event */ |
| 328 | switch (x52_event_struct.type & ~JS_EVENT_INIT) |
353 | switch (x52_event_struct.type & ~JS_EVENT_INIT) |
| 329 | { |
354 | { |
| 330 | case JS_EVENT_AXIS: |
355 | case JS_EVENT_AXIS: |
| 331 | axis [ x52_event_struct.number ] = x52_event_struct.value; |
356 | axis [ x52_event_struct.number ] = x52_event_struct.value; |
| 332 | break; |
357 | break; |
| 333 | case JS_EVENT_BUTTON: |
358 | case JS_EVENT_BUTTON: |
| 334 | button [ x52_event_struct.number ] = x52_event_struct.value; |
359 | button [ x52_event_struct.number ] = x52_event_struct.value; |
| 335 | break; |
360 | break; |
| 336 | } |
361 | } |
| 337 | } |
362 | } |
| 338 | 363 | ||
| 339 | for( x=0 ; x<num_of_buttons ; ++x ) |
364 | for( x=0 ; x<num_of_buttons ; ++x ) |
| 340 | if( button[x]==0) |
365 | if( button[x]==0) |
| 341 | button_trigger[x]=0; |
366 | button_trigger[x]=0; |
| Line 350... | Line 375... | ||
| 350 | { |
375 | { |
| 351 | 376 | ||
| 352 | 377 | ||
| 353 | case STATEID_SCANNING: |
378 | case STATEID_SCANNING: |
| 354 | 379 | ||
| 355 | state=STATEID_CONNECTING; //e |
380 | state=STATEID_CONNECTING; |
| 356 | 381 | ||
| 357 | ExternControl.Digital[0]=0; |
382 | ExternControl.Digital[0]=0; |
| 358 | ExternControl.Digital[1]=0; |
383 | ExternControl.Digital[1]=0; |
| 359 | ExternControl.RemoteTasten=0; |
384 | ExternControl.RemoteTasten=0; |
| 360 | ExternControl.Nick=(axis[1]>>8)*(-1)+127;; |
385 | ExternControl.Nick=(axis[1]>>8)*(-1)+127;; |
| Line 366... | Line 391... | ||
| 366 | ExternControl.Higt=0; |
391 | ExternControl.Higt=0; |
| 367 | ExternControl.free=0; |
392 | ExternControl.free=0; |
| 368 | ExternControl.Frame='t'; |
393 | ExternControl.Frame='t'; |
| 369 | ExternControl.Config=1; |
394 | ExternControl.Config=1; |
| 370 | 395 | ||
| 371 | - | ||
| 372 | - | ||
| 373 | printf("sending data\n"); |
396 | printf("sending data\n"); |
| 374 | 397 | ||
| 375 | SendOutData('b', 0, &ExternControl, sizeof(ExternControl)); |
- | |
| 376 | 398 | ||
| - | 399 | SendOutData('b', 0, (unsigned char *)&ExternControl, sizeof(ExternControl)); |
|
| - | 400 | gettimeofday(&time_struct1,NULL); |
|
| 377 | 401 | ||
| 378 | if (button_trigger[BUTTON_SELECT]==1) |
402 | if (button_trigger[BUTTON_SELECT]==1) |
| 379 | { |
403 | { |
| 380 | state=STATEID_CONNECTING; |
404 | state=STATEID_CONNECTING; |
| 381 | clear_display(); |
405 | clear_display(); |
| Line 396... | Line 420... | ||
| 396 | output_device_list() ; |
420 | output_device_list() ; |
| 397 | } |
421 | } |
| 398 | break; |
422 | break; |
| 399 | 423 | ||
| 400 | case STATEID_CONNECTING: |
424 | case STATEID_CONNECTING: |
| 401 | // for (polls=0;polls<10;polls++) // FIXME - better Polling |
- | |
| - | 425 | ||
| 402 | RxBuffer[1]=0; |
426 | RxBuffer[1]=0; |
| 403 | while (RxBuffer[1]!='t') |
- | |
| 404 | { |
- | |
| 405 | 427 | ||
| 406 | r=0; |
- | |
| 407 | in_char='#'; |
- | |
| 408 | 428 | ||
| - | 429 | // ftime(&time_struct); |
|
| - | 430 | //printf("t:%d",time_struct.millitm); |
|
| 409 | while(in_char!='\r') |
431 | while (RxBuffer[1]!='t') |
| 410 | { |
432 | { |
| - | 433 | ||
| 411 | count=read(s,&in_char,1); |
434 | r=0; |
| 412 | if (in_char!=0) |
435 | in_char='#'; |
| 413 | { |
436 | |
| 414 | RxBuffer[r++]=in_char; |
437 | while(in_char!='\r') |
| 415 | } |
- | |
| 416 | else |
- | |
| 417 | { |
438 | { |
| - | 439 | count=read(s,&in_char,1); |
|
| - | 440 | if (in_char!=0) |
|
| - | 441 | { |
|
| - | 442 | RxBuffer[r++]=in_char; |
|
| - | 443 | } |
|
| - | 444 | else |
|
| - | 445 | { |
|
| 418 | RxBuffer[r++]='0'; |
446 | RxBuffer[r++]='0'; |
| - | 447 | } |
|
| - | 448 | // printf("count:%d r:%d %d %c \n",count , r, in_char, in_char); |
|
| 419 | } |
449 | } |
| - | 450 | RxBuffer[r++]='\0'; |
|
| 420 | // printf("count:%d r:%d %d %c \n",count , r, in_char, in_char); |
451 | printf("--->%s\n",RxBuffer); |
| - | 452 | ||
| 421 | } |
453 | } |
| - | 454 | gettimeofday(&time_struct2,NULL); |
|
| - | 455 | ||
| - | 456 | printf("last trip: %d",(int)(time_struct1.tv_usec-time_struct2.tv_usec)); |
|
| - | 457 | act_mode=button[24] | (button[25]<<1); |
|
| - | 458 | ||
| 422 | RxBuffer[r++]='\0'; |
459 | switch (act_mode) |
| - | 460 | { |
|
| - | 461 | case 0: |
|
| - | 462 | act_nick=(axis[AXIS_NICK]>>8)*(INVERT_NICK); |
|
| - | 463 | act_roll=(axis[AXIS_ROLL]>>8)*(INVERT_ROLL); |
|
| - | 464 | act_gier=(axis[AXIS_GIER]>>8)*(INVERT_GIER); |
|
| - | 465 | act_gas=(axis[AXIS_GAS]>>8)*(INVERT_GAS); |
|
| - | 466 | ||
| - | 467 | break; |
|
| - | 468 | ||
| - | 469 | case 1: |
|
| - | 470 | act_nick=(axis[AXIS_NICK]>>8)*(INVERT_NICK)/2; |
|
| - | 471 | act_roll=(axis[AXIS_ROLL]>>8)*(INVERT_ROLL)/2; |
|
| - | 472 | act_gier=(axis[AXIS_GIER]>>8)*(INVERT_GIER)/2; |
|
| - | 473 | act_gas=(axis[AXIS_GAS]>>8)*(INVERT_GAS); |
|
| - | 474 | ||
| - | 475 | break; |
|
| - | 476 | ||
| - | 477 | case 2: |
|
| - | 478 | act_nick=(axis[AXIS_NICK]>>8)*(INVERT_NICK)/3; |
|
| - | 479 | act_roll=(axis[AXIS_ROLL]>>8)*(INVERT_ROLL)/3; |
|
| - | 480 | act_gier=(axis[AXIS_GIER]>>8)*(INVERT_GIER)/3; |
|
| 423 | printf("--->%s\n",RxBuffer); |
481 | act_gas=(axis[AXIS_GAS]>>8)*(INVERT_GAS); |
| 424 | 482 | ||
| 425 | } |
- | |
| 426 | 483 | ||
| - | 484 | break; |
|
| - | 485 | ||
| - | 486 | } |
|
| - | 487 | ||
| 427 | ExternControl.Digital[0]=0; |
488 | ExternControl.Digital[0]=0; |
| 428 | ExternControl.Digital[1]=0; |
489 | ExternControl.Digital[1]=0; |
| 429 | ExternControl.RemoteTasten=0; |
490 | ExternControl.RemoteTasten=0; |
| 430 | ExternControl.Nick=(axis[1]>>8)*(-1); |
491 | ExternControl.Nick=act_nick; //(axis[1]>>8)*(-1)/2; |
| 431 | printf("nick%d\n",ExternControl.Nick); |
492 | // printf("nick%d\n",ExternControl.Nick); |
| 432 | ExternControl.Roll=(axis[0]>>8)*(-1); |
493 | ExternControl.Roll=act_roll; //(axis[0]>>8)*(-1)/2; |
| 433 | ExternControl.Gier=(axis[5]>>8); |
494 | ExternControl.Gier=(axis[5]>>8); |
| 434 | ExternControl.Gas=(axis[2]>>8)*(-1)+127; |
495 | ExternControl.Gas=(axis[2]>>8)*(-1)+127; |
| 435 | ExternControl.Higt=0; |
496 | ExternControl.Higt=0; |
| 436 | ExternControl.free=0; |
497 | ExternControl.free=0; |
| 437 | ExternControl.Frame='t'; |
498 | ExternControl.Frame='t'; |
| - | 499 | ||
| 438 | ExternControl.Config=1; |
500 | ExternControl.Config=1; |
| - | 501 | printf("act_mode %d , act_nick %d , act_roll %d , act_gier %d , act_gas %d",act_mode , act_nick , act_roll , act_gier , act_gas); |
|
| 439 | 502 | ||
| 440 | 503 | ||
| 441 | 504 | ||
| 442 | printf("sending data\n"); |
505 | printf("sending data\n"); |
| 443 | 506 | ||
| 444 | SendOutData('b', 0, &ExternControl, sizeof(ExternControl)); |
507 | SendOutData('b', 0, (unsigned char *)&ExternControl, sizeof(ExternControl)); |
| 445 | - | ||
| - | 508 | gettimeofday(&time_struct1,NULL); |
|
| 446 | printf("sent data\n"); |
509 | printf("sent data\n"); |
| 447 | 510 | ||
| 448 | 511 | ||
| 449 | printf("sleeping\n"); |
512 | // printf("sleeping\n"); |
| 450 | // usleep(10000); |
513 | // usleep(10000); |
| 451 | - | ||
| 452 | printf("end_sleep\n"); |
514 | // printf("end_sleep\n"); |
| 453 | 515 | ||
| 454 | int v=axis[6]/655+50; |
516 | int v=axis[6]/655+50; |
| 455 | if (v!=v_old)if (x52_output) x52_setbri(x52_output, 0,v ); |
517 | if (v!=v_old)if (x52_output) x52_setbri(x52_output, 0,v ); |
| 456 | v_old=v; |
518 | v_old=v; |
| 457 | 519 | ||
| Line 470... | Line 532... | ||
| 470 | 532 | ||
| 471 | printf("\n"); |
533 | printf("\n"); |
| 472 | fflush(stdout); |
534 | fflush(stdout); |
| 473 | printf("loop fin"); |
535 | printf("loop fin"); |
| 474 | 536 | ||
| 475 | } |
537 | } |
| 476 | 538 | ||
| 477 | 539 | ||
| 478 | /******************** Cleanup **********************/ |
540 | /******************** Cleanup **********************/ |
| 479 | close(x52_input_fd); |
541 | close(x52_input_fd); |
| 480 | close(s); |
542 | close(s); |