Subversion Repositories Projects

Rev

Rev 48 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
41 ligi 1
#include <stdio.h>
2
#include <stdlib.h>
3
#include <string.h>
4
#include <fcntl.h>
5
#include "lib/x52/x52.h"
6
#include <unistd.h>
7
#include <sys/socket.h>
8
#include <bluetooth/bluetooth.h>
9
#include <bluetooth/hci.h>
10
#include <bluetooth/hci_lib.h>
11
 
50 ligi 12
#include <sys/time.h>
41 ligi 13
 
50 ligi 14
 
43 ligi 15
#include <bluetooth/rfcomm.h>
41 ligi 16
 
43 ligi 17
 
41 ligi 18
#include <linux/joystick.h>
19
#define JOY_DEV "/dev/input/js0"
20
 
21
#define MAX_BT_DEVICES 3
22
 
47 ligi 23
 
24
#define STATEID_SCANNING 0
25
#define STATEID_CONNECTING 1
26
 
27
#define BUTTON_SELECT 26
28
#define BUTTON_DOWN 28
29
#define BUTTON_UP 27
30
 
50 ligi 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
 
41 ligi 54
int bt_device_count=0;
55
 
56
char names[MAX_BT_DEVICES][248];
57
char addrs[MAX_BT_DEVICES][19];
58
 
43 ligi 59
int s, status;
60
unsigned char TxBuffer[150];
61
unsigned char _TxBuffer[150];
62
 
48 ligi 63
char RxBuffer[150];
64
 
65
 
47 ligi 66
int x52_input_fd, *axis=NULL, num_of_axis=0, num_of_buttons=0, x;
67
char *button=NULL,*button_trigger=NULL, name_of_joystick[80];
68
 
69
struct js_event x52_event_struct;
70
 
48 ligi 71
int engines_on=0;
72
int old_engines_on=0;
73
 
74
char in_char;
75
 
50 ligi 76
struct ExternControl_s
47 ligi 77
{
78
  unsigned char Digital[2];   // (noch unbenutzt)
79
  unsigned char RemoteTasten; //(gab es schon für das virtuelle Display)
80
  signed char   Nick;
81
  signed char   Roll;
82
  signed char   Gier;
83
  unsigned char Gas;          //(es wird das Stick-Gas auf diesen Wert begrenzt; --> StickGas ist das Maximum)
48 ligi 84
  signed char   Higt;        //(Höhenregler)
47 ligi 85
  unsigned char free;         // (unbenutzt)
86
  unsigned char Frame;        // (Bestätigung)
87
  unsigned char Config;
50 ligi 88
};
47 ligi 89
 
90
 
50 ligi 91
struct ExternControl_s  ExternControl ;
47 ligi 92
 
43 ligi 93
int state=STATEID_SCANNING;
94
 
95
 
96
struct x52 *x52_output;
97
 
98
int selected_bt_device=0;
41 ligi 99
void scan_bt()
100
{
101
  inquiry_info *ii = NULL;
102
 
103
  int dev_id, sock, len, flags;
104
  int i;
105
  char addr[19] = { 0 };
106
  char name[248] = { 0 };
107
 
108
  dev_id = hci_get_route(NULL);
109
  sock = hci_open_dev( dev_id );
110
  if (dev_id < 0 || sock < 0) {
111
    perror("opening socket");
112
    exit(1);
113
  }
114
 
115
  len  = 8;
116
 
117
  flags = IREQ_CACHE_FLUSH;
118
  ii = (inquiry_info*)malloc(MAX_BT_DEVICES * sizeof(inquiry_info));
119
 
120
  bt_device_count = hci_inquiry(dev_id, len, MAX_BT_DEVICES, NULL, &ii, flags);
121
  if(  bt_device_count < 0 ) perror("hci_inquiry");
122
 
123
  for (i = 0; i <  bt_device_count; i++) {
124
    ba2str(&(ii+i)->bdaddr, addr);
125
    sprintf(addrs[i],"%s",addr);
126
 
127
    memset(name, 0, sizeof(name));
128
 
129
    if (hci_read_remote_name(sock, &(ii+i)->bdaddr, sizeof(name),
130
                             name, 0) < 0)
131
      sprintf(names[i],"[unknown]");
132
    else
133
      sprintf(names[i],"%s",name);
134
 
135
  }
136
 
137
 
138
  free( ii );
139
  close( sock );
140
}
141
 
142
 
