WebSVN - Projects - Diff - Rev 50 and 117

 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
 #include <fcntl.h>
 #include "lib/x52/x52.h"
 #include <unistd.h>
 #include <sys/socket.h>
 #include <bluetooth/bluetooth.h>
 #include <bluetooth/hci.h>
 #include <bluetooth/hci_lib.h>
 #include <sys/time.h>
 #include <bluetooth/rfcomm.h>
 #include <linux/joystick.h>
 #define JOY_DEV "/dev/input/js0"
 #define MAX_BT_DEVICES 3
 #define STATEID_SCANNING 0
 #define STATEID_CONNECTING 1
 #define BUTTON_SELECT 26
 #define BUTTON_DOWN 28
 #define BUTTON_UP 27
+// #define AXIS_ROLL 0
+// #define AXIS_NICK 1
+// #define AXIS_GIER 5
+// #define AXIS_GAS  2
+// for x52
+/*
-#define AXIS_ROLL 0
+#define AXIS_ROLL 4
-#define AXIS_NICK 1
+#define AXIS_NICK 3
 #define AXIS_GIER 5
 #define AXIS_GAS  2
 #define INVERT_ROLL -1
 #define INVERT_NICK -1
 #define INVERT_GIER 1
 #define INVERT_GAS  -1
+*/
+#define AXIS_ROLL 0
+#define AXIS_NICK 1
+#define AXIS_GIER 3
+#define AXIS_GAS  2
+#define INVERT_ROLL 1
+#define INVERT_NICK -1
+#define INVERT_GIER 1
+#define INVERT_GAS  -1
+#include <stdio.h>
+#include <errno.h>
+#include <string.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <netinet/in.h>
+#include <unistd.h>//for close() for socket
+#define INPUT_NONE 0
+#define INPUT_JOYDEV 1
+#define INPUT_EVDEV 2
 // time struct for measuring
 struct timeval time_struct1;
 struct timeval time_struct2;
 int act_nick=0;
 int act_roll=0;
 int act_gier=0;
 int act_gas=0;
 int act_mode=0;
 int bt_device_count=0;
 char names[MAX_BT_DEVICES][248];
 char addrs[MAX_BT_DEVICES][19];
 int s, status;
 unsigned char TxBuffer[150];
 unsigned char _TxBuffer[150];
 char RxBuffer[150];
 int x52_input_fd, *axis=NULL, num_of_axis=0, num_of_buttons=0, x;
 char *button=NULL,*button_trigger=NULL, name_of_joystick[80];
 struct js_event x52_event_struct;
 int engines_on=0;
 int old_engines_on=0;
 char in_char;
 struct ExternControl_s
+{
   unsigned char Digital[2];   // (noch unbenutzt)
   unsigned char RemoteTasten; //(gab es schon für das virtuelle Display)
   signed char   Nick;
   signed char   Roll;
   signed char   Gier;
   unsigned char Gas;          //(es wird das Stick-Gas auf diesen Wert begrenzt; --> StickGas ist das Maximum)
   signed char   Higt;        //(Höhenregler)
   unsigned char free;         // (unbenutzt)
   unsigned char Frame;        // (Bestätigung)
   unsigned char Config;
 };
 struct ExternControl_s  ExternControl ;
 int state=STATEID_SCANNING;
 struct x52 *x52_output;
 int selected_bt_device=0;
 void scan_bt()
+{
   inquiry_info *ii = NULL;
   int dev_id, sock, len, flags;
   int i;
   char addr[19] = { 0 };
   char name[248] = { 0 };
   dev_id = hci_get_route(NULL);
   sock = hci_open_dev( dev_id );
   if (dev_id < 0 || sock < 0) {
     perror("opening socket");
     exit(1);
+  }
   len  = 8;
   flags = IREQ_CACHE_FLUSH;
   ii = (inquiry_info*)malloc(MAX_BT_DEVICES * sizeof(inquiry_info));
   bt_device_count = hci_inquiry(dev_id, len, MAX_BT_DEVICES, NULL, &ii, flags);
   if(  bt_device_count < 0 ) perror("hci_inquiry");
   for (i = 0; i <  bt_device_count; i++) {
     ba2str(&(ii+i)->bdaddr, addr);
     sprintf(addrs[i],"%s",addr);
     memset(name, 0, sizeof(name));
     if (hci_read_remote_name(sock, &(ii+i)->bdaddr, sizeof(name),
                              name, 0) < 0)
       sprintf(names[i],"[unknown]");
     else
       sprintf(names[i],"%s",name);
+  }
   free( ii );
   close( sock );
+}
+ int yalv;           /* loop counter */
+    size_t read_bytes;  /* how many bytes were read */
+    struct input_event ev[64]; /* the events (up to 64 at once) */
+int evdev_fd;
+int connect_evdev()
+{
+  if ((evdev_fd = open("/dev/input/event10", O_RDONLY)) < 0) {
+        printf(" cant open evdev ");
+        return 0;
+    }
+  return 1;
+}
 int connect_joy()
