Subversion Repositories Projects

Rev

Rev 48 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 48 Rev 50
1
#include <stdio.h>
1
#include <stdio.h>
2
#include <stdlib.h>
2
#include <stdlib.h>
3
#include <string.h>
3
#include <string.h>
4
#include <fcntl.h>
4
#include <fcntl.h>
5
#include "lib/x52/x52.h"
5
#include "lib/x52/x52.h"
6
#include <unistd.h>
6
#include <unistd.h>
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
 
-
 
12
#include <sys/time.h>
11
 
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>
17
#define JOY_DEV "/dev/input/js0"
19
#define JOY_DEV "/dev/input/js0"
18
 
20
 
19
#define MAX_BT_DEVICES 3
21
#define MAX_BT_DEVICES 3
20
 
22
 
21
 
23
 
22
#define STATEID_SCANNING 0
24
#define STATEID_SCANNING 0
23
#define STATEID_CONNECTING 1
25
#define STATEID_CONNECTING 1
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
-
 
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;
28
 
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
 
34
int s, status;
59
int s, status;
35
unsigned char TxBuffer[150];
60
unsigned char TxBuffer[150];
36
unsigned char _TxBuffer[150];
61
unsigned char _TxBuffer[150];
37
 
62
 
38
char RxBuffer[150];
63
char RxBuffer[150];
39
 
64
 
40
 
65
 
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
 
-
 
46
 
70
 
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
 
-
 
52
struct
-
 
53
{
-
 
54
  char val[4];
75
 
55
} MotortestParam;
-
 
56
 
-
 
57
struct
76
struct ExternControl_s
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;
63
  signed char   Gier;
82
  signed char   Gier;
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
};
-
 
89
 
70
 
90
 
71
 
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;
77
 
97
 