50 ligi 143
int connect_joy()
43 ligi 144
{
41 ligi 145
 
43 ligi 146
 
147
  if( ( x52_input_fd = open( JOY_DEV, O_RDONLY ) ) < 0 )
148
    {
149
      printf( "Couldn't open joystick device %s\n", JOY_DEV );
50 ligi 150
      printf( "try modprobe joydev\n"  );
151
      return 0;
43 ligi 152
    }
153
 
154
  ioctl( x52_input_fd, JSIOCGAXES, &num_of_axis );
155
  ioctl( x52_input_fd, JSIOCGBUTTONS, &num_of_buttons );
156
  ioctl( x52_input_fd, JSIOCGNAME(80), &name_of_joystick );
157
 
158
  axis = (int *) calloc( num_of_axis, sizeof( int ) );
159
  button = (char *)calloc( num_of_buttons, sizeof( char ) );
160
  button_trigger = (char *) calloc( num_of_buttons, sizeof( char ) );
161
 
162
  printf("Joystick detected: %s\n\t%d axis\n\t%d buttons\n\n"
163
         , name_of_joystick
164
         , num_of_axis
165
         , num_of_buttons );
166
 
167
  fcntl( x52_input_fd, F_SETFL, O_NONBLOCK );   /* use non-blocking mode */
168
 
50 ligi 169
  return 1;
43 ligi 170
}
171
 
172
 
41 ligi 173
 
174
 
43 ligi 175
void AddCRC(unsigned int wieviele)
176
{
177
  unsigned int tmpCRC = 0,i;
178
  for(i = 0; i < wieviele;i++)
179
    {
180
      tmpCRC += TxBuffer[i];
181
    }
182
  tmpCRC %= 4096;
183
  TxBuffer[i++] = '=' + tmpCRC / 64;
184
  TxBuffer[i++] = '=' + tmpCRC % 64;
185
  TxBuffer[i++] = '\r';
186
}
41 ligi 187
 
48 ligi 188
 
189
void SendOutData(unsigned char cmd,unsigned char modul, unsigned char *snd, unsigned char len)
43 ligi 190
{
50 ligi 191
  unsigned int pt = 0;
192
  unsigned char a,b,c;
193
  unsigned char ptr = 0;
43 ligi 194
 
50 ligi 195
  TxBuffer[pt++] = '#';               // Startzeichen
196
  TxBuffer[pt++] = modul;             // Adresse (a=0; b=1,...)
197
  TxBuffer[pt++] = cmd;                 // Commando
48 ligi 198
 
50 ligi 199
  while(len)
200
    {
201
      if(len) { a = snd[ptr++]; len--;} else a = 0;
202
      if(len) { b = snd[ptr++]; len--;} else b = 0;
203
      if(len) { c = snd[ptr++]; len--;} else c = 0;
204
      TxBuffer[pt++] = '=' + (a >> 2);
205
      TxBuffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
206
      TxBuffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
207
      TxBuffer[pt++] = '=' + ( c & 0x3f);
208
    }
43 ligi 209
 
48 ligi 210
 
211
 
212
  AddCRC(pt);
213
  printf("Sending to MK %d \n" , pt);
214
 
43 ligi 215
  status = send(s,"\r" , 1, 0);
48 ligi 216
 
217
 
218
  //  for (c=0;c<pt+2;c++)
219
  // {
50 ligi 220
  status = write(s,&TxBuffer , pt+3);
221
  //   printf("Send to MK %d \n" , TxBuffer[c] );
222
  // }
48 ligi 223
  /*while(TxBuffer[i] !='\r' && i<150)
43 ligi 224
    {
50 ligi 225
    //     TxBuffer[i]='#';
226
    status = send(s,&TxBuffer[i] , 1, 0);
227
    printf(" +%d%c ",i,TxBuffer[i]);
228
    i++;
43 ligi 229
    }
48 ligi 230
 
50 ligi 231
    status = send(s,"\r" , 1, 0);
48 ligi 232
  */
233
  // status = send(s,"\r" , 1, 0);
43 ligi 234
  printf("\n");
235
}
41 ligi 236
 
237
 
43 ligi 238
void write_display(int line,char* text)
239
{
240
  if (x52_output) x52_settext(x52_output, line , text, strlen(text));
241
}
242
 
243
void clear_display()
244
{
245
  write_display(0,"");
246
  write_display(1,"");
247
  write_display(2,"");
248
}
249
 
250
 
251
void output_device_list()
252
{
253
  int i;
254
  char disp_txt[20];
255
  for(i=0;i<bt_device_count;i++)
256
    {
257
      if (i<3)
258
        {
259
 
260
          if (selected_bt_device==i)
261
            sprintf(disp_txt,"#%s",names[i]);
262
          else
263
            sprintf(disp_txt," %s",names[i]);
264
          write_display(i,disp_txt);
265
        }
266
    }
267
}
268
 