+{
+  axis = (int *) calloc( 100, sizeof( int ) );
+  button = (char *)calloc( 100, sizeof( char ) );
+  button_trigger = (char *) calloc( 100, sizeof( char ) );
+  //  axis = (int *) calloc( num_of_axis, sizeof( int ) );
+  // button = (char *)calloc( num_of_buttons, sizeof( char ) );
+  // button_trigger = (char *) calloc( num_of_buttons, sizeof( char ) );
   if( ( x52_input_fd = open( JOY_DEV, O_RDONLY ) ) < 0 )
+    {
       printf( "Couldn't open joystick device %s\n", JOY_DEV );
       printf( "try modprobe joydev\n"  );
       return 0;
+    }
   ioctl( x52_input_fd, JSIOCGAXES, &num_of_axis );
   ioctl( x52_input_fd, JSIOCGBUTTONS, &num_of_buttons );
   ioctl( x52_input_fd, JSIOCGNAME(80), &name_of_joystick );
-  axis = (int *) calloc( num_of_axis, sizeof( int ) );
-  button = (char *)calloc( num_of_buttons, sizeof( char ) );
-  button_trigger = (char *) calloc( num_of_buttons, sizeof( char ) );
   printf("Joystick detected: %s\n\t%d axis\n\t%d buttons\n\n"
          , name_of_joystick
          , num_of_axis
          , num_of_buttons );
   fcntl( x52_input_fd, F_SETFL, O_NONBLOCK );   /* use non-blocking mode */
   return 1;
+}
 void AddCRC(unsigned int wieviele)
+{
   unsigned int tmpCRC = 0,i;
   for(i = 0; i < wieviele;i++)
+    {
       tmpCRC += TxBuffer[i];
+    }
   tmpCRC %= 4096;
   TxBuffer[i++] = '=' + tmpCRC / 64;
   TxBuffer[i++] = '=' + tmpCRC % 64;
   TxBuffer[i++] = '\r';
+}
 void SendOutData(unsigned char cmd,unsigned char modul, unsigned char *snd, unsigned char len)
+{
   unsigned int pt = 0;
   unsigned char a,b,c;
   unsigned char ptr = 0;
   TxBuffer[pt++] = '#';               // Startzeichen
   TxBuffer[pt++] = modul;             // Adresse (a=0; b=1,...)
   TxBuffer[pt++] = cmd;                 // Commando
   while(len)
+    {
       if(len) { a = snd[ptr++]; len--;} else a = 0;
       if(len) { b = snd[ptr++]; len--;} else b = 0;
       if(len) { c = snd[ptr++]; len--;} else c = 0;
       TxBuffer[pt++] = '=' + (a >> 2);
       TxBuffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
       TxBuffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
       TxBuffer[pt++] = '=' + ( c & 0x3f);
+    }
   AddCRC(pt);
   printf("Sending to MK %d \n" , pt);
   status = send(s,"\r" , 1, 0);
   //  for (c=0;c<pt+2;c++)
   // {
   status = write(s,&TxBuffer , pt+3);
   //   printf("Send to MK %d \n" , TxBuffer[c] );
   // }
   /*while(TxBuffer[i] !='\r' && i<150)
+    {
     //     TxBuffer[i]='#';
     status = send(s,&TxBuffer[i] , 1, 0);
     printf(" +%d%c ",i,TxBuffer[i]);
     i++;
+    }
     status = send(s,"\r" , 1, 0);
   */
   // status = send(s,"\r" , 1, 0);
   printf("\n");
+}
 void write_display(int line,char* text)
+{
   if (x52_output) x52_settext(x52_output, line , text, strlen(text));
+}
 void clear_display()
+{
   write_display(0,"");
   write_display(1,"");
   write_display(2,"");
+}
 void output_device_list()