78
int selected_bt_device=0;
98
int selected_bt_device=0;
79
void scan_bt()
99
void scan_bt()
80
{
100
{
81
  inquiry_info *ii = NULL;
101
  inquiry_info *ii = NULL;
82
 
102
 
83
  int dev_id, sock, len, flags;
103
  int dev_id, sock, len, flags;
84
  int i;
104
  int i;
85
  char addr[19] = { 0 };
105
  char addr[19] = { 0 };
86
  char name[248] = { 0 };
106
  char name[248] = { 0 };
87
 
107
 
88
  dev_id = hci_get_route(NULL);
108
  dev_id = hci_get_route(NULL);
89
  sock = hci_open_dev( dev_id );
109
  sock = hci_open_dev( dev_id );
90
  if (dev_id < 0 || sock < 0) {
110
  if (dev_id < 0 || sock < 0) {
91
    perror("opening socket");
111
    perror("opening socket");
92
    exit(1);
112
    exit(1);
93
  }
113
  }
94
 
114
 
95
  len  = 8;
115
  len  = 8;
96
 
116
 
97
  flags = IREQ_CACHE_FLUSH;
117
  flags = IREQ_CACHE_FLUSH;
98
  ii = (inquiry_info*)malloc(MAX_BT_DEVICES * sizeof(inquiry_info));
118
  ii = (inquiry_info*)malloc(MAX_BT_DEVICES * sizeof(inquiry_info));
99
 
119
 
100
  bt_device_count = hci_inquiry(dev_id, len, MAX_BT_DEVICES, NULL, &ii, flags);
120
  bt_device_count = hci_inquiry(dev_id, len, MAX_BT_DEVICES, NULL, &ii, flags);
101
  if(  bt_device_count < 0 ) perror("hci_inquiry");
121
  if(  bt_device_count < 0 ) perror("hci_inquiry");
102
 
122
 
103
  for (i = 0; i <  bt_device_count; i++) {
123
  for (i = 0; i <  bt_device_count; i++) {
104
    ba2str(&(ii+i)->bdaddr, addr);
124
    ba2str(&(ii+i)->bdaddr, addr);
105
    sprintf(addrs[i],"%s",addr);
125
    sprintf(addrs[i],"%s",addr);
106
 
126
 
107
    memset(name, 0, sizeof(name));
127
    memset(name, 0, sizeof(name));
108
   
128
   
109
    if (hci_read_remote_name(sock, &(ii+i)->bdaddr, sizeof(name),
129
    if (hci_read_remote_name(sock, &(ii+i)->bdaddr, sizeof(name),
110
                             name, 0) < 0)
130
                             name, 0) < 0)
111
      sprintf(names[i],"[unknown]");
131
      sprintf(names[i],"[unknown]");
112
    else
132
    else
113
      sprintf(names[i],"%s",name);
133
      sprintf(names[i],"%s",name);
114
 
134
 
115
  }
135
  }
116
 
136
 
117
 
137
 
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 );
136
 
157
 
137
  axis = (int *) calloc( num_of_axis, sizeof( int ) );
158
  axis = (int *) calloc( num_of_axis, sizeof( int ) );
138
  button = (char *)calloc( num_of_buttons, sizeof( char ) );
159
  button = (char *)calloc( num_of_buttons, sizeof( char ) );
139
  button_trigger = (char *) calloc( num_of_buttons, sizeof( char ) );
160
  button_trigger = (char *) calloc( num_of_buttons, sizeof( char ) );
140
 
161
 
141
  printf("Joystick detected: %s\n\t%d axis\n\t%d buttons\n\n"
162
  printf("Joystick detected: %s\n\t%d axis\n\t%d buttons\n\n"
142
         , name_of_joystick
163
         , name_of_joystick
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 */
-
 
168
 
147
 
169
  return 1;
148
}
170
}
149
 
171
 
150
 
172
 
151
 
173
 
152
 
174
 
153
void AddCRC(unsigned int wieviele)
175
void AddCRC(unsigned int wieviele)
154
{
176
{
155
  unsigned int tmpCRC = 0,i;
177
  unsigned int tmpCRC = 0,i;
156
  for(i = 0; i < wieviele;i++)
178
  for(i = 0; i < wieviele;i++)
157
    {
179
    {
158
      tmpCRC += TxBuffer[i];
180
      tmpCRC += TxBuffer[i];
159
    }
181
    }
160
  tmpCRC %= 4096;
182
  tmpCRC %= 4096;
161
  TxBuffer[i++] = '=' + tmpCRC / 64;
183
  TxBuffer[i++] = '=' + tmpCRC / 64;
162
  TxBuffer[i++] = '=' + tmpCRC % 64;
184
  TxBuffer[i++] = '=' + tmpCRC % 64;
163
  TxBuffer[i++] = '\r';
185
  TxBuffer[i++] = '\r';
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);
192
 
214
 
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
 
215
 
237
 
216
void write_display(int line,char* text)
238
void write_display(int line,char* text)
217
{
239
{
218
  if (x52_output) x52_settext(x52_output, line , text, strlen(text));
240
  if (x52_output) x52_settext(x52_output, line , text, strlen(text));
219
}
241
}
220
 
242
 
221
void clear_display()
243
void clear_display()
222
{
244
{
223
  write_display(0,"");
245
  write_display(0,"");
224
  write_display(1,"");
246
  write_display(1,"");
225
  write_display(2,"");
247
  write_display(2,"");
226
}
248
}
227
 
249
 
228
 
250
 
229
void output_device_list()
251
void output_device_list()
230
{
252
{
231
  int i;
253
  int i;
232
  char disp_txt[20];
254
  char disp_txt[20];
233
  for(i=0;i<bt_device_count;i++)
255
  for(i=0;i<bt_device_count;i++)
234
    {
256
    {
235
      if (i<3)
257
      if (i<3)
236
        {
258
        {
237
         
259
         
238
          if (selected_bt_device==i)
260
          if (selected_bt_device==i)
239
            sprintf(disp_txt,"#%s",names[i]);
261
            sprintf(disp_txt,"#%s",names[i]);
240
          else
262
          else
241
            sprintf(disp_txt," %s",names[i]);
263
            sprintf(disp_txt," %s",names[i]);
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);
253
 
275
 
254
  // set the connection parameters (who to connect to)
276
  // set the connection parameters (who to connect to)
255
  addr.rc_family = AF_BLUETOOTH;
277
  addr.rc_family = AF_BLUETOOTH;
256
  addr.rc_channel = 1;
278
  addr.rc_channel = 1;
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));
-
 
283
 
-
 
284
  return status;
261
 
285
 
262
}
286
}
263
 
287
 
264
 
288
 
265
 
289
 
266
 
290
 
267
          int r=0;
291
int r=0;
-
 
292
int count=0;
268
          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
   
291
  clear_display();
322
  clear_display();
292
 
323
 
293
  write_display(0, "RIDDIM active");
324
  write_display(0, "RIDDIM active");
294
 
325
 
295
  if (x52_output) x52_setbri(x52_output, 1,128);  
326
  if (x52_output) x52_setbri(x52_output, 1,128);  
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
 
-
 
301
  printf("Scanning for Bluetooth Devices ..\n");
-
 
302
  write_display(1,"Bluetooth Scan");
-
 
303
 
331
 
304
  //  scan_bt();
-
 
305
  printf(" done \n");  
-
 
306
  printf(" %d Devices found \n",bt_device_count);  
-
 
307
 
-
 
308
 
-
 
309
  /*  for(i=0;i<bt_device_count;i++)
332
  if (!connected)
310
    {
333
    {
311
      printf(" %d -> %s (%s) \n",i,names[i],addrs[i]);  
334
      printf("Scanning for Bluetooth Devices ..\n");
-
 
335
      write_display(1,"Bluetooth Scan");
-
 
336
      scan_bt();
-
 
337
      printf(" done \n");  
312
      if (i<3) write_display(i,names[i]);
338
      printf(" %d Devices found \n",bt_device_count);  
313
    }
339
    }
314
 
-
 
315
  output_device_list() ;
-
 
316
  */
340
 
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;
342
        else
367
        else
343
          {
368
          {
344
            if (button_trigger[x]<100)button_trigger[x]++;
369
            if (button_trigger[x]<100)button_trigger[x]++;
345
          }
370
          }
346
 
371
 
347
 
372
 
348
               
373
               
349
      switch(state)
374
      switch(state)
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;;
361
 
386
 
362
          printf("nick%d\n",ExternControl.Nick);         
387
          printf("nick%d\n",ExternControl.Nick);         
363
          ExternControl.Roll=(axis[0]>>8)*(-1)+127;;
388
          ExternControl.Roll=(axis[0]>>8)*(-1)+127;;
364
          ExternControl.Gier=(axis[5]>>8)*(-1)+127;;
389
          ExternControl.Gier=(axis[5]>>8)*(-1)+127;;
365
          ExternControl.Gas=(axis[2]>>8)*(-1)+127;
390
          ExternControl.Gas=(axis[2]>>8)*(-1)+127;
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
 
-
 
371
 
-
 
372
 
395
 
373
          printf("sending data\n");
-
 
374
 
396
          printf("sending data\n");
-
 
397
 
-
 
398
         
375
          SendOutData('b', 0, &ExternControl, sizeof(ExternControl));
399
          SendOutData('b', 0, (unsigned char *)&ExternControl, sizeof(ExternControl));
376
 
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();
382
              write_display(0,"connecting to");
406
              write_display(0,"connecting to");
383
              write_display(1,names[selected_bt_device]);
407
              write_display(1,names[selected_bt_device]);
384
              connect_mk(addrs[selected_bt_device]);
408
              connect_mk(addrs[selected_bt_device]);
385
              write_display(0,"connected to");
409
              write_display(0,"connected to");
386
            }
410
            }
387
                   
411
                   
388
          if ((button_trigger[BUTTON_UP]+button_trigger[BUTTON_DOWN])==1)
412
          if ((button_trigger[BUTTON_UP]+button_trigger[BUTTON_DOWN])==1)
389
            {
413
            {
390
              printf("-> sel_dev %d - %d\n",selected_bt_device,button_trigger[19]);
414
              printf("-> sel_dev %d - %d\n",selected_bt_device,button_trigger[19]);
391
              if (button_trigger[BUTTON_DOWN]==1)
415
              if (button_trigger[BUTTON_DOWN]==1)
392
                if (selected_bt_device>0) selected_bt_device--;
416
                if (selected_bt_device>0) selected_bt_device--;
393
              if (button_trigger[BUTTON_UP]==1)
417
              if (button_trigger[BUTTON_UP]==1)
394
                if (selected_bt_device<bt_device_count-1) selected_bt_device++;
418
                if (selected_bt_device<bt_device_count-1) selected_bt_device++;
395
             
419
             
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
 
-
 
406
          r=0;
-
 
-
 
427
 
-
 
428
 
407
          in_char='#';
429
          //      ftime(&time_struct);
408
 
430
          //printf("t:%d",time_struct.millitm);
-
 
431
          while (RxBuffer[1]!='t')
409
          while(in_char!='\r')
432
            {
410
            {
433
 
411
              count=read(s,&in_char,1);
434
              r=0;
412
              if (in_char!=0)
435
              in_char='#';
413
                {
-
 
414
                  RxBuffer[r++]=in_char;
-
 
415
                }
436
 
-
 
437
              while(in_char!='\r')
-
 
438
                {
-
 
439
                  count=read(s,&in_char,1);
-
 
440
                  if (in_char!=0)
-
 
441
                    {
-
 
442
                      RxBuffer[r++]=in_char;
-
 
443
                    }
416
              else
444
                  else
-
 
445
                    {
-
 
446
                      RxBuffer[r++]='0';
417
                {
447
                    }
-
 
448
                  //  printf("count:%d r:%d %d %c \n",count , r, in_char, in_char);
418
                  RxBuffer[r++]='0';
449
                }
-
 
450
              RxBuffer[r++]='\0';
419
                }
451
              printf("--->%s\n",RxBuffer);
-
 
452
 
-
 
453
            }
-
 
454
          gettimeofday(&time_struct2,NULL);
-
 
455
 
-
 
456
          printf("last trip: %d",(int)(time_struct1.tv_usec-time_struct2.tv_usec));
420
              //  printf("count:%d r:%d %d %c \n",count , r, in_char, in_char);
457
          act_mode=button[24] | (button[25]<<1);
-
 
458
 
-
 
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;
421
            }
479
              act_roll=(axis[AXIS_ROLL]>>8)*(INVERT_ROLL)/3;
422
          RxBuffer[r++]='\0';
-
 
-
 
480
              act_gier=(axis[AXIS_GIER]>>8)*(INVERT_GIER)/3;
-
 
481
              act_gas=(axis[AXIS_GAS]>>8)*(INVERT_GAS);
-
 
482
 
-
 
483
 
423
          printf("--->%s\n",RxBuffer);
484
              break;
424
 
485
 
425
          }
486
            }
426
 
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);
-
 
451
                 
513
          // usleep(10000);
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
         
458
          printf("v: %d \n",v);
520
          printf("v: %d \n",v);
459
 
521
 
460
                   
522
                   
461
          for (i=0;i<num_of_axis;i++)
523
          for (i=0;i<num_of_axis;i++)
462
            printf("A%d: %d  ", i,axis[i]>>8 );
524
            printf("A%d: %d  ", i,axis[i]>>8 );
463
         
525
         
464
          for( x=0 ; x<num_of_buttons ; ++x )
526
          for( x=0 ; x<num_of_buttons ; ++x )
465
           
527
           
466
            printf("B%d: %d  ", x, button[x] );            
528
            printf("B%d: %d  ", x, button[x] );            
467
                   
529
                   
468
          break;
530
          break;
469
        }
531
        }
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);
481
 
543
 
482
  if (x52_output) x52_close(x52_output);
544
  if (x52_output) x52_close(x52_output);
483
  return 0;
545
  return 0;
484
}
546
}
485
 
547