WebSVN - Projects - /Riddim/riddim.c

/**************************************************
*
*
* Riddim
* Remote Interactive Digital Drone Interface Mashup
*
* 2007-2008 Marcus -LiGi- Bueschleb
*
*
**************************************************/

#include "riddim.h"

#define FALSE 0
#define TRUE 1

#define test_bit(bit, array) (array[bit/8] & (1<<(bit%8)))

int state=STATEID_SCANNING;

// from config
char *input_evdev_name;
int mk_socket_port=0;
int loop_delay=0;

double nick_mul=0.3f;
double roll_mul=0.3f;
double gier_mul=0.3f;
double gas_mul=0.3f;

int rel_axis_nick=1;
int rel_axis_roll=0;
int rel_axis_gier=5;
int rel_axis_gas=2;

cfg_bool_t exit_after_init = cfg_false;

// time struct for measuring
struct timeval time_struct1;
struct timeval time_struct2;

char RxBuffer[150];

int status;

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 x52 *x52_output;

int selected_bt_device=0;

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

int evdev_rw=TRUE;

int connect_evdev()
{

struct input_devinfo {
uint16_t bustype;
uint16_t vendor;
uint16_t product;
uint16_t version;
};
struct input_devinfo device_info;

if ((evdev_out_fd = open(input_evdev_name, O_WRONLY)) < 0)
{
printf(" cant open evdev read/write - trying readonly\n");
evdev_rw=FALSE;
}
if ((evdev_fd = open(input_evdev_name, O_RDONLY)) < 0)
{
printf(" cant open evdev !");
return 0;
}

ioctl(evdev_fd,EVIOCGID,&device_info);

printf("vendor 0x%04hx product 0x%04hx version 0x%04hx \n",
device_info.vendor, device_info.product,
device_info.version);

char name[256]= "Unknown";

if(ioctl(evdev_fd, EVIOCGNAME(sizeof(name)), name) < 0) {
perror("evdev ioctl");
}

printf("EVDEV reports name: %s\n", name);

/* this macro is used to tell if "bit" is set in "array"
* it selects a byte from the array, and does a boolean AND
* operation with a byte that only has the relevant bit set.
* eg. to check for the 12th bit, we do (array[1] & 1<<4)
*/

uint8_t evtype_bitmask[EV_MAX/8 + 1];

if(ioctl(evdev_fd, EVIOCGBIT(0, sizeof(evtype_bitmask)), evtype_bitmask) < 0)
perror("evdev ioctl");

printf("Supported event types:\n");
for (yalv = 0; yalv < EV_MAX; yalv++) {
if (test_bit(yalv, evtype_bitmask)) {
/* this means that the bit is set in the event types list */
printf(" Event type 0x%02x ", yalv);
switch ( yalv)
{
case EV_KEY :
printf(" (Keys or Buttons)\n");
break;
case EV_ABS :
printf(" (Absolute Axes)\n");
break;
case EV_LED :
printf(" (LEDs)\n");
break;
case EV_REP :
printf(" (Repeat)\n");
break;
case EV_SYN :
printf(" (Sync?)\n");
break;
case EV_REL :
printf(" (Relative Axis)\n");
break;
case EV_MSC :
printf(" (Misc)\n");
break;
default:
printf(" (Unknown event type: 0x%04hx)\n", yalv);
}}}

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 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 print_device_list()
{
int i;
for(i=0;i<bt_device_count;i++)
printf("device%i->%s\n",i,names[i]);
}

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");
printf("\nusage:\n");
printf("\t riddim [config_file]\n\n");

cfg_opt_t opts[] = {

CFG_SIMPLE_BOOL("exit_after_init", &exit_after_init),
CFG_SIMPLE_STR("input_evdev", &input_evdev_name),
CFG_SIMPLE_INT("loop_delay", &loop_delay),
CFG_SIMPLE_INT("mk_socket_port", &mk_socket_port),

CFG_SIMPLE_FLOAT("nick_mul", &nick_mul),
CFG_SIMPLE_FLOAT("roll_mul", &roll_mul),
CFG_SIMPLE_FLOAT("gier_mul", &gier_mul),
CFG_SIMPLE_FLOAT("gas_mul", &gas_mul),

CFG_SIMPLE_INT("rel_axis_nick", &rel_axis_nick),
CFG_SIMPLE_INT("rel_axis_roll", &rel_axis_roll),
CFG_SIMPLE_INT("rel_axis_gier", &rel_axis_gier),
CFG_SIMPLE_INT("rel_axis_gas", &rel_axis_gas),

CFG_END()
};

cfg_t *cfg;

// input_evdev_name=strdup("/dev/event0");
printf("Parsing config file ");

cfg = cfg_init(opts, 0);

if (argv[1])
{
printf("%s\n ",argv[1]);
cfg_parse(cfg, argv[1]);
}
else
{
printf("/etc/riddim.conf\n");
cfg_parse(cfg, "/etc/riddim.conf");
}
printf("input %s:\n",input_evdev_name);

// 1st argument -> Bluetooth adrees to bypass scanning ( takes to long for short testing roundtrips )

if (mk_socket_port)
{
printf("connecting to local port: %i\n",mk_socket_port);

if (connect_mk_localhost_socket(mk_socket_port)==-1)
printf("cant connect !!");
else
{
printf("connected to QC at adress: %s\n",argv[1]);
connected=TRUE;
}
}

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);
print_device_list() ;
}
*/

