WebSVN - Projects - Diff - Rev 48 and 50

 #include <sys/socket.h>
 #include <bluetooth/bluetooth.h>
 #include <bluetooth/hci.h>
 #include <bluetooth/hci_lib.h>
+#include <sys/time.h>
 #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
+#define INVERT_ROLL -1
+#define INVERT_NICK -1
+#define INVERT_GIER 1
+#define INVERT_GAS  -1
+// 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;
-#define BUTTON_UP 27
+int act_mode=0;
 int bt_device_count=0;
 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;
 struct js_event x52_event_struct;
 int engines_on=0;
-int old_engines_on=0;
-char in_char;
+int old_engines_on=0;
-struct
-{
   char val[4];
-} MotortestParam;
+char in_char;
-struct
+struct ExternControl_s
+{
   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;
-} ExternControl;
+};
 struct ExternControl_s  ExternControl ;
   free( ii );
   close( sock );
+}
-void connect_joy()
+int connect_joy()
+{
+  if( ( x52_input_fd = open( JOY_DEV, O_RDONLY ) ) < 0 )
   if( ( x52_input_fd = open( JOY_DEV, O_RDONLY ) ) < 0 )
       printf( "Couldn't open joystick device %s\n", JOY_DEV );
-      printf( "Couldn't open joystick device %s\n", JOY_DEV );
+      printf( "try modprobe joydev\n"  );
-      return ;
+      return 0;
          , num_of_axis
          , num_of_buttons );
+  fcntl( x52_input_fd, F_SETFL, O_NONBLOCK );   /* use non-blocking mode */
   fcntl( x52_input_fd, F_SETFL, O_NONBLOCK );   /* use non-blocking mode */
+}
 void SendOutData(unsigned char cmd,unsigned char modul, unsigned char *snd, unsigned char len)
 {
- unsigned int pt = 0;
+  unsigned int pt = 0;
- unsigned char a,b,c;
+  unsigned char a,b,c;
- unsigned char ptr = 0;
+  unsigned char ptr = 0;
- TxBuffer[pt++] = '#';               // Startzeichen
+  TxBuffer[pt++] = '#';               // Startzeichen
- TxBuffer[pt++] = modul;             // Adresse (a=0; b=1,...)
+  TxBuffer[pt++] = modul;             // Adresse (a=0; b=1,...)
- TxBuffer[pt++] = cmd;                  // Commando
+  TxBuffer[pt++] = cmd;                 // Commando
- while(len)
+  while(len)
-  {
+    {
-   if(len) { a = snd[ptr++]; len--;} else a = 0;
+      if(len) { a = snd[ptr++]; len--;} else a = 0;
-   if(len) { b = snd[ptr++]; len--;} else b = 0;
+      if(len) { b = snd[ptr++]; len--;} else b = 0;
-   if(len) { c = snd[ptr++]; len--;} else c = 0;
+      if(len) { c = snd[ptr++]; len--;} else c = 0;
-   TxBuffer[pt++] = '=' + (a >> 2);
+      TxBuffer[pt++] = '=' + (a >> 2);
-   TxBuffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
+      TxBuffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
-   TxBuffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
+      TxBuffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
-   TxBuffer[pt++] = '=' + ( c & 0x3f);
+      TxBuffer[pt++] = '=' + ( c & 0x3f);
+    }
   status = send(s,"\r" , 1, 0);
   //  for (c=0;c<pt+2;c++)
   // {
-      status = write(s,&TxBuffer , pt+3);
+  status = write(s,&TxBuffer , pt+3);
-      //   printf("Send to MK %d \n" , TxBuffer[c] );
+  //   printf("Send to MK %d \n" , TxBuffer[c] );
-      // }
+  // }
   /*while(TxBuffer[i] !='\r' && i<150)
     {
-      //     TxBuffer[i]='#';
+    //     TxBuffer[i]='#';
-      status = send(s,&TxBuffer[i] , 1, 0);
+    status = send(s,&TxBuffer[i] , 1, 0);
-      printf(" +%d%c ",i,TxBuffer[i]);
+    printf(" +%d%c ",i,TxBuffer[i]);
-      i++;
+    i++;
+    }
-  status = send(s,"\r" , 1, 0);
+    status = send(s,"\r" , 1, 0);
   */
           write_display(i,disp_txt);
+        }
+    }
+}
-void connect_mk(char dest[18])
+int connect_mk(char dest[18])
+{
   struct sockaddr_rc addr ;
   str2ba( dest, &addr.rc_bdaddr );
   // connect to server
+  status = connect(s, (struct sockaddr *)&addr, sizeof(addr));
-  status = connect(s, (struct sockaddr *)&addr, sizeof(addr));
+  return status;
 }
 int r=0;