+{
   int i;
   char disp_txt[20];
   for(i=0;i<bt_device_count;i++)
+    {
       if (i<3)
+        {
           if (selected_bt_device==i)
             sprintf(disp_txt,"#%s",names[i]);
           else
             sprintf(disp_txt," %s",names[i]);
           write_display(i,disp_txt);
+        }
+    }
+}
 int connect_mk(char dest[18])
+{
+  struct sockaddr_rc addr ;
+  struct sockaddr_in sa;
+ sa.sin_family = AF_INET;
+  sa.sin_addr.s_addr = htonl(0x0);
+  sa.sin_port = htons(54321);
+  // allocate a socket
+  //  s = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
+  s = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
+  //);
+  // set the connection parameters (who to connect to)
+  addr.rc_family = AF_BLUETOOTH;
+  addr.rc_channel = 1;
+  str2ba( dest, &addr.rc_bdaddr );
+  // connect to server
+  //  status = connect(s, (struct sockaddr *)&addr, sizeof(addr));
+  status = connect(s,(struct sockaddr*) &sa, sizeof(struct sockaddr_in));
+  return status;
+  /*
   struct sockaddr_rc addr ;
   // allocate a socket
   s = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
   // set the connection parameters (who to connect to)
   addr.rc_family = AF_BLUETOOTH;
   addr.rc_channel = 1;
   str2ba( dest, &addr.rc_bdaddr );
   // connect to server
   status = connect(s, (struct sockaddr *)&addr, sizeof(addr));
   return status;
+  */
+}
 int r=0;
 int count=0;
 int connected=0;
+int input=INPUT_NONE;
 int main(int argc, char**argv)
