Subversion Repositories Projects

Rev

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