Subversion Repositories Projects

#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

#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;
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 connect_joy()
{

 
  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 ;

  // 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 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(".. done");//


  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;
  while( 1 )    
    {
      // poll values from input device
      int polls=0;
      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]++;
          }


               
      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);
          while (RxBuffer[1]!='t')
            {

              r=0;
              in_char='#';

              while(in_char!='\r')
                {
                  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);
                }
              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:
              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.Gier=(axis[5]>>8);
          ExternControl.Gas=(axis[2]>>8)*(-1)+127;
          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");
          // usleep(10000);
          // 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;
}