1 /*
2 *
3 * Copyright (c) 2003 The Regents of the University of California. All
4 * rights reserved.
5 *
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted provided that the following conditions
8 * are met:
9 *
10 * - Redistributions of source code must retain the above copyright
11 * notice, this list of conditions and the following disclaimer.
12 *
13 * - Neither the name of the University nor the names of its
14 * contributors may be used to endorse or promote products derived
15 * from this software without specific prior written permission.
16 *
17 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS''
18 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
19 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
20 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
21 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
22 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
23 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
24 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
25 * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 *
29 */
30
31
32 #include <stdio.h>
33 #include <stdlib.h>
34 #include <unistd.h>
35 #include <ctype.h>
36 #include <sys/time.h>
37 #include <arpa/inet.h>
38
39 #include "emrun/emrun.h"
40 #include "link/link.h"
41 #include <link/link_headers.h>
42 #include <libmisc/misc.h>
43 #include <libdev/status_dev.h>
44 #include <libdev/command_dev.h>
45 #include <libdev/query_client.h>
46 #include <libdev/sensor_dev.h>
47 #include <timesync/sync_timehist.h>
48 #include <libdev/sensor_client.h>
49 #include <libdev/command_dev.h>
50 #include <libdev/status_client.h>
51 #include <camctrl.h>
52
53 #define MAX_NUM_MOTES 10
54 #define DEFAULT_ROOT 1
55
56 #define MH_CMD_DEV sim_path("/dev/link/mh/command")
57 #define DEMO_CMD_DEV sim_path("/dev/cam/demo_cmd")
58
59
60 #define IDLE 0
61 #define PRE_ZOOM 1
62 #define PT 2
63 #define ZOOM 3
64 #define SHOOT 4
65
66
67
68 typedef struct _position {
69 int32_t x;
70 int32_t y;
71 int32_t z;
72 } position_t;
73
74
75 typedef struct _trig_state trig_state_t;
76
77 typedef struct _mote_element {
78 node_id_t id;
79 position_t pos;
80 int watchable;
81 int root;
82 trig_state_t *trig_ref;
83 char *link_name;
84 lu_context_t *link_ref;
85
86 } mote_element_t;
87
88
89
90 struct _trig_state {
91
92 query_client_ctx_t *cmd_ref;
93
94 mote_element_t *mote_list[MAX_NUM_MOTES];
95 int num_motes;
96
97 int active_node_index;
98
99 status_client_context_t *cam_status_ref;
100 command_context_t *command_ref;
101 int mote_configure;
102
103 };
104
105
106 int query_response(query_client_ctx_t *qc, int retval, buf_t *response);
107 int image_cb(void *arg, sc_sample_t *sample);
108 int cam_status_cb(void *new_buffer, size_t size, void *data);
109 void open_cam_status_dev(trig_state_t *tstate);
110
111
112 int command_handler(char *cmdline, size_t size, void *data)
113 {
114 int i=0;
115 int id=0;
116 trig_state_t *tstate = (trig_state_t *)data;
117
118 id = atoi(cmdline);
119
120 for (i=0; i<tstate->num_motes; i++) {
121 if (id == tstate->mote_list[i]->id) {
122 elog(LOG_NOTICE, "Configuring node %d", id);
123 tstate->mote_configure=id;
124 open_cam_status_dev(tstate);
125 goto done;
126 }
127 }
128
129 elog(LOG_ERR, "Couldn't find mote with id %d in mote list", id);
130
131 done:
132 return EVENT_RENEW;
133 }
134
135
136
137
138
139
140
141
142 void create_command_dev(trig_state_t *tstate)
143 {
144 cmd_dev_opts_t opts = {
145 device: {
146 devname: DEMO_CMD_DEV,
147 device_info: tstate,
148 },
149 command: command_handler,
150 };
151
152 if (g_command_dev(&opts, &tstate->command_ref) < 0) {
153 elog(LOG_ERR, "Can't create command dev %s: %m",
154 opts.device.devname);
155 exit(1);
156 }
157 }
158
159
160
161 void open_cam_status_dev(trig_state_t *tstate)
162 {
163 status_client_opts_t s_opts = {
164 devname: CAM_STATUS_DEV,
165 private_data: tstate,
166 handler: cam_status_cb,
167 };
168
169
170 if (g_status_client_full(&s_opts, &tstate->cam_status_ref) < 0) {
171 elog(LOG_ERR, "Can't open status device %s: %m",
172 s_opts.devname);
173 exit(1);
174 }
175
176 }
177
178
179
180
181 void trig_shutdown(void *data)
182 {
183 exit(0);
184 }
185
186
187
188 void trigger_camera(mote_element_t *melement)
189 {
190 query_client_opts_t opts = {
191 device:CAM_QUERY_DEV,
192 response: query_response,
193 };
194
195 trig_state_t *tstate = melement->trig_ref;
196
197 buf_t *buf = buf_new();
198
199 opts.private_data = tstate;
200
201
202 // first, unzoom completely to get the fastest response time
203 // bufprintf(buf, "zoom=0,");
204
205 // create the pt command
206 bufprintf(buf, "pan=%d,pan_speed=1,tilt=%d,tilt_speed=1,",
207 melement->pos.x, melement->pos.y);
208
209
210 bufprintf(buf, "zoom=%d,", melement->pos.z);
211
212 bufprintf(buf, "sleep=500,");
213
214 // create the take pic command
215 bufprintf(buf, "trigger");
216
217 // go
218 if (query_client_new(&opts, &tstate->cmd_ref) < 0) {
219 elog(LOG_ERR, "Can't open query device %s: %m",
220 opts.device);
221 exit(1);
222 }
223
224 if (query_client_issue(melement->trig_ref->cmd_ref, buf) < 0) {
225 elog(LOG_ERR, "Can't issue query: %m");
226 }
227
228 elog(LOG_NOTICE, "CLICK!");
229
230 buf_free(buf);
231 }
232
233
234
235 static int pkt_rcvd(lu_context_t *link, link_pkt_t *lpkt, int datalen)
236 {
237 mote_element_t *melement = (mote_element_t *)lu_data(link);
238 trig_state_t *tstate=NULL;
239 int i=0;
240
241 if (melement==NULL) {
242 elog(LOG_ERR, "NULL pointer");
243 exit(1);
244 }
245
246 elog(LOG_NOTICE, "Got packet from node %d",
247 lpkt->src.id);
248
249 tstate = melement->trig_ref;
250 // Ok got the pkt, look in the array to find a matching entry
251 for (i=0; i<tstate->num_motes; i++) {
252 if (tstate->mote_list[i]->id == lpkt->src.id) {
253 // found matching entry
254 // init the SM
255 tstate->active_node_index = i;
256 trigger_camera(tstate->mote_list[i]);
257 goto done;
258
259 }
260 }
261
262 elog(LOG_ERR, "No matching entry found for pkt with src.id %d",
263 lpkt->src.id);
264
265 done:
266 free(lpkt);
267 return EVENT_RENEW;
268 }
269
270
271
272
273
274 int main(int argc, char **argv)
275 {
276 trig_state_t *tstate = NULL;
277 int i=0;
278 int root=0;
279
280 emrun_opts_t emrun_opts = {
281 shutdown: trig_shutdown,
282 data: (void *)tstate,
283 silent: 1
284 };
285
286 // we need to be in sim mode
287 //in_sim = 1;
288
289 tstate = malloc(sizeof(trig_state_t));
290 memset(tstate, 0, sizeof(trig_state_t));
291
292
293 misc_init(&argc, argv, CVSTAG);
294
295 misc_parse_option_as_int(&argc, argv, "num-nodes", 'n',
296 &tstate->num_motes);
297
298 misc_parse_option_as_int(&argc, argv, "root", 'r',
299 &root);
300
301 if (tstate->num_motes==0) {
302 elog(LOG_CRIT, "Number of motes is zero!");
303 exit(1);
304 } else {
305 elog(LOG_NOTICE, "Running with %d motes", tstate->num_motes);
306 }
307
308 if (root==0) {
309 elog(LOG_NOTICE, "No root specified; defaulting to %d",
310 my_node_id);
311 root=my_node_id;
312 }
313
314 create_command_dev(tstate);
315 // allocate mem for each mote
316 for (i=0; i<tstate->num_motes; i++) {
317
318 mote_element_t *melement = malloc(sizeof(mote_element_t));
319 memset(melement, 0, sizeof(mote_element_t));
320
321 // this is a hack, the correct way is to read the node's if_id
322 // from the link status device
323
324 if (i==0) {
325 melement->id = root;
326 } else {
327 melement->id = i+1;
328 }
329
330 // melement->id = i+1;
331 // my_node_id=melement->id;
332 melement->trig_ref = tstate;
333 tstate->mote_list[i]=melement;
334
335 if (melement->id==root) {
336 // we ONLY care about the root, in terms of link packets
337 lu_opts_t lu_opts = {
338 opts: {
339 data: (void *)melement,
340 },
341 receive: pkt_rcvd,
342 };
343
344 melement->root = 1;
345 melement->watchable = 1;
346 // switch id to the root's id, so that we can get
347 // link names to work
348 lu_opts.opts.name = link_parse_uses(&argc, argv, "mh");
349 if (lu_opts.opts.name == NULL) {
350 elog(LOG_CRIT, "Please specify --uses!");
351 exit(1);
352 }
353
354 melement->link_name = lu_opts.opts.name;
355
356 // open the ld
357 if (lu_open(&lu_opts, &(melement->link_ref)) < 0) {
358 elog(LOG_CRIT, "can't open %s: %m",
359 link_name(&lu_opts.opts, NULL));
360 exit(1);
361 } else {
362 elog(LOG_NOTICE, "Using link device %s",
363 lu_name(melement->link_ref, NULL));
364 printf_to_file(MH_CMD_DEV, "root");
365 }
366
367
368
369
370 }
371
372 // UBERHACK: assigning values statically
373 // plus, x==pan, y==tilt, z==zoom, instead of actual coordinates
374 /*
375 if (melement->id==2) {
376 melement->pos.x = -200;
377 melement->pos.y = -380;
378 melement->pos.z = 14000;
379 } else if (melement->id==3) {
380 melement->pos.x = 220;
381 melement->pos.y = -350;
382 melement->pos.z = 14000;
383 } else {
384 melement->pos.x = 0;
385 melement->pos.y = 0;
386 melement->pos.z = 0;
387 }
388 */
389 }
390
391
392 // Ok now it's time to start emrun
393 emrun_init(&emrun_opts);
394 g_main();
395 elog(LOG_CRIT, "event loop terminated abnormally!");
396 return 0;
397 }
398
399
400
401 int query_response(query_client_ctx_t *qc, int retval, buf_t *response)
402 {
403 trig_state_t *tstate = (trig_state_t *)query_client_data(qc);
404
405 if (response!=NULL) {
406 buf_free(response);
407 } else {
408 elog(LOG_WARNING, "NULL response");
409 // return EVENT_RENEW;
410 }
411
412 if (tstate==NULL) {
413 elog(LOG_ERR, "NULL tstate");
414 exit(1);
415 }
416
417 // picture is ready, get it from sensor dev
418 sc_get_next_sample("/dev/sensors/camera", 0, image_cb, NULL, NULL);
419
420
421 return EVENT_DONE;
422 }
423
424
425
426 int image_cb(void *arg, sc_sample_t *sample)
427 {
428 FILE *f;
429 size_t size;
430
431 size = sample->data->len;
432 elog(LOG_NOTICE, "Image length is %d", size);
433 f=fopen("/scratch/foo.jpg", "w");
434 if (f==NULL) {
435 elog(LOG_ERR, "NULL file ptr");
436 } else {
437 fwrite(sample->data->buf,size, 1, f);
438 fclose(f);
439 }
440
441
442 return EVENT_DONE;
443 }
444
445
446
447 int cam_status_cb(void *new_buffer, size_t size, void *data)
448 {
449 int id=0;
450 int i=0;
451 trig_state_t *tstate = (trig_state_t *)data;
452 cmd_state_t *istate = (cmd_state_t *)new_buffer;
453 id = tstate->mote_configure;
454
455
456 for (i=0; i<tstate->num_motes; i++) {
457 if (id == tstate->mote_list[i]->id) {
458
459 mote_element_t *melement = tstate->mote_list[i];
460
461 melement->pos.x = istate->pan_value;
462 melement->pos.y = istate->tilt_value;
463 melement->pos.z = istate->zoom_value;
464
465 elog(LOG_NOTICE, "Configured node %d with values: %d, %d, %d",
466 id, melement->pos.x, melement->pos.y, melement->pos.z);
467 tstate->mote_configure = 0;
468 break;
469 }
470 }
471
472
473 return EVENT_DONE;
474
475 }
476
477
478
This page was automatically generated by the
LXR engine.
Visit the LXR main site for more
information.