Rev 48 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 48 | Rev 50 | ||
---|---|---|---|
1 | #include <stdio.h> |
1 | #include <stdio.h> |
2 | #include <stdlib.h> |
2 | #include <stdlib.h> |
3 | #include <string.h> |
3 | #include <string.h> |
4 | #include <fcntl.h> |
4 | #include <fcntl.h> |
5 | #include "lib/x52/x52.h" |
5 | #include "lib/x52/x52.h" |
6 | #include <unistd.h> |
6 | #include <unistd.h> |
7 | #include <sys/socket.h> |
7 | #include <sys/socket.h> |
8 | #include <bluetooth/bluetooth.h> |
8 | #include <bluetooth/bluetooth.h> |
9 | #include <bluetooth/hci.h> |
9 | #include <bluetooth/hci.h> |
10 | #include <bluetooth/hci_lib.h> |
10 | #include <bluetooth/hci_lib.h> |
- | 11 | ||
- | 12 | #include <sys/time.h> |
|
11 | 13 | ||
12 | 14 | ||
13 | #include <bluetooth/rfcomm.h> |
15 | #include <bluetooth/rfcomm.h> |
14 | 16 | ||
15 | 17 | ||
16 | #include <linux/joystick.h> |
18 | #include <linux/joystick.h> |
17 | #define JOY_DEV "/dev/input/js0" |
19 | #define JOY_DEV "/dev/input/js0" |
18 | 20 | ||
19 | #define MAX_BT_DEVICES 3 |
21 | #define MAX_BT_DEVICES 3 |
20 | 22 | ||
21 | 23 | ||
22 | #define STATEID_SCANNING 0 |
24 | #define STATEID_SCANNING 0 |
23 | #define STATEID_CONNECTING 1 |
25 | #define STATEID_CONNECTING 1 |
24 | 26 | ||
25 | #define BUTTON_SELECT 26 |
27 | #define BUTTON_SELECT 26 |
26 | #define BUTTON_DOWN 28 |
28 | #define BUTTON_DOWN 28 |
27 | #define BUTTON_UP 27 |
29 | #define BUTTON_UP 27 |
- | 30 | ||
- | 31 | ||
- | 32 | #define AXIS_ROLL 0 |
|
- | 33 | #define AXIS_NICK 1 |
|
- | 34 | #define AXIS_GIER 5 |
|
- | 35 | #define AXIS_GAS 2 |
|
- | 36 | ||
- | 37 | #define INVERT_ROLL -1 |
|
- | 38 | #define INVERT_NICK -1 |
|
- | 39 | #define INVERT_GIER 1 |
|
- | 40 | #define INVERT_GAS -1 |
|
- | 41 | ||
- | 42 | ||
- | 43 | // time struct for measuring |
|
- | 44 | struct timeval time_struct1; |
|
- | 45 | struct timeval time_struct2; |
|
- | 46 | ||
- | 47 | ||
- | 48 | int act_nick=0; |
|
- | 49 | int act_roll=0; |
|
- | 50 | int act_gier=0; |
|
- | 51 | int act_gas=0; |
|
- | 52 | int act_mode=0; |
|
28 | 53 | ||
29 | int bt_device_count=0; |
54 | int bt_device_count=0; |
30 | 55 | ||
31 | char names[MAX_BT_DEVICES][248]; |
56 | char names[MAX_BT_DEVICES][248]; |
32 | char addrs[MAX_BT_DEVICES][19]; |
57 | char addrs[MAX_BT_DEVICES][19]; |
33 | 58 | ||
34 | int s, status; |
59 | int s, status; |
35 | unsigned char TxBuffer[150]; |
60 | unsigned char TxBuffer[150]; |
36 | unsigned char _TxBuffer[150]; |
61 | unsigned char _TxBuffer[150]; |
37 | 62 | ||
38 | char RxBuffer[150]; |
63 | char RxBuffer[150]; |
39 | 64 | ||
40 | 65 | ||
41 | int x52_input_fd, *axis=NULL, num_of_axis=0, num_of_buttons=0, x; |
66 | int x52_input_fd, *axis=NULL, num_of_axis=0, num_of_buttons=0, x; |
42 | char *button=NULL,*button_trigger=NULL, name_of_joystick[80]; |
67 | char *button=NULL,*button_trigger=NULL, name_of_joystick[80]; |
43 | 68 | ||
44 | struct js_event x52_event_struct; |
69 | struct js_event x52_event_struct; |
45 | - | ||
46 | 70 | ||
47 | int engines_on=0; |
71 | int engines_on=0; |
48 | int old_engines_on=0; |
72 | int old_engines_on=0; |
49 | 73 | ||
50 | char in_char; |
74 | char in_char; |
51 | - | ||
52 | struct |
- | |
53 | { |
- | |
54 | char val[4]; |
75 | |
55 | } MotortestParam; |
- | |
56 | - | ||
57 | struct |
76 | struct ExternControl_s |
58 | { |
77 | { |
59 | unsigned char Digital[2]; // (noch unbenutzt) |
78 | unsigned char Digital[2]; // (noch unbenutzt) |
60 | unsigned char RemoteTasten; //(gab es schon für das virtuelle Display) |
79 | unsigned char RemoteTasten; //(gab es schon für das virtuelle Display) |
61 | signed char Nick; |
80 | signed char Nick; |
62 | signed char Roll; |
81 | signed char Roll; |
63 | signed char Gier; |
82 | signed char Gier; |
64 | unsigned char Gas; //(es wird das Stick-Gas auf diesen Wert begrenzt; --> StickGas ist das Maximum) |
83 | unsigned char Gas; //(es wird das Stick-Gas auf diesen Wert begrenzt; --> StickGas ist das Maximum) |
65 | signed char Higt; //(Höhenregler) |
84 | signed char Higt; //(Höhenregler) |
66 | unsigned char free; // (unbenutzt) |
85 | unsigned char free; // (unbenutzt) |
67 | unsigned char Frame; // (Bestätigung) |
86 | unsigned char Frame; // (Bestätigung) |
68 | unsigned char Config; |
87 | unsigned char Config; |
69 | } ExternControl; |
88 | }; |
- | 89 | ||
70 | 90 | ||
71 | 91 | struct ExternControl_s ExternControl ; |
|
72 | 92 | ||
73 | int state=STATEID_SCANNING; |
93 | int state=STATEID_SCANNING; |
74 | 94 | ||
75 | 95 | ||
76 | struct x52 *x52_output; |
96 | struct x52 *x52_output; |
77 | 97 | ||
78 | int selected_bt_device=0; |
98 | int selected_bt_device=0; |
79 | void scan_bt() |
99 | void scan_bt() |
80 | { |
100 | { |
81 | inquiry_info *ii = NULL; |
101 | inquiry_info *ii = NULL; |
82 | 102 | ||
83 | int dev_id, sock, len, flags; |
103 | int dev_id, sock, len, flags; |
84 | int i; |
104 | int i; |
85 | char addr[19] = { 0 }; |
105 | char addr[19] = { 0 }; |
86 | char name[248] = { 0 }; |
106 | char name[248] = { 0 }; |
87 | 107 | ||
88 | dev_id = hci_get_route(NULL); |
108 | dev_id = hci_get_route(NULL); |
89 | sock = hci_open_dev( dev_id ); |
109 | sock = hci_open_dev( dev_id ); |
90 | if (dev_id < 0 || sock < 0) { |
110 | if (dev_id < 0 || sock < 0) { |
91 | perror("opening socket"); |
111 | perror("opening socket"); |
92 | exit(1); |
112 | exit(1); |
93 | } |
113 | } |
94 | 114 | ||
95 | len = 8; |
115 | len = 8; |
96 | 116 | ||
97 | flags = IREQ_CACHE_FLUSH; |
117 | flags = IREQ_CACHE_FLUSH; |
98 | ii = (inquiry_info*)malloc(MAX_BT_DEVICES * sizeof(inquiry_info)); |
118 | ii = (inquiry_info*)malloc(MAX_BT_DEVICES * sizeof(inquiry_info)); |
99 | 119 | ||
100 | bt_device_count = hci_inquiry(dev_id, len, MAX_BT_DEVICES, NULL, &ii, flags); |
120 | bt_device_count = hci_inquiry(dev_id, len, MAX_BT_DEVICES, NULL, &ii, flags); |
101 | if( bt_device_count < 0 ) perror("hci_inquiry"); |
121 | if( bt_device_count < 0 ) perror("hci_inquiry"); |
102 | 122 | ||
103 | for (i = 0; i < bt_device_count; i++) { |
123 | for (i = 0; i < bt_device_count; i++) { |
104 | ba2str(&(ii+i)->bdaddr, addr); |
124 | ba2str(&(ii+i)->bdaddr, addr); |
105 | sprintf(addrs[i],"%s",addr); |
125 | sprintf(addrs[i],"%s",addr); |
106 | 126 | ||
107 | memset(name, 0, sizeof(name)); |
127 | memset(name, 0, sizeof(name)); |
108 | 128 | ||
109 | if (hci_read_remote_name(sock, &(ii+i)->bdaddr, sizeof(name), |
129 | if (hci_read_remote_name(sock, &(ii+i)->bdaddr, sizeof(name), |
110 | name, 0) < 0) |
130 | name, 0) < 0) |
111 | sprintf(names[i],"[unknown]"); |
131 | sprintf(names[i],"[unknown]"); |
112 | else |
132 | else |
113 | sprintf(names[i],"%s",name); |
133 | sprintf(names[i],"%s",name); |
114 | 134 | ||
115 | } |
135 | } |
116 | 136 | ||
117 | 137 | ||
118 | free( ii ); |
138 | free( ii ); |
119 | close( sock ); |
139 | close( sock ); |
120 | } |
140 | } |
121 | 141 | ||
122 | 142 | ||
123 | void connect_joy() |
143 | int connect_joy() |
124 | { |
144 | { |
125 | 145 | ||
126 | 146 | ||
127 | if( ( x52_input_fd = open( JOY_DEV, O_RDONLY ) ) < 0 ) |
147 | if( ( x52_input_fd = open( JOY_DEV, O_RDONLY ) ) < 0 ) |
128 | { |
148 | { |
129 | printf( "Couldn't open joystick device %s\n", JOY_DEV ); |
149 | printf( "Couldn't open joystick device %s\n", JOY_DEV ); |
- | 150 | printf( "try modprobe joydev\n" ); |
|
130 | return ; |
151 | return 0; |
131 | } |
152 | } |
132 | 153 | ||
133 | ioctl( x52_input_fd, JSIOCGAXES, &num_of_axis ); |
154 | ioctl( x52_input_fd, JSIOCGAXES, &num_of_axis ); |
134 | ioctl( x52_input_fd, JSIOCGBUTTONS, &num_of_buttons ); |
155 | ioctl( x52_input_fd, JSIOCGBUTTONS, &num_of_buttons ); |
135 | ioctl( x52_input_fd, JSIOCGNAME(80), &name_of_joystick ); |
156 | ioctl( x52_input_fd, JSIOCGNAME(80), &name_of_joystick ); |
136 | 157 | ||
137 | axis = (int *) calloc( num_of_axis, sizeof( int ) ); |
158 | axis = (int *) calloc( num_of_axis, sizeof( int ) ); |
138 | button = (char *)calloc( num_of_buttons, sizeof( char ) ); |
159 | button = (char *)calloc( num_of_buttons, sizeof( char ) ); |
139 | button_trigger = (char *) calloc( num_of_buttons, sizeof( char ) ); |
160 | button_trigger = (char *) calloc( num_of_buttons, sizeof( char ) ); |
140 | 161 | ||
141 | printf("Joystick detected: %s\n\t%d axis\n\t%d buttons\n\n" |
162 | printf("Joystick detected: %s\n\t%d axis\n\t%d buttons\n\n" |
142 | , name_of_joystick |
163 | , name_of_joystick |
143 | , num_of_axis |
164 | , num_of_axis |
144 | , num_of_buttons ); |
165 | , num_of_buttons ); |
145 | 166 | ||
146 | fcntl( x52_input_fd, F_SETFL, O_NONBLOCK ); /* use non-blocking mode */ |
167 | fcntl( x52_input_fd, F_SETFL, O_NONBLOCK ); /* use non-blocking mode */ |
- | 168 | ||
147 | 169 | return 1; |
|
148 | } |
170 | } |
149 | 171 | ||
150 | 172 | ||
151 | 173 | ||
152 | 174 | ||
153 | void AddCRC(unsigned int wieviele) |
175 | void AddCRC(unsigned int wieviele) |
154 | { |
176 | { |
155 | unsigned int tmpCRC = 0,i; |
177 | unsigned int tmpCRC = 0,i; |
156 | for(i = 0; i < wieviele;i++) |
178 | for(i = 0; i < wieviele;i++) |
157 | { |
179 | { |
158 | tmpCRC += TxBuffer[i]; |
180 | tmpCRC += TxBuffer[i]; |
159 | } |
181 | } |
160 | tmpCRC %= 4096; |
182 | tmpCRC %= 4096; |
161 | TxBuffer[i++] = '=' + tmpCRC / 64; |
183 | TxBuffer[i++] = '=' + tmpCRC / 64; |
162 | TxBuffer[i++] = '=' + tmpCRC % 64; |
184 | TxBuffer[i++] = '=' + tmpCRC % 64; |
163 | TxBuffer[i++] = '\r'; |
185 | TxBuffer[i++] = '\r'; |
164 | } |
186 | } |
165 | 187 | ||
166 | 188 | ||
167 | void SendOutData(unsigned char cmd,unsigned char modul, unsigned char *snd, unsigned char len) |
189 | void SendOutData(unsigned char cmd,unsigned char modul, unsigned char *snd, unsigned char len) |
168 | { |
190 | { |
169 | unsigned int pt = 0; |
191 | unsigned int pt = 0; |
170 | unsigned char a,b,c; |
192 | unsigned char a,b,c; |
171 | unsigned char ptr = 0; |
193 | unsigned char ptr = 0; |
172 | 194 | ||
173 | TxBuffer[pt++] = '#'; // Startzeichen |
195 | TxBuffer[pt++] = '#'; // Startzeichen |
174 | TxBuffer[pt++] = modul; // Adresse (a=0; b=1,...) |
196 | TxBuffer[pt++] = modul; // Adresse (a=0; b=1,...) |
175 | TxBuffer[pt++] = cmd; // Commando |
197 | TxBuffer[pt++] = cmd; // Commando |
176 | 198 | ||
177 | while(len) |
199 | while(len) |
178 | { |
200 | { |
179 | if(len) { a = snd[ptr++]; len--;} else a = 0; |
201 | if(len) { a = snd[ptr++]; len--;} else a = 0; |
180 | if(len) { b = snd[ptr++]; len--;} else b = 0; |
202 | if(len) { b = snd[ptr++]; len--;} else b = 0; |
181 | if(len) { c = snd[ptr++]; len--;} else c = 0; |
203 | if(len) { c = snd[ptr++]; len--;} else c = 0; |
182 | TxBuffer[pt++] = '=' + (a >> 2); |
204 | TxBuffer[pt++] = '=' + (a >> 2); |
183 | TxBuffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
205 | TxBuffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
184 | TxBuffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
206 | TxBuffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
185 | TxBuffer[pt++] = '=' + ( c & 0x3f); |
207 | TxBuffer[pt++] = '=' + ( c & 0x3f); |
186 | } |
208 | } |
187 | 209 | ||
188 | 210 | ||
189 | 211 | ||
190 | AddCRC(pt); |
212 | AddCRC(pt); |
191 | printf("Sending to MK %d \n" , pt); |
213 | printf("Sending to MK %d \n" , pt); |
192 | 214 | ||
193 | status = send(s,"\r" , 1, 0); |
215 | status = send(s,"\r" , 1, 0); |
194 | 216 | ||
195 | 217 | ||
196 | // for (c=0;c<pt+2;c++) |
218 | // for (c=0;c<pt+2;c++) |
197 | // { |
219 | // { |
198 | status = write(s,&TxBuffer , pt+3); |
220 | status = write(s,&TxBuffer , pt+3); |
199 | // printf("Send to MK %d \n" , TxBuffer[c] ); |
221 | // printf("Send to MK %d \n" , TxBuffer[c] ); |
200 | // } |
222 | // } |
201 | /*while(TxBuffer[i] !='\r' && i<150) |
223 | /*while(TxBuffer[i] !='\r' && i<150) |
202 | { |
224 | { |
203 | // TxBuffer[i]='#'; |
225 | // TxBuffer[i]='#'; |
204 | status = send(s,&TxBuffer[i] , 1, 0); |
226 | status = send(s,&TxBuffer[i] , 1, 0); |
205 | printf(" +%d%c ",i,TxBuffer[i]); |
227 | printf(" +%d%c ",i,TxBuffer[i]); |
206 | i++; |
228 | i++; |
207 | } |
229 | } |
208 | 230 | ||
209 | status = send(s,"\r" , 1, 0); |
231 | status = send(s,"\r" , 1, 0); |
210 | */ |
232 | */ |
211 | // status = send(s,"\r" , 1, 0); |
233 | // status = send(s,"\r" , 1, 0); |
212 | printf("\n"); |
234 | printf("\n"); |
213 | } |
235 | } |
214 | 236 | ||
215 | 237 | ||
216 | void write_display(int line,char* text) |
238 | void write_display(int line,char* text) |
217 | { |
239 | { |
218 | if (x52_output) x52_settext(x52_output, line , text, strlen(text)); |
240 | if (x52_output) x52_settext(x52_output, line , text, strlen(text)); |
219 | } |
241 | } |
220 | 242 | ||
221 | void clear_display() |
243 | void clear_display() |
222 | { |
244 | { |
223 | write_display(0,""); |
245 | write_display(0,""); |
224 | write_display(1,""); |
246 | write_display(1,""); |
225 | write_display(2,""); |
247 | write_display(2,""); |
226 | } |
248 | } |
227 | 249 | ||
228 | 250 | ||
229 | void output_device_list() |
251 | void output_device_list() |
230 | { |
252 | { |
231 | int i; |
253 | int i; |
232 | char disp_txt[20]; |
254 | char disp_txt[20]; |
233 | for(i=0;i<bt_device_count;i++) |
255 | for(i=0;i<bt_device_count;i++) |
234 | { |
256 | { |
235 | if (i<3) |
257 | if (i<3) |
236 | { |
258 | { |
237 | 259 | ||
238 | if (selected_bt_device==i) |
260 | if (selected_bt_device==i) |
239 | sprintf(disp_txt,"#%s",names[i]); |
261 | sprintf(disp_txt,"#%s",names[i]); |
240 | else |
262 | else |
241 | sprintf(disp_txt," %s",names[i]); |
263 | sprintf(disp_txt," %s",names[i]); |
242 | write_display(i,disp_txt); |
264 | write_display(i,disp_txt); |
243 | } |
265 | } |
244 | } |
266 | } |
245 | } |
267 | } |
246 | 268 | ||
247 | void connect_mk(char dest[18]) |
269 | int connect_mk(char dest[18]) |
248 | { |
270 | { |
249 | struct sockaddr_rc addr ; |
271 | struct sockaddr_rc addr ; |
250 | 272 | ||
251 | // allocate a socket |
273 | // allocate a socket |
252 | s = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM); |
274 | s = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM); |
253 | 275 | ||
254 | // set the connection parameters (who to connect to) |
276 | // set the connection parameters (who to connect to) |
255 | addr.rc_family = AF_BLUETOOTH; |
277 | addr.rc_family = AF_BLUETOOTH; |
256 | addr.rc_channel = 1; |
278 | addr.rc_channel = 1; |
257 | str2ba( dest, &addr.rc_bdaddr ); |
279 | str2ba( dest, &addr.rc_bdaddr ); |
258 | 280 | ||
259 | // connect to server |
281 | // connect to server |
260 | status = connect(s, (struct sockaddr *)&addr, sizeof(addr)); |
282 | status = connect(s, (struct sockaddr *)&addr, sizeof(addr)); |
- | 283 | ||
- | 284 | return status; |
|
261 | 285 | ||
262 | } |
286 | } |
263 | 287 | ||
264 | 288 | ||
265 | 289 | ||
266 | 290 | ||
267 | int r=0; |
291 | int r=0; |
- | 292 | int count=0; |
|
268 | int count=0; |
293 | int connected=0; |
269 | 294 | ||
270 | int main(int argc, char**argv) |
295 | int main(int argc, char**argv) |
271 | { |
296 | { |
272 | printf("Starting Riddim \n"); |
297 | printf("Starting Riddim \n"); |
273 | printf("\tRemote Interactive Digital Drone Interface Mashup\n"); |
298 | printf("\tRemote Interactive Digital Drone Interface Mashup\n"); |
274 | 299 | ||
- | 300 | // 1st argument -> Bluetooth adrees to bypass scanning ( takes to long for short testing roundtrips ) |
|
275 | //int tmp=2; |
301 | if (argv[1]) |
- | 302 | { |
|
276 | connect_mk("00:0B:CE:01:6B:4F"); |
303 | if (connect_mk(argv[1])==-1) |
277 | 304 | { |
|
- | 305 | printf("cant connect to QC at adress: %s\n",argv[1]); |
|
278 | 306 | return 0; |
|
279 | 307 | } |
|
- | 308 | printf("connected to QC at adress: %s\n",argv[1]); |
|
- | 309 | connected=1; |
|
280 | 310 | } |
|
281 | int i; |
311 | int i; |
282 | 312 | ||
283 | printf("\nInitializing X-52 input ..\n"); |
313 | printf("\nInitializing X-52 input ..\n"); |
284 | connect_joy(); |
314 | if (!connect_joy()) return 0; |
285 | printf(".. done"); |
315 | printf(".. done");// |
- | 316 | ||
286 | 317 | ||
287 | printf("\nInitializing X-52 output .."); |
318 | printf("\nInitializing X-52 output .."); |
288 | 319 | ||
289 | x52_output = x52_init(); |
320 | x52_output = x52_init(); |
290 | 321 | ||
291 | clear_display(); |
322 | clear_display(); |
292 | 323 | ||
293 | write_display(0, "RIDDIM active"); |
324 | write_display(0, "RIDDIM active"); |
294 | 325 | ||
295 | if (x52_output) x52_setbri(x52_output, 1,128); |
326 | if (x52_output) x52_setbri(x52_output, 1,128); |
296 | if (x52_output) |
327 | if (x52_output) |
297 | printf(" done \n"); |
328 | printf(" done \n"); |
298 | else |
329 | else |
299 | printf(" not found \n"); |
330 | printf(" not found \n"); |
300 | - | ||
301 | printf("Scanning for Bluetooth Devices ..\n"); |
- | |
302 | write_display(1,"Bluetooth Scan"); |
- | |
303 | 331 | ||
304 | // scan_bt(); |
- | |
305 | printf(" done \n"); |
- | |
306 | printf(" %d Devices found \n",bt_device_count); |
- | |
307 | - | ||
308 | - | ||
309 | /* for(i=0;i<bt_device_count;i++) |
332 | if (!connected) |
310 | { |
333 | { |
311 | printf(" %d -> %s (%s) \n",i,names[i],addrs[i]); |
334 | printf("Scanning for Bluetooth Devices ..\n"); |
- | 335 | write_display(1,"Bluetooth Scan"); |
|
- | 336 | scan_bt(); |
|
- | 337 | printf(" done \n"); |
|
312 | if (i<3) write_display(i,names[i]); |
338 | printf(" %d Devices found \n",bt_device_count); |
313 | } |
339 | } |
314 | |
- | |
315 | output_device_list() ; |
- | |
316 | */ |
340 | |
317 | 341 | ||
318 | int v_old; |
342 | int v_old; |
319 | while( 1 ) |
343 | while( 1 ) |
320 | { |
344 | { |
- | 345 | // poll values from input device |
|
321 | int polls=0; |
346 | int polls=0; |
322 | for (polls=0;polls<1000;polls++) // FIXME - better Polling |
347 | for (polls=0;polls<100;polls++) // FIXME - better Polling |
323 | { |
348 | { |
324 | read(x52_input_fd, &x52_event_struct, sizeof(struct js_event)); |
349 | read(x52_input_fd, &x52_event_struct, sizeof(struct js_event)); |
325 | 350 | ||
326 | 351 | ||
327 | /* see what to do with the event */ |
352 | /* see what to do with the event */ |
328 | switch (x52_event_struct.type & ~JS_EVENT_INIT) |
353 | switch (x52_event_struct.type & ~JS_EVENT_INIT) |
329 | { |
354 | { |
330 | case JS_EVENT_AXIS: |
355 | case JS_EVENT_AXIS: |
331 | axis [ x52_event_struct.number ] = x52_event_struct.value; |
356 | axis [ x52_event_struct.number ] = x52_event_struct.value; |
332 | break; |
357 | break; |
333 | case JS_EVENT_BUTTON: |
358 | case JS_EVENT_BUTTON: |
334 | button [ x52_event_struct.number ] = x52_event_struct.value; |
359 | button [ x52_event_struct.number ] = x52_event_struct.value; |
335 | break; |
360 | break; |
336 | } |
361 | } |
337 | } |
362 | } |
338 | 363 | ||
339 | for( x=0 ; x<num_of_buttons ; ++x ) |
364 | for( x=0 ; x<num_of_buttons ; ++x ) |
340 | if( button[x]==0) |
365 | if( button[x]==0) |
341 | button_trigger[x]=0; |
366 | button_trigger[x]=0; |
342 | else |
367 | else |
343 | { |
368 | { |
344 | if (button_trigger[x]<100)button_trigger[x]++; |
369 | if (button_trigger[x]<100)button_trigger[x]++; |
345 | } |
370 | } |
346 | 371 | ||
347 | 372 | ||
348 | 373 | ||
349 | switch(state) |
374 | switch(state) |
350 | { |
375 | { |
351 | 376 | ||
352 | 377 | ||
353 | case STATEID_SCANNING: |
378 | case STATEID_SCANNING: |
354 | 379 | ||
355 | state=STATEID_CONNECTING; //e |
380 | state=STATEID_CONNECTING; |
356 | 381 | ||
357 | ExternControl.Digital[0]=0; |
382 | ExternControl.Digital[0]=0; |
358 | ExternControl.Digital[1]=0; |
383 | ExternControl.Digital[1]=0; |
359 | ExternControl.RemoteTasten=0; |
384 | ExternControl.RemoteTasten=0; |
360 | ExternControl.Nick=(axis[1]>>8)*(-1)+127;; |
385 | ExternControl.Nick=(axis[1]>>8)*(-1)+127;; |
361 | 386 | ||
362 | printf("nick%d\n",ExternControl.Nick); |
387 | printf("nick%d\n",ExternControl.Nick); |
363 | ExternControl.Roll=(axis[0]>>8)*(-1)+127;; |
388 | ExternControl.Roll=(axis[0]>>8)*(-1)+127;; |
364 | ExternControl.Gier=(axis[5]>>8)*(-1)+127;; |
389 | ExternControl.Gier=(axis[5]>>8)*(-1)+127;; |
365 | ExternControl.Gas=(axis[2]>>8)*(-1)+127; |
390 | ExternControl.Gas=(axis[2]>>8)*(-1)+127; |
366 | ExternControl.Higt=0; |
391 | ExternControl.Higt=0; |
367 | ExternControl.free=0; |
392 | ExternControl.free=0; |
368 | ExternControl.Frame='t'; |
393 | ExternControl.Frame='t'; |
369 | ExternControl.Config=1; |
394 | ExternControl.Config=1; |
370 | - | ||
371 | - | ||
372 | 395 | ||
373 | printf("sending data\n"); |
- | |
374 | 396 | printf("sending data\n"); |
|
- | 397 | ||
- | 398 | ||
375 | SendOutData('b', 0, &ExternControl, sizeof(ExternControl)); |
399 | SendOutData('b', 0, (unsigned char *)&ExternControl, sizeof(ExternControl)); |
376 | 400 | gettimeofday(&time_struct1,NULL); |
|
377 | 401 | ||
378 | if (button_trigger[BUTTON_SELECT]==1) |
402 | if (button_trigger[BUTTON_SELECT]==1) |
379 | { |
403 | { |
380 | state=STATEID_CONNECTING; |
404 | state=STATEID_CONNECTING; |
381 | clear_display(); |
405 | clear_display(); |
382 | write_display(0,"connecting to"); |
406 | write_display(0,"connecting to"); |
383 | write_display(1,names[selected_bt_device]); |
407 | write_display(1,names[selected_bt_device]); |
384 | connect_mk(addrs[selected_bt_device]); |
408 | connect_mk(addrs[selected_bt_device]); |
385 | write_display(0,"connected to"); |
409 | write_display(0,"connected to"); |
386 | } |
410 | } |
387 | 411 | ||
388 | if ((button_trigger[BUTTON_UP]+button_trigger[BUTTON_DOWN])==1) |
412 | if ((button_trigger[BUTTON_UP]+button_trigger[BUTTON_DOWN])==1) |
389 | { |
413 | { |
390 | printf("-> sel_dev %d - %d\n",selected_bt_device,button_trigger[19]); |
414 | printf("-> sel_dev %d - %d\n",selected_bt_device,button_trigger[19]); |
391 | if (button_trigger[BUTTON_DOWN]==1) |
415 | if (button_trigger[BUTTON_DOWN]==1) |
392 | if (selected_bt_device>0) selected_bt_device--; |
416 | if (selected_bt_device>0) selected_bt_device--; |
393 | if (button_trigger[BUTTON_UP]==1) |
417 | if (button_trigger[BUTTON_UP]==1) |
394 | if (selected_bt_device<bt_device_count-1) selected_bt_device++; |
418 | if (selected_bt_device<bt_device_count-1) selected_bt_device++; |
395 | 419 | ||
396 | output_device_list() ; |
420 | output_device_list() ; |
397 | } |
421 | } |
398 | break; |
422 | break; |
399 | 423 | ||
400 | case STATEID_CONNECTING: |
424 | case STATEID_CONNECTING: |
401 | // for (polls=0;polls<10;polls++) // FIXME - better Polling |
- | |
- | 425 | ||
402 | RxBuffer[1]=0; |
426 | RxBuffer[1]=0; |
403 | while (RxBuffer[1]!='t') |
- | |
404 | { |
- | |
405 | - | ||
406 | r=0; |
- | |
- | 427 | ||
- | 428 | ||
407 | in_char='#'; |
429 | // ftime(&time_struct); |
408 | 430 | //printf("t:%d",time_struct.millitm); |
|
- | 431 | while (RxBuffer[1]!='t') |
|
409 | while(in_char!='\r') |
432 | { |
410 | { |
433 | |
411 | count=read(s,&in_char,1); |
434 | r=0; |
412 | if (in_char!=0) |
435 | in_char='#'; |
413 | { |
- | |
414 | RxBuffer[r++]=in_char; |
- | |
415 | } |
436 | |
- | 437 | while(in_char!='\r') |
|
- | 438 | { |
|
- | 439 | count=read(s,&in_char,1); |
|
- | 440 | if (in_char!=0) |
|
- | 441 | { |
|
- | 442 | RxBuffer[r++]=in_char; |
|
- | 443 | } |
|
416 | else |
444 | else |
- | 445 | { |
|
- | 446 | RxBuffer[r++]='0'; |
|
417 | { |
447 | } |
- | 448 | // printf("count:%d r:%d %d %c \n",count , r, in_char, in_char); |
|
418 | RxBuffer[r++]='0'; |
449 | } |
- | 450 | RxBuffer[r++]='\0'; |
|
419 | } |
451 | printf("--->%s\n",RxBuffer); |
- | 452 | ||
- | 453 | } |
|
- | 454 | gettimeofday(&time_struct2,NULL); |
|
- | 455 | ||
- | 456 | printf("last trip: %d",(int)(time_struct1.tv_usec-time_struct2.tv_usec)); |
|
420 | // printf("count:%d r:%d %d %c \n",count , r, in_char, in_char); |
457 | act_mode=button[24] | (button[25]<<1); |
- | 458 | ||
- | 459 | switch (act_mode) |
|
- | 460 | { |
|
- | 461 | case 0: |
|
- | 462 | act_nick=(axis[AXIS_NICK]>>8)*(INVERT_NICK); |
|
- | 463 | act_roll=(axis[AXIS_ROLL]>>8)*(INVERT_ROLL); |
|
- | 464 | act_gier=(axis[AXIS_GIER]>>8)*(INVERT_GIER); |
|
- | 465 | act_gas=(axis[AXIS_GAS]>>8)*(INVERT_GAS); |
|
- | 466 | ||
- | 467 | break; |
|
- | 468 | ||
- | 469 | case 1: |
|
- | 470 | act_nick=(axis[AXIS_NICK]>>8)*(INVERT_NICK)/2; |
|
- | 471 | act_roll=(axis[AXIS_ROLL]>>8)*(INVERT_ROLL)/2; |
|
- | 472 | act_gier=(axis[AXIS_GIER]>>8)*(INVERT_GIER)/2; |
|
- | 473 | act_gas=(axis[AXIS_GAS]>>8)*(INVERT_GAS); |
|
- | 474 | ||
- | 475 | break; |
|
- | 476 | ||
- | 477 | case 2: |
|
- | 478 | act_nick=(axis[AXIS_NICK]>>8)*(INVERT_NICK)/3; |
|
421 | } |
479 | act_roll=(axis[AXIS_ROLL]>>8)*(INVERT_ROLL)/3; |
422 | RxBuffer[r++]='\0'; |
- | |
- | 480 | act_gier=(axis[AXIS_GIER]>>8)*(INVERT_GIER)/3; |
|
- | 481 | act_gas=(axis[AXIS_GAS]>>8)*(INVERT_GAS); |
|
- | 482 | ||
- | 483 | ||
423 | printf("--->%s\n",RxBuffer); |
484 | break; |
424 | 485 | ||
425 | } |
486 | } |
426 | 487 | ||
427 | ExternControl.Digital[0]=0; |
488 | ExternControl.Digital[0]=0; |
428 | ExternControl.Digital[1]=0; |
489 | ExternControl.Digital[1]=0; |
429 | ExternControl.RemoteTasten=0; |
490 | ExternControl.RemoteTasten=0; |
430 | ExternControl.Nick=(axis[1]>>8)*(-1); |
491 | ExternControl.Nick=act_nick; //(axis[1]>>8)*(-1)/2; |
431 | printf("nick%d\n",ExternControl.Nick); |
492 | // printf("nick%d\n",ExternControl.Nick); |
432 | ExternControl.Roll=(axis[0]>>8)*(-1); |
493 | ExternControl.Roll=act_roll; //(axis[0]>>8)*(-1)/2; |
433 | ExternControl.Gier=(axis[5]>>8); |
494 | ExternControl.Gier=(axis[5]>>8); |
434 | ExternControl.Gas=(axis[2]>>8)*(-1)+127; |
495 | ExternControl.Gas=(axis[2]>>8)*(-1)+127; |
435 | ExternControl.Higt=0; |
496 | ExternControl.Higt=0; |
436 | ExternControl.free=0; |
497 | ExternControl.free=0; |
437 | ExternControl.Frame='t'; |
498 | ExternControl.Frame='t'; |
- | 499 | ||
438 | ExternControl.Config=1; |
500 | ExternControl.Config=1; |
- | 501 | 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); |
|
439 | 502 | ||
440 | 503 | ||
441 | 504 | ||
442 | printf("sending data\n"); |
505 | printf("sending data\n"); |
443 | 506 | ||
444 | SendOutData('b', 0, &ExternControl, sizeof(ExternControl)); |
- | |
- | 507 | SendOutData('b', 0, (unsigned char *)&ExternControl, sizeof(ExternControl)); |
|
445 | 508 | gettimeofday(&time_struct1,NULL); |
|
446 | printf("sent data\n"); |
509 | printf("sent data\n"); |
447 | 510 | ||
448 | 511 | ||
449 | printf("sleeping\n"); |
512 | // printf("sleeping\n"); |
450 | // usleep(10000); |
- | |
451 | 513 | // usleep(10000); |
|
452 | printf("end_sleep\n"); |
514 | // printf("end_sleep\n"); |
453 | 515 | ||
454 | int v=axis[6]/655+50; |
516 | int v=axis[6]/655+50; |
455 | if (v!=v_old)if (x52_output) x52_setbri(x52_output, 0,v ); |
517 | if (v!=v_old)if (x52_output) x52_setbri(x52_output, 0,v ); |
456 | v_old=v; |
518 | v_old=v; |
457 | 519 | ||
458 | printf("v: %d \n",v); |
520 | printf("v: %d \n",v); |
459 | 521 | ||
460 | 522 | ||
461 | for (i=0;i<num_of_axis;i++) |
523 | for (i=0;i<num_of_axis;i++) |
462 | printf("A%d: %d ", i,axis[i]>>8 ); |
524 | printf("A%d: %d ", i,axis[i]>>8 ); |
463 | 525 | ||
464 | for( x=0 ; x<num_of_buttons ; ++x ) |
526 | for( x=0 ; x<num_of_buttons ; ++x ) |
465 | 527 | ||
466 | printf("B%d: %d ", x, button[x] ); |
528 | printf("B%d: %d ", x, button[x] ); |
467 | 529 | ||
468 | break; |
530 | break; |
469 | } |
531 | } |
470 | 532 | ||
471 | printf("\n"); |
533 | printf("\n"); |
472 | fflush(stdout); |
534 | fflush(stdout); |
473 | printf("loop fin"); |
535 | printf("loop fin"); |
474 | 536 | ||
475 | } |
537 | } |
476 | 538 | ||
477 | 539 | ||
478 | /******************** Cleanup **********************/ |
540 | /******************** Cleanup **********************/ |
479 | close(x52_input_fd); |
541 | close(x52_input_fd); |
480 | close(s); |
542 | close(s); |
481 | 543 | ||
482 | if (x52_output) x52_close(x52_output); |
544 | if (x52_output) x52_close(x52_output); |
483 | return 0; |
545 | return 0; |
484 | } |
546 | } |
485 | 547 |