int v_old;
int polls=0;

int retval;
if (exit_after_init)
exit(0);
printf("starting loop ..\n");

struct input_event led_event;
led_event.type=EV_LED;

int complete_misses=0;
if (evdev_out_fd)
{
led_event.code = LED_MISC;
led_event.value = 1;
retval = write(evdev_out_fd, &led_event, sizeof(struct input_event));
}

int confirm_misses;
while( 1 )
{

if (evdev_out_fd)
{
if (led_event.value)
led_event.value = 0;
else
led_event.value = 1 ;
retval = write(evdev_out_fd, &led_event, sizeof(struct input_event));
}

usleep(loop_delay);
switch (input)
{

case INPUT_NONE:
printf("starting input none\n");
break;

case INPUT_EVDEV:
printf("input evdev\n");

struct timeval tv;
int retval;
fd_set rfds;
FD_ZERO(&rfds);
FD_SET(evdev_fd,&rfds);

tv.tv_sec = 0;
tv.tv_usec = 5;

retval = select(evdev_fd+1, &rfds, NULL, NULL, &tv);

if (retval==-1)
printf("error in select!!!!!!!!\n");
else if (retval)
{
read_bytes = read(evdev_fd, ev, sizeof(struct input_event) * 64);

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

printf("%d type:%d code:%d val:%d \n",yalv,ev[yalv].type,ev[yalv].code,ev[yalv].value);
if (ev[yalv].type==EV_REL) axis[ ev[yalv].code]= ev[yalv].value;
if (ev[yalv].type==EV_KEY) button[ ev[yalv].code-256]= ev[yalv].value;
}

for (yalv=0;yalv<10;yalv++)
printf("A%d %d -" , yalv, axis[yalv] );
printf("\n");

for (yalv=0;yalv<10;yalv++)
printf("B%d %d -" , yalv, button[yalv] );
// printf("input read done: nick:%d roll:%d gier:%d gas:%d\n",axis[3] , axis[4] , axis[5] , axis[2]);
}
else
printf("no data from evdev data from evdev: \n");

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

if (connected)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++;

}
break;

case STATEID_CONNECTING:

confirm_misses=0;
printf("connected:%d",connected);
if (connected) while (RxBuffer[1]!='t')
{

RxBuffer[1]=0;

// ftime(&time_struct);
printf("waiting for confirm frame ( misses:%d )\n",confirm_misses);
RxBuffer[2]=0;

r=0;
in_char='#';

while(in_char!='\n')
{
count=read(mk_socket,&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("%d--->%s\n",complete_misses,RxBuffer);
// new
if (button_trigger[12]>1)
{
SendOutData('s', 0, (unsigned char *)&ExternControl, sizeof(ExternControl));
button_trigger[12]=0;
}
if (++confirm_misses>4)
{
complete_misses++;
printf("sending again\n");
SendOutData('b', 0, (unsigned char *)&ExternControl, sizeof(ExternControl));
}
}
else
printf("not connected to mk\n");
gettimeofday(&time_struct2,NULL);

printf("last trip: %d",(int)(time_struct1.tv_usec-time_struct2.tv_usec));
act_mode=button[24] | (button[25]<<1);

// Step converting axis data to nick/roll/gier/gas/..

act_nick=axis[rel_axis_nick]*nick_mul;
act_roll=axis[rel_axis_roll]*roll_mul;
act_gier=axis[rel_axis_gier]*gier_mul;
act_gas=axis[rel_axis_gas]*gas_mul;
act_gas=255;

/*
switch (act_mode)
{
case 0:

act_nick=(axis[AXIS_NICK])*(INVERT_NICK);
act_roll=(axis[AXIS_ROLL])*(INVERT_ROLL);
act_gier=(axis[AXIS_GIER])*(INVERT_GIER);
act_gas=((axis[AXIS_GAS])-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);

if (connected)
{
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 ( misses:%d)\n",complete_misses);

}

/******************** Cleanup **********************/
close(x52_input_fd);
close(mk_socket);

if (x52_output) x52_close(x52_output);
return 0;
}

Subversion Repositories Projects

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