-          int r=0;
+int count=0;
+int connected=0;
-          int count=0;
+int main(int argc, char**argv)
+{
-int main(int argc, char**argv)
+  printf("Starting Riddim \n");
+  printf("\tRemote Interactive Digital Drone Interface Mashup\n");
-{
-  printf("Starting Riddim \n");
+  // 1st argument -> Bluetooth adrees to bypass scanning ( takes to long for short testing roundtrips )
+  if (argv[1])
+    {
-  printf("\tRemote Interactive Digital Drone Interface Mashup\n");
+      if (connect_mk(argv[1])==-1)
+        {
-  //int tmp=2;
+          printf("cant connect to QC at adress: %s\n",argv[1]);
-  connect_mk("00:0B:CE:01:6B:4F");
+          return 0;
+        }
+        printf("connected to QC at adress: %s\n",argv[1]);
         connected=1;
+    }
   if (x52_output)
     printf(" done \n");
   else
     printf(" not found \n");
-  printf("Scanning for Bluetooth Devices ..\n");
-  write_display(1,"Bluetooth Scan");
-  //  scan_bt();
-  printf(" done \n");
-  printf(" %d Devices found \n",bt_device_count);
-  /*  for(i=0;i<bt_device_count;i++)
+  if (!connected)
     {
-      printf(" %d -> %s (%s) \n",i,names[i],addrs[i]);
+      printf("Scanning for Bluetooth Devices ..\n");
+      write_display(1,"Bluetooth Scan");
+      scan_bt();
+      printf(" done \n");
-      if (i<3) write_display(i,names[i]);
+      printf(" %d Devices found \n",bt_device_count);
     }
-  output_device_list() ;
   */
   int v_old;
+  while( 1 )
   while( 1 )
-    {
+      // poll values from input device
       int polls=0;
-      for (polls=0;polls<1000;polls++) // FIXME - better Polling
+      for (polls=0;polls<100;polls++) // FIXME - better Polling
         {
-      read(x52_input_fd, &x52_event_struct, sizeof(struct js_event));
+          read(x52_input_fd, &x52_event_struct, sizeof(struct js_event));
-                        /* see what to do with the event */
+          /* see what to do with the event */
-                switch (x52_event_struct.type & ~JS_EVENT_INIT)
+          switch (x52_event_struct.type & ~JS_EVENT_INIT)
-                {
+            {
-                        case JS_EVENT_AXIS:
+            case JS_EVENT_AXIS:
-                                axis   [ x52_event_struct.number ] = x52_event_struct.value;
+              axis   [ x52_event_struct.number ] = x52_event_struct.value;
-                                break;
+              break;
-                        case JS_EVENT_BUTTON:
+            case JS_EVENT_BUTTON:
-                                button [ x52_event_struct.number ] = x52_event_struct.value;
+              button [ x52_event_struct.number ] = x52_event_struct.value;
-                                break;
+              break;
+            }
+        {
         case STATEID_SCANNING:
-          state=STATEID_CONNECTING; //e
+          state=STATEID_CONNECTING;
           ExternControl.Higt=0;
           ExternControl.free=0;
           ExternControl.Frame='t';
           ExternControl.Config=1;
-          printf("sending data\n");
+          printf("sending data\n");
-          SendOutData('b', 0, &ExternControl, sizeof(ExternControl));
+          SendOutData('b', 0, (unsigned char *)&ExternControl, sizeof(ExternControl));
           gettimeofday(&time_struct1,NULL);
           if (button_trigger[BUTTON_SELECT]==1)
               output_device_list()                ;
+            }
           break;
-        case STATEID_CONNECTING:
+        case STATEID_CONNECTING:
           //      for (polls=0;polls<10;polls++) // FIXME - better Polling
-          RxBuffer[1]=0;
-          while (RxBuffer[1]!='t')
-          {
+          RxBuffer[1]=0;
-          r=0;
-          in_char='#';
+          //      ftime(&time_struct);
+          //printf("t:%d",time_struct.millitm);
+          while (RxBuffer[1]!='t')
-          while(in_char!='\r')
+            {
-            {
-              count=read(s,&in_char,1);
+              r=0;
-              if (in_char!=0)
-                {
-                  RxBuffer[r++]=in_char;
+              in_char='#';
+              while(in_char!='\r')
+                {
+                  count=read(s,&in_char,1);
+                  if (in_char!=0)
+                    {
+                      RxBuffer[r++]=in_char;
-                }
+                    }
+                  else
+                    {
-              else
+                      RxBuffer[r++]='0';
+                    }
-                {
+                  //  printf("count:%d r:%d %d %c \n",count , r, in_char, in_char);
+                }
-                  RxBuffer[r++]='0';
+              RxBuffer[r++]='\0';
+              printf("--->%s\n",RxBuffer);
+            }
+          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);
+              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:
-              //  printf("count:%d r:%d %d %c \n",count , r, in_char, in_char);
+              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);
           RxBuffer[r++]='\0';
-          printf("--->%s\n",RxBuffer);
+              break;
-          }
+            }
           ExternControl.Digital[0]=0;
           ExternControl.Digital[1]=0;
           ExternControl.RemoteTasten=0;
-          ExternControl.Nick=(axis[1]>>8)*(-1);
+          ExternControl.Nick=act_nick;  //(axis[1]>>8)*(-1)/2;
-          printf("nick%d\n",ExternControl.Nick);
+          //      printf("nick%d\n",ExternControl.Nick);
- ExternControl.Roll=(axis[0]>>8)*(-1);
+          ExternControl.Roll=act_roll; //(axis[0]>>8)*(-1)/2;
+          ExternControl.Gier=(axis[5]>>8);
-          ExternControl.Gier=(axis[5]>>8);
+          ExternControl.Gas=(axis[2]>>8)*(-1)+127;
+          ExternControl.Higt=0;
-          ExternControl.Gas=(axis[2]>>8)*(-1)+127;
+          ExternControl.free=0;
-          ExternControl.Higt=0;
+          ExternControl.Frame='t';
-          ExternControl.free=0;
-          ExternControl.Frame='t';
+          ExternControl.Config=1;
-          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("sending data\n");
           printf("sending data\n");
            SendOutData('b', 0, &ExternControl, sizeof(ExternControl));
       printf("\n");
       fflush(stdout);
       printf("loop fin");
+    }

Subversion Repositories Projects

(root)/Riddim/riddim.c @ 75 - Rev 48 → 50