WebSVN - Projects - Diff - Rev 125 and 127

+/**************************************************
+ *
+ *
+ * Riddim
-#include <confuse.h>
+ * Remote Interactive Digital Drone Interface Mashup
+ *
-#include <string.h>
+ * 2007-2008 Marcus -LiGi- Bueschleb
+ *
+ *
+**************************************************/
-#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 "riddim.h"
+#define FALSE 0
-#include <sys/time.h>
-#include <bluetooth/rfcomm.h>
+#define TRUE 1
-#include <linux/joystick.h>
-#define JOY_DEV "/dev/input/js0"
-#define MAX_BT_DEVICES 3
+#define test_bit(bit, array)    (array[bit/8] & (1<<(bit%8)))
+int state=STATEID_SCANNING;
-#define STATEID_SCANNING 0
-#define STATEID_CONNECTING 1
-#define BUTTON_SELECT 26
+// from config
+char *input_evdev_name;
-#define BUTTON_DOWN 28
-#define BUTTON_UP 27
+int  mk_socket_port=0;
+int  loop_delay=0;
-// #define AXIS_ROLL 0
-// #define AXIS_NICK 1
-// #define AXIS_GIER 5
-// #define AXIS_GAS  2
-// for x52
-/*
-#define AXIS_ROLL 4
-#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>
+double nick_mul=0.3f;
-#include <sys/types.h>
+double roll_mul=0.3f;
-#include <netinet/in.h>
+double gier_mul=0.3f;
-#include <unistd.h>//for close() for socket
-#define INPUT_NONE 0
-#define INPUT_JOYDEV 1
-#define INPUT_EVDEV 2
-// from config
- char *input_evdev_name;
-// time struct for measuring
-struct timeval time_struct1;
-struct timeval time_struct2;
 double gas_mul=0.3f;
+int rel_axis_nick=1;
+int rel_axis_roll=0;
+int rel_axis_gier=5;
-int act_nick=0;
+int rel_axis_gas=2;
 int act_roll=0;
 int act_gier=0;
-int act_gas=0;
+cfg_bool_t exit_after_init = cfg_false;
 int act_mode=0;
 // time struct for measuring
-int bt_device_count=0;
+struct timeval time_struct1;
-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 timeval time_struct2;
-struct js_event x52_event_struct;
+char RxBuffer[150];
+int status;
-int engines_on=0;
-int old_engines_on=0;
-char in_char;
-struct ExternControl_s
+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];
-  unsigned char Digital[2];   // (noch unbenutzt)
+struct js_event x52_event_struct;
-  unsigned char RemoteTasten; //(gab es schon für das virtuelle Display)
+int engines_on=0;
+int old_engines_on=0;
-  signed char   Nick;
+char in_char;
-  signed char   Roll;
+struct x52 *x52_output;
-  signed char   Gier;
+int selected_bt_device=0;
-  unsigned char Gas;          //(es wird das Stick-Gas auf diesen Wert begrenzt; --> StickGas ist das Maximum)
-  signed char   Higt;        //(Höhenregler)
+ int yalv;           /* loop counter */
-  unsigned char free;         // (unbenutzt)
+    size_t read_bytes;  /* how many bytes were read */
-  unsigned char Frame;        // (Bestätigung)
+    struct input_event ev[64]; /* the events (up to 64 at once) */
-  unsigned char Config;
+int evdev_fd;
-};
-struct ExternControl_s  ExternControl ;
+int evdev_out_fd;
-int state=STATEID_SCANNING;
+int evdev_rw=TRUE;
-struct x52 *x52_output;
-int selected_bt_device=0;
+int connect_evdev()
+{
+  struct input_devinfo {
+    uint16_t bustype;
-void scan_bt()
+    uint16_t vendor;
-{
-  inquiry_info *ii = NULL;
+    uint16_t product;
-  int dev_id, sock, len, flags;
-  int i;
+    uint16_t version;
-  char addr[19] = { 0 };
+  };
-  char name[248] = { 0 };
+  struct input_devinfo device_info;
+  if ((evdev_out_fd = open(input_evdev_name, O_WRONLY)) < 0)
+    {
+      printf(" cant open evdev read/write - trying readonly\n");
+      evdev_rw=FALSE;
   dev_id = hci_get_route(NULL);
+  if ((evdev_fd = open(input_evdev_name, O_RDONLY)) < 0)
-  sock = hci_open_dev( dev_id );
+    {
+      printf(" cant open evdev !");
+      return 0;
+    }
+  ioctl(evdev_fd,EVIOCGID,&device_info);
+  printf("vendor 0x%04hx product 0x%04hx version 0x%04hx \n",
+         device_info.vendor, device_info.product,
+         device_info.version);
+  char name[256]= "Unknown";
+  if(ioctl(evdev_fd, EVIOCGNAME(sizeof(name)), name) < 0) {
-  if (dev_id < 0 || sock < 0) {
+    perror("evdev ioctl");
+  }
+  printf("EVDEV reports name: %s\n", name);
-    perror("opening socket");
+  /* this macro is used to tell if "bit" is set in "array"
+   * it selects a byte from the array, and does a boolean AND
-    exit(1);
+   * operation with a byte that only has the relevant bit set.
    * eg. to check for the 12th bit, we do (array[1] & 1<<4)
-  len  = 8;
    */
   flags = IREQ_CACHE_FLUSH;
   ii = (inquiry_info*)malloc(MAX_BT_DEVICES * sizeof(inquiry_info));
+}
-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");
           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;
+void print_device_list()
+{
-  /*
-  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;
+  int i;
-  addr.rc_channel = 1;
-  str2ba( dest, &addr.rc_bdaddr );
-  // connect to server
-  status = connect(s, (struct sockaddr *)&addr, sizeof(addr));
-  return status;
+  for(i=0;i<bt_device_count;i++)
-  */
-}
     printf("device%i->%s\n",i,names[i]);
+}
 int main(int argc, char**argv)
+{
   printf("Starting Riddim \n");
-  printf("\tRemote Interactive Digital Drone Interface Mashup\n");
+  printf("\tRemote Interactive Digital Drone Interface Mashup\n");
+  printf("\nusage:\n");
   printf("\t riddim [config_file]\n\n");
   cfg_opt_t opts[] = {
+    CFG_SIMPLE_BOOL("exit_after_init", &exit_after_init),
+    CFG_SIMPLE_STR("input_evdev", &input_evdev_name),
+    CFG_SIMPLE_INT("loop_delay", &loop_delay),
+    CFG_SIMPLE_INT("mk_socket_port", &mk_socket_port),
+    CFG_SIMPLE_FLOAT("nick_mul", &nick_mul),
+    CFG_SIMPLE_FLOAT("roll_mul", &roll_mul),
+    CFG_SIMPLE_FLOAT("gier_mul", &gier_mul),
+    CFG_SIMPLE_FLOAT("gas_mul", &gas_mul),
+    CFG_SIMPLE_INT("rel_axis_nick", &rel_axis_nick),
+    CFG_SIMPLE_INT("rel_axis_roll", &rel_axis_roll),
     CFG_SIMPLE_INT("rel_axis_gier", &rel_axis_gier),
-  cfg_opt_t opts[] = {
+    CFG_SIMPLE_INT("rel_axis_gas", &rel_axis_gas),
     CFG_SIMPLE_STR("input_evdev", &input_evdev_name),
     CFG_END()
   };
-  cfg_t *cfg;
-  //  input_evdev_name=strdup("/dev/event0");
-  printf("Parsing config file");
+  cfg_t *cfg;
-  cfg = cfg_init(opts, 0);
+  //  input_evdev_name=strdup("/dev/event0");
-  cfg_parse(cfg, "/etc/riddim.conf");
-  printf("input %s:",input_evdev_name);
+  printf("Parsing config file ");
   // 1st argument -> Bluetooth adrees to bypass scanning ( takes to long for short testing roundtrips )
+  cfg = cfg_init(opts, 0);
   if (argv[1])
     {
-      if (connect_mk(argv[1])==-1)
+      printf("%s\n ",argv[1]);
-        {
+      cfg_parse(cfg, argv[1]);
-          printf("cant connect to QC at adress: %s\n",argv[1]);
+    }
+  else
+    {
+      printf("/etc/riddim.conf\n");
+      cfg_parse(cfg, "/etc/riddim.conf");
+    }
+  printf("input %s:\n",input_evdev_name);
+  // 1st argument -> Bluetooth adrees to bypass scanning ( takes to long for short testing roundtrips )
+  if (mk_socket_port)
+    {
+        printf("connecting to local port: %i\n",mk_socket_port);
           return 0;
       if (connect_mk_localhost_socket(mk_socket_port)==-1)
-        printf("connected to QC at adress: %s\n",argv[1]);
+        printf("cant connect !!");
-        connected=1;
+      else
   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);
-      printf(" %d Devices found \n",bt_device_count);
+      print_device_list() ;
     }
   */
+  int v_old;
+  int polls=0;
+  int retval;
-  int v_old;
+  if (exit_after_init)
+    exit(0);
+  printf("starting loop ..\n");
+  struct input_event led_event;
+  led_event.type=EV_LED;
+  int complete_misses=0;
+  if (evdev_out_fd)
+    {
+      led_event.code = LED_MISC;
-  int polls=0;
+      led_event.value = 1;
-  printf("starting loop ..\n");
+      retval = write(evdev_out_fd, &led_event, sizeof(struct input_event));
+    }
+  int confirm_misses;
+  while( 1 )
+    {
+      if (evdev_out_fd)
+        {
+          if (led_event.value)
+            led_event.value = 0;
           else
             led_event.value = 1 ;
-  while( 1 )
-    {
+          retval = write(evdev_out_fd, &led_event, sizeof(struct input_event));
+        }
-      switch (input)
+      usleep(loop_delay);
       switch (input)
-        case INPUT_NONE:
+        {
+        case INPUT_NONE:
+          printf("starting input none\n");
+          break;
+        case INPUT_EVDEV:
+           printf("input evdev\n");
+           struct timeval tv;
+           int retval;
+           fd_set rfds;
+           FD_ZERO(&rfds);
+           FD_SET(evdev_fd,&rfds);
           printf("input none\n");
-          usleep(10000);
+           tv.tv_sec = 0;
            tv.tv_usec = 5;
           break;
            retval = select(evdev_fd+1, &rfds, NULL, NULL, &tv);
         case INPUT_EVDEV:
-           printf("input evdev\n");
+           if (retval==-1)
-          usleep(30000);
+             printf("error in select!!!!!!!!\n");
-        read_bytes = read(evdev_fd, ev, sizeof(struct input_event) * 64);
-           printf("read done\n");
-        if (read_bytes < (int) sizeof(struct input_event)) {
-            perror("evtest: short read");
+           else if (retval)
-            exit (1);
+             {
-        }
-        for (yalv = 0; yalv < (int) (read_bytes / sizeof(struct input_event)); yalv++)
+             read_bytes = read(evdev_fd, ev, sizeof(struct input_event) * 64);
-            {
-              // 1 -> nick
-              // 2 -> gas
+             if (read_bytes < (int) sizeof(struct input_event)) {
+               perror("evtest: short read");
+               exit (1);
+             }
+             for (yalv = 0; yalv < (int) (read_bytes / sizeof(struct input_event)); yalv++)
+               {
+                 printf("%d type:%d code:%d val:%d \n",yalv,ev[yalv].type,ev[yalv].code,ev[yalv].value);
+                 if (ev[yalv].type==EV_REL) axis[ ev[yalv].code]= ev[yalv].value;
+                 if (ev[yalv].type==EV_KEY) button[ ev[yalv].code-256]= ev[yalv].value;
+               }
               // 4->roll
-              // 5-> gier
+             for (yalv=0;yalv<10;yalv++)
-              axis[ ev[yalv].code]= ev[yalv].value;
+               printf("A%d %d -" , yalv, axis[yalv] );
              printf("\n");
               printf("nick:%d roll:%d gier:%d gas:%d\n",axis[3] , axis[4] , axis[5] , axis[2]);
           ExternControl.Config=1;
           printf("sending data\n");
-          SendOutData('b', 0, (unsigned char *)&ExternControl, sizeof(ExternControl));
+          if (connected)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]);
+              //connect_mk(addrs[selected_bt_device]);
               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:
+          confirm_misses=0;
+          printf("connected:%d",connected);
           if (connected) while (RxBuffer[1]!='t')
         case STATEID_CONNECTING:
-          RxBuffer[1]=0;
+              RxBuffer[1]=0;
-          //      ftime(&time_struct);
           printf("waiting for confirm frame\n");
-          RxBuffer[2]=0;
+              //          ftime(&time_struct);
-          int confirm_misses=0;
+              printf("waiting for confirm frame ( misses:%d )\n",confirm_misses);
               RxBuffer[2]=0;
-          while (RxBuffer[2]!='t')
               r=0;
               in_char='#';
                       RxBuffer[r++]='0';
+                    }
                   //               printf("\ncount:%d r:%d %d %c \n",count , r, in_char, in_char);
+                }
               RxBuffer[r++]='\0';
-              printf("--->%s\n",RxBuffer);
+              printf("%d--->%s\n",complete_misses,RxBuffer);
               // new
               if (button_trigger[12]>1)
+                {
                   SendOutData('s', 0, (unsigned char *)&ExternControl, sizeof(ExternControl));
                   button_trigger[12]=0;
+                }
               if (++confirm_misses>4)
+                {
+                  complete_misses++;
+                  printf("sending again\n");
-              SendOutData('b', 0, (unsigned char *)&ExternControl, sizeof(ExternControl));
+                  SendOutData('b', 0, (unsigned char *)&ExternControl, sizeof(ExternControl));
+                }
+            }
+          else
+            printf("not connected to mk\n");
           gettimeofday(&time_struct2,NULL);
           printf("last trip: %d",(int)(time_struct1.tv_usec-time_struct2.tv_usec));
+          act_mode=button[24] | (button[25]<<1);
+          // Step converting axis data to nick/roll/gier/gas/..
+          act_nick=axis[rel_axis_nick]*nick_mul;
+          act_roll=axis[rel_axis_roll]*roll_mul;
+          act_gier=axis[rel_axis_gier]*gier_mul;
+          act_gas=axis[rel_axis_gas]*gas_mul;
+          act_gas=255;
           act_mode=button[24] | (button[25]<<1);
           /*
           switch (act_mode)
             {
             case 0:
-              act_nick=(axis[AXIS_NICK]>>8)*(INVERT_NICK);
+              act_nick=(axis[AXIS_NICK])*(INVERT_NICK);
-              act_roll=(axis[AXIS_ROLL]>>8)*(INVERT_ROLL);
+              act_roll=(axis[AXIS_ROLL])*(INVERT_ROLL);
-              act_gier=(axis[AXIS_GIER]>>8)*(INVERT_GIER);
+              act_gier=(axis[AXIS_GIER])*(INVERT_GIER);
               break;
+            }
           */
           ExternControl.Digital[0]=0;
           ExternControl.Digital[1]=0;
           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);
           if (connected)
+            {
-          printf("sending data\n");
+              printf("sending data\n");
-          SendOutData('b', 0, (unsigned char *)&ExternControl, sizeof(ExternControl));
+              SendOutData('b', 0, (unsigned char *)&ExternControl, sizeof(ExternControl));
-          gettimeofday(&time_struct1,NULL);
+              gettimeofday(&time_struct1,NULL);
-          printf("sent data\n");
+              printf("sent data\n");
           break;
+        }
       printf("\n");
       fflush(stdout);
-      printf("loop fin");
+      printf("loop fin ( misses:%d)\n",complete_misses);
+    }
   /******************** Cleanup **********************/
   close(x52_input_fd);

Subversion Repositories Projects

(root)/Riddim/riddim.c @ 130 - Rev 125 → 127