WebSVN - Projects - /Riddim/riddim.c

#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

// 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("/dev/input/event10", 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");

// 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 ..\n");
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;
}

Subversion Repositories Projects

(root)/Riddim/riddim.c @ 117 - Rev