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 */
           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 count=0;
-          int r=0;
+int connected=0;
-          int count=0;
+int main(int argc, char**argv)
+{
+  printf("Starting Riddim \n");
-int main(int argc, char**argv)
+  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])
-  printf("Starting Riddim \n");
+    {
-  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");
+  if (!connected)
+    {
-  printf("Scanning for Bluetooth Devices ..\n");
+      printf("Scanning for Bluetooth Devices ..\n");
-  write_display(1,"Bluetooth Scan");
+      write_display(1,"Bluetooth Scan");
-  //  scan_bt();
+      scan_bt();
-  printf(" done \n");
+      printf(" done \n");
-  printf(" %d Devices found \n",bt_device_count);
-  /*  for(i=0;i<bt_device_count;i++)
-    {
-      printf(" %d -> %s (%s) \n",i,names[i],addrs[i]);
-      if (i<3) write_display(i,names[i]);
+      printf(" %d Devices found \n",bt_device_count);
-    }
   output_device_list() ;
   */
+  int v_old;
-  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
+        {
         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);
               output_device_list()                ;
+            }
           break;
-        case STATEID_CONNECTING:
+        case STATEID_CONNECTING:
           //      for (polls=0;polls<10;polls++) // FIXME - better Polling
+          RxBuffer[1]=0;
+          //      ftime(&time_struct);
-          RxBuffer[1]=0;
+          //printf("t:%d",time_struct.millitm);
           while (RxBuffer[1]!='t')
+            {
+                }
-          RxBuffer[r++]='\0';
+              RxBuffer[r++]='\0';
-          printf("--->%s\n",RxBuffer);
+              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:
+              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=(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.Gas=(axis[2]>>8)*(-1)+127;
           ExternControl.Higt=0;
+          ExternControl.free=0;
-          ExternControl.free=0;
+          ExternControl.Frame='t';
-          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");
-           SendOutData('b', 0, &ExternControl, sizeof(ExternControl));
           SendOutData('b', 0, (unsigned char *)&ExternControl, sizeof(ExternControl));
-          printf("sent data\n");
+          gettimeofday(&time_struct1,NULL);
           printf("sent data\n");

Subversion Repositories Projects

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