Subversion Repositories Projects

#include <confuse.h>

#include <string.h>

#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 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>

#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

// from config 

 char *input_evdev_name;

// 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(input_evdev_name, 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 );

  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");

  cfg_opt_t opts[] = {

    CFG_SIMPLE_STR("input_evdev", &input_evdev_name),

    CFG_END()

  };

  cfg_t *cfg;

  //  input_evdev_name=strdup("/dev/event0");

  printf("Parsing config file");

  cfg = cfg_init(opts, 0);

  cfg_parse(cfg, "/etc/riddim.conf");

  printf("input %s:",input_evdev_name);  

  // 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())

    {

    printf(".. done");//

    input=INPUT_JOYDEV;

    }

  else

    printf(".. ERROR ");//

    printf("\nInitializing evdev input (%s) ..\n",input_evdev_name);

  if (connect_evdev())

    {

    printf(".. done");//

    input=INPUT_EVDEV;

    }

  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 )    

    {

      switch (input)

        {

        case INPUT_NONE:

          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);

           printf("read done\n");

        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++)

            {

              // 1 -> nick

              // 2 -> gas

              // 4->roll

              // 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,

                       ev[yalv].code, ev[yalv].value);

              */

            }

          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("waiting for confirm frame\n");

          RxBuffer[2]=0;

          int confirm_misses=0;

          while (RxBuffer[2]!='t')

            {

              r=0;

              in_char='#';

              while(in_char!='\n')

                {

                  count=read(s,&in_char,1);

                  if (in_char!=0)

                    {

                      RxBuffer[r++]=in_char;

                    }

                  else

                    {

                      RxBuffer[r++]='0';

                    }

                  //               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)-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*(-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("sending data\n");

          SendOutData('b', 0, (unsigned char *)&ExternControl, sizeof(ExternControl));

          gettimeofday(&time_struct1,NULL);

          printf("sent data\n");

          // printf("sleeping\n");

          //      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;

}