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 <bluetooth/rfcomm.h>

#include <linux/joystick.h>

#define JOY_DEV "/dev/input/js0"

#define MAX_BT_DEVICES 3

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

#define STATEID_SCANNING 0

#define STATEID_CONNECTING 1

int state=STATEID_SCANNING;

#define BUTTON_SELECT 26

#define BUTTON_DOWN 27

#define BUTTON_UP 28

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

void connect_joy()

{

  if( ( x52_input_fd = open( JOY_DEV, O_RDONLY ) ) < 0 )

    {

      printf( "Couldn't open joystick device %s\n", JOY_DEV );

      return ;

    }

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

}

struct

{

  char val[4];

} MotortestParam;

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, int len)

{

  unsigned int pt = 0,ptr=0,i;

  int a,b,c;

  TxBuffer[pt++] = '#';           // Startzeichen

  TxBuffer[pt++] = modul;        // Adresse (a=0; b=1,...)

  TxBuffer[pt++] = cmd;          // Commando

  while(len)

    {

      if(len) { a = _TxBuffer[ptr++]; len--;} else a = 0;

      if(len) { b = _TxBuffer[ptr++]; len--;} else b = 0;

      if(len) { c = _TxBuffer[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);

    }

  status = send(s,"\r" , 1, 0);

  i=0;

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

  printf("\n");

  AddCRC(pt);

  printf("Sending to MK\n");

}

int engines_on=0;

int old_engines_on=0;

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

        }

    }

}

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

}

int main(int argc, char**argv)

{

  printf("Starting Riddim \n");

  printf("\tRemote Interactive Digital Drone Interface Mashup\n");

  //int tmp=2;

  //  connect_mk("00:0B:CE:01:6B:4F");

  /*

  {

  // send a message

  MotortestParam.val[0]=tmp;

  MotortestParam.val[1]=tmp;

  MotortestParam.val[2]=tmp;

  MotortestParam.val[3]=tmp;

  engines_on=1;

  else

  engines_on=0;

  if (engines_on!=old_engines_on)

  {

  int c;

  if( 0 == status )for (c=0;c<10;c++)

  int c;

  for (c=0;c<2;c++)

  SendOutData('t', 0,  &MotortestParam, sizeof(MotortestParam));

  sleep(1);

  }

        */

    if( status < 0 ) perror("uh oh");

    printf ("send status %d",status);

    //    close(s);

 int i;

  /*

  MotortestParam.val[0]=2;

  MotortestParam.val[1]=2;

  MotortestParam.val[2]=2;

  MotortestParam.val[3]=2;

  while(1)

    {

      initSerial("/dev/rfcomm0");

      sleep(2);

      SendOutData('t', 0,  &MotortestParam, sizeof(MotortestParam));

      sleep(1);

    }

  */

  printf("\nInitializing X-52 input ..\n");

  connect_joy();

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

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

    {

      printf(" %d -> %s (%s) \n",i,names[i],addrs[i]);  

      if (i<3) write_display(i,names[i]);

    }

  output_device_list() ;

  int v_old;

  while( 1 )    

    {

      int polls=0;

      for (polls=0;polls<1000;polls++)

        {

      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;

                }

        }

                if (1)

                  {

                  }

                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:

                    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:

                    _TxBuffer[0]=(axis[2]>>8)*(-1)+127;

                    if (button[7]!=1)_TxBuffer[0] =0;

                    _TxBuffer[1]=_TxBuffer[0];

                    _TxBuffer[2]=_TxBuffer[0];

                    _TxBuffer[3]=_TxBuffer[0];

                    SendOutData('t', 0, 4);

                    sleep(0.05);       

                    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 \r",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("\r");

fflush(stdout);

                /*

      if( read( x52_input_fd, &x52_input_struct, JS_RETURN ) != JS_RETURN )

        {

          printf( "\nFailed to read from Joystick\n" );

        }

      if (x52_output) x52_setbri(x52_output, 1,x52_input_struct.y );  

      //      if (x52_output) x52_setbri(x52_output, 2,x52_input_struct.x );  

*/

      }

  close(x52_input_fd);  /* too bad we never get here */

  if (x52_output) x52_close(x52_output);

  return 0;

}