Rev 43 |
Go to most recent revision |
Blame |
Last modification |
View Log
| RSS feed
#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
#define STATEID_SCANNING 0
#define STATEID_CONNECTING 1
#define BUTTON_SELECT 26
#define BUTTON_DOWN 28
#define BUTTON_UP 27
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];
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
{
char val
[4];
} MotortestParam
;
struct
{
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 Hight
; //(Höhenregler)
unsigned char free; // (unbenutzt)
unsigned char Frame
; // (Bestätigung)
unsigned char Config
;
} 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
);
}
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 */
}
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");
int i
;
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
++) // 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
:
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
);
}
/******************** Cleanup **********************/
close
(x52_input_fd
);
close
(s
);
if (x52_output
) x52_close
(x52_output
);
return 0;
}