Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 126 → Rev 127

/Riddim/riddim.c
1,106 → 1,56
#include <confuse.h>
#include <string.h>
/**************************************************
*
*
* Riddim
* Remote Interactive Digital Drone Interface Mashup
*
* 2007-2008 Marcus -LiGi- Bueschleb
*
*
**************************************************/
 
#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 "riddim.h"
 
#include <sys/time.h>
#define FALSE 0
#define TRUE 1
 
#define test_bit(bit, array) (array[bit/8] & (1<<(bit%8)))
 
#include <bluetooth/rfcomm.h>
int state=STATEID_SCANNING;
 
 
#include <linux/joystick.h>
#define JOY_DEV "/dev/input/js0"
// from config
char *input_evdev_name;
int mk_socket_port=0;
int loop_delay=0;
 
#define MAX_BT_DEVICES 3
 
 
#define STATEID_SCANNING 0
#define STATEID_CONNECTING 1
double nick_mul=0.3f;
double roll_mul=0.3f;
double gier_mul=0.3f;
double gas_mul=0.3f;
 
#define BUTTON_SELECT 26
#define BUTTON_DOWN 28
#define BUTTON_UP 27
 
int rel_axis_nick=1;
int rel_axis_roll=0;
int rel_axis_gier=5;
int rel_axis_gas=2;
 
// #define AXIS_ROLL 0
// #define AXIS_NICK 1
// #define AXIS_GIER 5
// #define AXIS_GAS 2
 
cfg_bool_t exit_after_init = cfg_false;
 
 
// 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
 
 
// from config
char *input_evdev_name;
 
// 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;
char RxBuffer[150];
 
int bt_device_count=0;
 
char names[MAX_BT_DEVICES][248];
char addrs[MAX_BT_DEVICES][19];
int status;
 
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;
113,87 → 63,101
 
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 x52 *x52_output;
 
int selected_bt_device=0;
 
struct ExternControl_s ExternControl ;
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 state=STATEID_SCANNING;
int evdev_fd;
int evdev_out_fd;
 
int evdev_rw=TRUE;
 
struct x52 *x52_output;
int connect_evdev()
{
 
int selected_bt_device=0;
void scan_bt()
{
inquiry_info *ii = NULL;
struct input_devinfo {
uint16_t bustype;
uint16_t vendor;
uint16_t product;
uint16_t version;
};
struct input_devinfo device_info;
 
int dev_id, sock, len, flags;
int i;
char addr[19] = { 0 };
char name[248] = { 0 };
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;
}
dev_id = hci_get_route(NULL);
sock = hci_open_dev( dev_id );
if (dev_id < 0 || sock < 0) {
perror("opening socket");
exit(1);
}
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);
 
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);
char name[256]= "Unknown";
 
if(ioctl(evdev_fd, EVIOCGNAME(sizeof(name)), name) < 0) {
perror("evdev ioctl");
}
 
free( ii );
close( sock );
}
printf("EVDEV reports name: %s\n", name);
 
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) */
/* 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)
*/
 
int evdev_fd;
int connect_evdev()
{
if ((evdev_fd = open(input_evdev_name, O_RDONLY)) < 0) {
printf(" cant open evdev ");
return 0;
}
 
return 1;
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()
233,69 → 197,9
 
 
 
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));
327,53 → 231,15
}
}
 
int connect_mk(char dest[18])
 
void print_device_list()
{
 
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 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;
387,11 → 253,27
 
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()
};
398,30 → 280,39
cfg_t *cfg;
 
// input_evdev_name=strdup("/dev/event0");
printf("Parsing config file");
printf("Parsing config file ");
cfg = cfg_init(opts, 0);
cfg_parse(cfg, "/etc/riddim.conf");
printf("input %s:",input_evdev_name);
 
// 1st argument -> Bluetooth adrees to bypass scanning ( takes to long for short testing roundtrips )
if (argv[1])
{
if (connect_mk(argv[1])==-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("cant connect to QC at adress: %s\n",argv[1]);
return 0;
printf("connected to QC at adress: %s\n",argv[1]);
connected=TRUE;
}
printf("connected to QC at adress: %s\n",argv[1]);
connected=1;
}
 
int i;
 
 
 
 
 
printf("\nInitializing X-52 input ..\n");
if (connect_joy())
{
454,6 → 345,7
else
printf(" not found \n");
 
/*
if (!connected)
{
printf("Scanning for Bluetooth Devices ..\n");
461,57 → 353,98
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("input none\n");
usleep(10000);
printf("starting input none\n");
break;
 
case INPUT_EVDEV:
printf("input evdev\n");
usleep(30000);
 
struct timeval tv;
int retval;
fd_set rfds;
FD_ZERO(&rfds);
FD_SET(evdev_fd,&rfds);
 
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);
}
tv.tv_sec = 0;
tv.tv_usec = 5;
 
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);
*/
}
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);
 
break;
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
570,7 → 503,7
printf("sending data\n");
 
SendOutData('b', 0, (unsigned char *)&ExternControl, sizeof(ExternControl));
if (connected)SendOutData('b', 0, (unsigned char *)&ExternControl, sizeof(ExternControl));
gettimeofday(&time_struct1,NULL);
 
if (button_trigger[BUTTON_SELECT]==1)
579,7 → 512,7
clear_display();
write_display(0,"connecting to");
write_display(1,names[selected_bt_device]);
connect_mk(addrs[selected_bt_device]);
//connect_mk(addrs[selected_bt_device]);
write_display(0,"connected to");
}
591,29 → 524,33
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;
confirm_misses=0;
printf("connected:%d",connected);
if (connected) while (RxBuffer[1]!='t')
{
 
RxBuffer[1]=0;
 
// ftime(&time_struct);
printf("waiting for confirm frame\n");
RxBuffer[2]=0;
int confirm_misses=0;
 
while (RxBuffer[2]!='t')
{
// 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(s,&in_char,1);
count=read(mk_socket,&in_char,1);
if (in_char!=0)
{
RxBuffer[r++]=in_char;
625,7 → 562,7
// printf("\ncount:%d r:%d %d %c \n",count , r, in_char, in_char);
}
RxBuffer[r++]='\0';
printf("--->%s\n",RxBuffer);
printf("%d--->%s\n",complete_misses,RxBuffer);
// new
if (button_trigger[12]>1)
{
633,23 → 570,40
button_trigger[12]=0;
}
if (++confirm_misses>4)
SendOutData('b', 0, (unsigned char *)&ExternControl, sizeof(ExternControl));
{
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]>>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);
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;
677,7 → 631,7
break;
 
}
*/
ExternControl.Digital[0]=0;
ExternControl.Digital[1]=0;
ExternControl.RemoteTasten=0;
698,24 → 652,26
 
 
 
printf("sending data\n");
 
SendOutData('b', 0, (unsigned char *)&ExternControl, sizeof(ExternControl));
gettimeofday(&time_struct1,NULL);
printf("sent data\n");
 
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");
 
// 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 );
722,13 → 678,13
for( x=0 ; x<num_of_buttons ; ++x )
printf("B%d: %d ", x, button[x] );
break;
}
printf("\n");
fflush(stdout);
printf("loop fin");
printf("loop fin ( misses:%d)\n",complete_misses);
}
 
735,7 → 691,7
 
/******************** Cleanup **********************/
close(x52_input_fd);
close(s);
close(mk_socket);
 
if (x52_output) x52_close(x52_output);
return 0;