50 ligi 269
int connect_mk(char dest[18])
43 ligi 270
{
271
  struct sockaddr_rc addr ;
272
 
273
  // allocate a socket
274
  s = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
275
 
276
  // set the connection parameters (who to connect to)
277
  addr.rc_family = AF_BLUETOOTH;
278
  addr.rc_channel = 1;
279
  str2ba( dest, &addr.rc_bdaddr );
280
 
281
  // connect to server
282
  status = connect(s, (struct sockaddr *)&addr, sizeof(addr));
283
 
50 ligi 284
  return status;
285
 
43 ligi 286
}
287
 
288
 
289
 
290
 
50 ligi 291
int r=0;
292
int count=0;
293
int connected=0;
43 ligi 294
 
295
int main(int argc, char**argv)
296
{
297
  printf("Starting Riddim \n");
298
  printf("\tRemote Interactive Digital Drone Interface Mashup\n");
50 ligi 299
 
300
  // 1st argument -> Bluetooth adrees to bypass scanning ( takes to long for short testing roundtrips )
301
  if (argv[1])
302
    {
303
      if (connect_mk(argv[1])==-1)
304
        {
305
          printf("cant connect to QC at adress: %s\n",argv[1]);
306
          return 0;
307
        }
308
        printf("connected to QC at adress: %s\n",argv[1]);
309
        connected=1;
310
    }
47 ligi 311
  int i;
312
 
43 ligi 313
  printf("\nInitializing X-52 input ..\n");
50 ligi 314
  if (!connect_joy()) return 0;
315
  printf(".. done");//
43 ligi 316
 
50 ligi 317
 
43 ligi 318
  printf("\nInitializing X-52 output ..");
319
 
47 ligi 320
  x52_output = x52_init();
321
 
322
  clear_display();
43 ligi 323
 
47 ligi 324
  write_display(0, "RIDDIM active");
43 ligi 325
 
47 ligi 326
  if (x52_output) x52_setbri(x52_output, 1,128);  
327
  if (x52_output)
328
    printf(" done \n");  
329
  else
330
    printf(" not found \n");      
43 ligi 331
 
50 ligi 332
  if (!connected)
43 ligi 333
    {
50 ligi 334
      printf("Scanning for Bluetooth Devices ..\n");
335
      write_display(1,"Bluetooth Scan");
336
      scan_bt();
337
      printf(" done \n");  
338
      printf(" %d Devices found \n",bt_device_count);  
43 ligi 339
    }
41 ligi 340
 
50 ligi 341
 
43 ligi 342
  int v_old;
41 ligi 343
  while( 1 )    
344
    {
50 ligi 345
      // poll values from input device
43 ligi 346
      int polls=0;
50 ligi 347
      for (polls=0;polls<100;polls++) // FIXME - better Polling
43 ligi 348
        {
50 ligi 349
          read(x52_input_fd, &x52_event_struct, sizeof(struct js_event));
43 ligi 350
 
351
 
50 ligi 352
          /* see what to do with the event */
353
          switch (x52_event_struct.type & ~JS_EVENT_INIT)
354
            {
355
            case JS_EVENT_AXIS:
356
              axis   [ x52_event_struct.number ] = x52_event_struct.value;
357
              break;
358
            case JS_EVENT_BUTTON:
359
              button [ x52_event_struct.number ] = x52_event_struct.value;
360
              break;
361
            }
43 ligi 362
        }
47 ligi 363
 
364
      for( x=0 ; x<num_of_buttons ; ++x )
365
        if( button[x]==0)
366
          button_trigger[x]=0;
367
        else
368
          {
369
            if (button_trigger[x]<100)button_trigger[x]++;
370
          }
43 ligi 371
 
372
 
373
 
47 ligi 374
      switch(state)
375
        {
48 ligi 376
 
377
 
47 ligi 378
        case STATEID_SCANNING:
48 ligi 379
 
50 ligi 380
          state=STATEID_CONNECTING;
48 ligi 381
 
382
          ExternControl.Digital[0]=0;
383
          ExternControl.Digital[1]=0;
384
          ExternControl.RemoteTasten=0;
385
          ExternControl.Nick=(axis[1]>>8)*(-1)+127;;
386
 
387
          printf("nick%d\n",ExternControl.Nick);         
388
          ExternControl.Roll=(axis[0]>>8)*(-1)+127;;
389
          ExternControl.Gier=(axis[5]>>8)*(-1)+127;;
390
          ExternControl.Gas=(axis[2]>>8)*(-1)+127;
391
          ExternControl.Higt=0;
392
          ExternControl.free=0;
393
          ExternControl.Frame='t';
394
          ExternControl.Config=1;
395
 
396
          printf("sending data\n");
397
 
50 ligi 398
 
399
          SendOutData('b', 0, (unsigned char *)&ExternControl, sizeof(ExternControl));
400
          gettimeofday(&time_struct1,NULL);
48 ligi 401
 
47 ligi 402
          if (button_trigger[BUTTON_SELECT]==1)
403
            {
404
              state=STATEID_CONNECTING;
405
              clear_display();
406
              write_display(0,"connecting to");
407
              write_display(1,names[selected_bt_device]);
408
              connect_mk(addrs[selected_bt_device]);
409
              write_display(0,"connected to");
410
            }
43 ligi 411
 
47 ligi 412
          if ((button_trigger[BUTTON_UP]+button_trigger[BUTTON_DOWN])==1)
413
            {
414
              printf("-> sel_dev %d - %d\n",selected_bt_device,button_trigger[19]);
415
              if (button_trigger[BUTTON_DOWN]==1)
416
                if (selected_bt_device>0) selected_bt_device--;
417
              if (button_trigger[BUTTON_UP]==1)
418
                if (selected_bt_device<bt_device_count-1) selected_bt_device++;
419
 
420
              output_device_list()                ;
421
            }
422
          break;
48 ligi 423
 
47 ligi 424
        case STATEID_CONNECTING:
50 ligi 425
 
48 ligi 426
          RxBuffer[1]=0;
50 ligi 427
 
428
 
429
          //      ftime(&time_struct);
430
          //printf("t:%d",time_struct.millitm);
48 ligi 431
          while (RxBuffer[1]!='t')
50 ligi 432
            {
43 ligi 433
 
50 ligi 434
              r=0;
435
              in_char='#';
43 ligi 436
 
50 ligi 437
              while(in_char!='\r')
48 ligi 438
                {
50 ligi 439
                  count=read(s,&in_char,1);
440
                  if (in_char!=0)
441
                    {
442
                      RxBuffer[r++]=in_char;
443
                    }
444
                  else
445
                    {
446
                      RxBuffer[r++]='0';
447
                    }
448
                  //  printf("count:%d r:%d %d %c \n",count , r, in_char, in_char);
48 ligi 449
                }
50 ligi 450
              RxBuffer[r++]='\0';
451
              printf("--->%s\n",RxBuffer);
452
 
48 ligi 453
            }
50 ligi 454
          gettimeofday(&time_struct2,NULL);
43 ligi 455
 
50 ligi 456
          printf("last trip: %d",(int)(time_struct1.tv_usec-time_struct2.tv_usec));
457
          act_mode=button[24] | (button[25]<<1);
43 ligi 458
 
50 ligi 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;
481
              act_gas=(axis[AXIS_GAS]>>8)*(INVERT_GAS);
482
 
483
 
484
              break;
485
 
486
            }
487
 
48 ligi 488
          ExternControl.Digital[0]=0;
489
          ExternControl.Digital[1]=0;
490
          ExternControl.RemoteTasten=0;
50 ligi 491
          ExternControl.Nick=act_nick;  //(axis[1]>>8)*(-1)/2;
492
          //      printf("nick%d\n",ExternControl.Nick);         
493
          ExternControl.Roll=act_roll; //(axis[0]>>8)*(-1)/2;
48 ligi 494
          ExternControl.Gier=(axis[5]>>8);
495
          ExternControl.Gas=(axis[2]>>8)*(-1)+127;
496
          ExternControl.Higt=0;
497
          ExternControl.free=0;
498
          ExternControl.Frame='t';
50 ligi 499
 
48 ligi 500
          ExternControl.Config=1;
50 ligi 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);
48 ligi 502
 
503
 
504
 
505
          printf("sending data\n");
506
 
50 ligi 507
          SendOutData('b', 0, (unsigned char *)&ExternControl, sizeof(ExternControl));
508
          gettimeofday(&time_struct1,NULL);
48 ligi 509
          printf("sent data\n");
510
 
511
 
50 ligi 512
          // printf("sleeping\n");
513
          // usleep(10000);
514
          // printf("end_sleep\n");
48 ligi 515
 
47 ligi 516
          int v=axis[6]/655+50;
517
          if (v!=v_old)if (x52_output) x52_setbri(x52_output, 0,v );  
518
          v_old=v;
519
 
48 ligi 520
          printf("v: %d \n",v);
43 ligi 521
 
522
 
47 ligi 523
          for (i=0;i<num_of_axis;i++)
524
            printf("A%d: %d  ", i,axis[i]>>8 );
525
 
526
          for( x=0 ; x<num_of_buttons ; ++x )
527
 
528
            printf("B%d: %d  ", x, button[x] );            
43 ligi 529
 
47 ligi 530
          break;
531
        }
532
 
48 ligi 533
      printf("\n");
47 ligi 534
      fflush(stdout);
48 ligi 535
      printf("loop fin");
43 ligi 536
 
50 ligi 537
    }
41 ligi 538
 
43 ligi 539
 
47 ligi 540
  /******************** Cleanup **********************/
541
  close(x52_input_fd);
542
  close(s);
41 ligi 543
 
544
  if (x52_output) x52_close(x52_output);
545
  return 0;
546
}