+{
   printf("Starting Riddim \n");
   printf("\tRemote Interactive Digital Drone Interface Mashup\n");
   // 1st argument -> Bluetooth adrees to bypass scanning ( takes to long for short testing roundtrips )
   if (argv[1])
+    {
       if (connect_mk(argv[1])==-1)
+        {
           printf("cant connect to QC at adress: %s\n",argv[1]);
           return 0;
+        }
         printf("connected to QC at adress: %s\n",argv[1]);
         connected=1;
+    }
   int i;
-  printf("\nInitializing X-52 input ..\n");
-  if (!connect_joy()) return 0;
+  printf("\nInitializing X-52 input ..\n");
+  if (connect_joy())
+    {
+    printf(".. done");//
+    input=INPUT_JOYDEV;
+    }
+  else
+    printf(".. ERROR ");//
+  /*  printf("\nInitializing evdev input ..\n");
+  if (connect_evdev())
+    {
+    printf(".. done");//
+    input=INPUT_EVDEV;
+    }
-  printf(".. done");//
+  else
     printf(".. ERROR ");//
   */
   printf("\nInitializing X-52 output ..");
   x52_output = x52_init();
   clear_display();
   write_display(0, "RIDDIM active");
   if (x52_output) x52_setbri(x52_output, 1,128);
   if (x52_output)
     printf(" done \n");
   else
     printf(" not found \n");
   if (!connected)
+    {
       printf("Scanning for Bluetooth Devices ..\n");
       write_display(1,"Bluetooth Scan");
       scan_bt();
       printf(" done \n");
       printf(" %d Devices found \n",bt_device_count);
+    }
   int v_old;
+  int polls=0;
+  printf("starting loop ..\n");
   while( 1 )
+    {
-      // poll values from input device
-      int polls=0;
+      switch (input)
-      for (polls=0;polls<100;polls++) // FIXME - better Polling
+        {
+        case INPUT_NONE:
-          read(x52_input_fd, &x52_event_struct, sizeof(struct js_event));
+          printf("input none\n");
+          usleep(10000);
+          break;
+        case INPUT_EVDEV:
+           printf("input evdev\n");
+          usleep(30000);
+        read_bytes = read(evdev_fd, ev, sizeof(struct input_event) * 64);
-          /* see what to do with the event */
+           printf("read done\n");
+        if (read_bytes < (int) sizeof(struct input_event)) {
+            perror("evtest: short read");
+            exit (1);
+        }
-          switch (x52_event_struct.type & ~JS_EVENT_INIT)
+        for (yalv = 0; yalv < (int) (read_bytes / sizeof(struct input_event)); yalv++)
             {
-            case JS_EVENT_AXIS:
+              // 1 -> nick
-              axis   [ x52_event_struct.number ] = x52_event_struct.value;
+              // 2 -> gas
-              break;
+              // 4->roll
-            case JS_EVENT_BUTTON:
+              // 5-> gier
+              axis[ ev[yalv].code]= ev[yalv].value;
+              printf("nick:%d roll:%d gier:%d gas:%d\n",axis[3] , axis[4] , axis[5] , axis[2]);
+              /*if ( ev[yalv].code==5)
+                printf("Event: time %ld.%06ld, type %d, code %d, value %d\n",
+                       ev[yalv].time.tv_sec, ev[yalv].time.tv_usec, ev[yalv].type,
-              button [ x52_event_struct.number ] = x52_event_struct.value;
+                       ev[yalv].code, ev[yalv].value);
-              break;
+              */
-            }
-        }
-      for( x=0 ; x<num_of_buttons ; ++x )
-        if( button[x]==0)
-          button_trigger[x]=0;
-        else
-          {
-            if (button_trigger[x]<100)button_trigger[x]++;
+            }
+          break;
+        case INPUT_JOYDEV:
+          printf("input joydev\n");
+          // poll values from input device
+          for (polls=0;polls<100;polls++) // FIXME - better Polling
+            {
+              read(x52_input_fd, &x52_event_struct, sizeof(struct js_event));
+              /* see what to do with the event */
+              switch (x52_event_struct.type & ~JS_EVENT_INIT)
+                {
+                case JS_EVENT_AXIS:
+                  axis   [ x52_event_struct.number ] = x52_event_struct.value;
+                  break;
+                case JS_EVENT_BUTTON:
+                  button [ x52_event_struct.number ] = x52_event_struct.value;
+                  break;
+                }
+            }
+          for( x=0 ; x<num_of_buttons ; ++x )
+            if( button[x]==0)
+              button_trigger[x]=0;
+            else
+              {
+                if (button_trigger[x]<100)button_trigger[x]++;
+              }
+          break;
+        }
       printf("input done\n");
       switch(state)
+        {
         case STATEID_SCANNING:
           state=STATEID_CONNECTING;
           ExternControl.Digital[0]=0;
           ExternControl.Digital[1]=0;
           ExternControl.RemoteTasten=0;
           ExternControl.Nick=(axis[1]>>8)*(-1)+127;;
           printf("nick%d\n",ExternControl.Nick);
           ExternControl.Roll=(axis[0]>>8)*(-1)+127;;
           ExternControl.Gier=(axis[5]>>8)*(-1)+127;;
           ExternControl.Gas=(axis[2]>>8)*(-1)+127;
           ExternControl.Higt=0;
           ExternControl.free=0;
           ExternControl.Frame='t';
           ExternControl.Config=1;
           printf("sending data\n");
           SendOutData('b', 0, (unsigned char *)&ExternControl, sizeof(ExternControl));
           gettimeofday(&time_struct1,NULL);
           if (button_trigger[BUTTON_SELECT]==1)
+            {
               state=STATEID_CONNECTING;
               clear_display();
               write_display(0,"connecting to");
               write_display(1,names[selected_bt_device]);
               connect_mk(addrs[selected_bt_device]);
               write_display(0,"connected to");
+            }
           if ((button_trigger[BUTTON_UP]+button_trigger[BUTTON_DOWN])==1)
+            {
               printf("-> sel_dev %d - %d\n",selected_bt_device,button_trigger[19]);
               if (button_trigger[BUTTON_DOWN]==1)
                 if (selected_bt_device>0) selected_bt_device--;
               if (button_trigger[BUTTON_UP]==1)
                 if (selected_bt_device<bt_device_count-1) selected_bt_device++;
               output_device_list()                ;
+            }
           break;
         case STATEID_CONNECTING:
           RxBuffer[1]=0;
           //      ftime(&time_struct);
-          //printf("t:%d",time_struct.millitm);
+          printf("waiting for confirm frame\n");
+          RxBuffer[2]=0;
+          int confirm_misses=0;
-          while (RxBuffer[1]!='t')
+          while (RxBuffer[2]!='t')
+            {
               r=0;
               in_char='#';
-              while(in_char!='\r')
+              while(in_char!='\n')
+                {
                   count=read(s,&in_char,1);
                   if (in_char!=0)
+                    {
                       RxBuffer[r++]=in_char;
+                    }
                   else
+                    {
                       RxBuffer[r++]='0';
+                    }
-                  //  printf("count:%d r:%d %d %c \n",count , r, in_char, in_char);
+                  //               printf("\ncount:%d r:%d %d %c \n",count , r, in_char, in_char);
+                }
               RxBuffer[r++]='\0';
               printf("--->%s\n",RxBuffer);
+              // new
+              if (button_trigger[12]>1)
+                {
+                  SendOutData('s', 0, (unsigned char *)&ExternControl, sizeof(ExternControl));
+                  button_trigger[12]=0;
+                }
+              if (++confirm_misses>4)
+              SendOutData('b', 0, (unsigned char *)&ExternControl, sizeof(ExternControl));
+            }
           gettimeofday(&time_struct2,NULL);
           printf("last trip: %d",(int)(time_struct1.tv_usec-time_struct2.tv_usec));
           act_mode=button[24] | (button[25]<<1);
           switch (act_mode)
+            {
             case 0:
               act_nick=(axis[AXIS_NICK]>>8)*(INVERT_NICK);
               act_roll=(axis[AXIS_ROLL]>>8)*(INVERT_ROLL);
               act_gier=(axis[AXIS_GIER]>>8)*(INVERT_GIER);
-              act_gas=(axis[AXIS_GAS]>>8)*(INVERT_GAS);
+              act_gas=((axis[AXIS_GAS]>>8)-128)*(-1);
+              // clip gas
+              if (act_gas<0) act_gas=0;
+              if (act_gas>250) act_gas=250;
 ////////              act_gas=0;
               break;
             case 1:
               act_nick=(axis[AXIS_NICK]>>8)*(INVERT_NICK)/2;
               act_roll=(axis[AXIS_ROLL]>>8)*(INVERT_ROLL)/2;
               act_gier=(axis[AXIS_GIER]>>8)*(INVERT_GIER)/2;
               act_gas=(axis[AXIS_GAS]>>8)*(INVERT_GAS);
               break;
             case 2:
               act_nick=(axis[AXIS_NICK]>>8)*(INVERT_NICK)/3;
               act_roll=(axis[AXIS_ROLL]>>8)*(INVERT_ROLL)/3;
               act_gier=(axis[AXIS_GIER]>>8)*(INVERT_GIER)/3;
               act_gas=(axis[AXIS_GAS]>>8)*(INVERT_GAS);
               break;
+            }
           ExternControl.Digital[0]=0;
           ExternControl.Digital[1]=0;
           ExternControl.RemoteTasten=0;
           ExternControl.Nick=act_nick;  //(axis[1]>>8)*(-1)/2;
           //      printf("nick%d\n",ExternControl.Nick);
-          ExternControl.Roll=act_roll; //(axis[0]>>8)*(-1)/2;
+          ExternControl.Roll=act_roll*(-1); //(axis[0]>>8)*(-1)/2;
           ExternControl.Gier=(axis[5]>>8);
+          ExternControl.Gier=act_gier; // ************
           ExternControl.Gas=(axis[2]>>8)*(-1)+127;
+          ExternControl.Gas=act_gas; // ************
+          //      ExternControl.Gas=0; // ************
           ExternControl.Higt=0;
           ExternControl.free=0;
           ExternControl.Frame='t';
           ExternControl.Config=1;
-          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);
+          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);
           printf("sending data\n");
           SendOutData('b', 0, (unsigned char *)&ExternControl, sizeof(ExternControl));
           gettimeofday(&time_struct1,NULL);
           printf("sent data\n");
           // printf("sleeping\n");
-          // usleep(10000);
+          //      for (polls=0;polls<100;polls++) // FIXME - better Polling
           // printf("end_sleep\n");
           int v=axis[6]/655+50;
           if (v!=v_old)if (x52_output) x52_setbri(x52_output, 0,v );
           v_old=v;
           printf("v: %d \n",v);
           for (i=0;i<num_of_axis;i++)
             printf("A%d: %d  ", i,axis[i]>>8 );
           for( x=0 ; x<num_of_buttons ; ++x )
             printf("B%d: %d  ", x, button[x] );
           break;
+        }
       printf("\n");
       fflush(stdout);
       printf("loop fin");
+    }
   /******************** Cleanup **********************/
   close(x52_input_fd);
   close(s);
   if (x52_output) x52_close(x52_output);
   return 0;
+}

Subversion Repositories Projects

(root)/Riddim/riddim.c @ 438 - Rev 50 → 117