cmucam2.c

CMUcam2 Emulation Source - Anthony Rowe, 03/16/2008 03:32 pm

Download (44.2 kB)

 
1
#include <cc3.h>
2
#include <cc3_ilp.h>
3
#include <cc3_color_track.h>
4
#include <cc3_color_info.h>
5
#include <cc3_histogram.h>
6
#include <cc3_frame_diff.h>
7
#include <math.h>
8
#include <stdbool.h>
9
#include <stdio.h>
10
#include <string.h>
11
#include <ctype.h>
12
#include <stdlib.h>
13
#include <cc3_jpg.h>
14
#include <cc3_math.h>
15
#include <cc3_hsv.h>
16

    
17
// Uncomment line below to reverse servo direction for auto-servo and demo mode 
18
#define SERVO_REVERSE_DIRECTION_PAN
19
#define SERVO_REVERSE_DIRECTION_TILT
20

    
21
//#define SERIAL_BAUD_RATE  CC3_UART_RATE_230400
22
#define SERIAL_BAUD_RATE  CC3_UART_RATE_115200
23
//#define SERIAL_BAUD_RATE  CC3_UART_RATE_57600
24
//#define SERIAL_BAUD_RATE  CC3_UART_RATE_38400
25
//#define SERIAL_BAUD_RATE  CC3_UART_RATE_19200
26
//#define SERIAL_BAUD_RATE  CC3_UART_RATE_9600
27
//#define SERIAL_BAUD_RATE  CC3_UART_RATE_4800
28
//#define SERIAL_BAUD_RATE  CC3_UART_RATE_2400
29
//#define SERIAL_BAUD_RATE  CC3_UART_RATE_1200
30
//#define SERIAL_BAUD_RATE  CC3_UART_RATE_300
31

    
32
#define SERVO_MIN 0
33
#define SERVO_MID 128
34
#define SERVO_MAX 255
35
// Define a jitter guard such that more than SERVO_GUARD pixels are required
36
// for the servo to move.
37

    
38
#define DEFAULT_COLOR 0
39
#define HSV_COLOR     1
40

    
41
static const unsigned int MAX_ARGS = 10;
42
static const unsigned int MAX_LINE = 128;
43

    
44
static const char VERSION_BANNER[] = "CMUcam2 v1.00 c6";
45

    
46
typedef struct {
47
  uint8_t pan_step, tilt_step;
48
  uint8_t pan_range_near, tilt_range_near;
49
  uint8_t pan_range_far, tilt_range_far;
50
  int16_t x;
51
  int16_t y;
52
  bool y_control;
53
  bool x_control;
54
  bool y_report;
55
  bool x_report;
56
} cmucam2_servo_t;
57

    
58

    
59
typedef enum {
60
  RESET,
61
  TRACK_COLOR,
62
  SEND_FRAME,
63
  HI_RES,
64
  FRAME_DIFF,
65
  GET_VERSION,
66
  GET_MEAN,
67
  SET_SERVO,
68
  CAMERA_REG,
69
  CAMERA_POWER,
70
  POLL_MODE,
71
  LINE_MODE,
72
  SEND_JPEG,
73
  VIRTUAL_WINDOW,
74
  DOWN_SAMPLE,
75
  GET_HISTOGRAM,
76
  TRACK_WINDOW,
77
  GET_TRACK,
78
  GET_WINDOW,
79
  LED_0,
80
  NOISE_FILTER,
81
  TRACK_INVERT,
82
  SERVO_MASK,
83
  SERVO_PARAMETERS,
84
  SERVO_OUTPUT,
85
  GET_SERVO,
86
  SET_INPUT,
87
  GET_INPUT,
88
  SET_TRACK,
89
  BUF_MODE,
90
  READ_FRAME,
91
  OUTPUT_MASK,
92
  PACKET_FILTER,
93
  CONF_HISTOGRAM,
94
  GET_BUTTON,
95
  FRAME_DIFF_CHANNEL,
96
  LOAD_FRAME,
97
  RAW_MODE,
98
  COLOR_SPACE,
99
  HIRES_DIFF,
100
  FRAME_STREAM,
101

    
102
  RETURN,                       // Must be second to last
103
  CMUCAM2_CMDS_COUNT            // Must be last entry so array sizes are correct
104
} cmucam2_command_t;
105

    
106
static const char cmucam2_cmds[CMUCAM2_CMDS_COUNT][3] = {
107
  [RETURN] = "",
108

    
109
  /* Buffer Commands */
110
  [BUF_MODE] = "BM",
111
  [READ_FRAME] = "RF",
112

    
113
  /* Camera Module Commands */
114
  [CAMERA_REG] = "CR",
115
  [CAMERA_POWER] = "CP",
116
  //  CT camera type
117

    
118
  /* Data Rate Commands */
119
  //  DM delay mode
120
  [FRAME_STREAM] = "FS",
121
  [POLL_MODE] = "PM",
122
  //  PS packet skip
123
  [RAW_MODE] = "RM",
124
  [PACKET_FILTER] = "PF",
125
  [OUTPUT_MASK] = "OM",
126

    
127
  /* Servo Commands */
128
  [SET_SERVO] = "SV",
129
  [GET_SERVO] = "GS",
130
  [SERVO_OUTPUT] = "SO",
131
  [SERVO_MASK] = "SM",
132
  [SERVO_PARAMETERS] = "SP",
133

    
134
  /* Image Windowing Commands */
135
  [SEND_FRAME] = "SF",
136
  [DOWN_SAMPLE] = "DS",
137
  [VIRTUAL_WINDOW] = "VW",
138
  //  FS frame stream
139
  [HI_RES] = "HR",
140
  [GET_WINDOW] = "GW",
141
  //  PD pixel difference
142

    
143
  /* Auxiliary I/O Commands */
144
  [GET_INPUT] = "GI",
145
  [SET_INPUT] = "SI",           // new for cmucam3
146
  [GET_BUTTON] = "GB",
147
  [LED_0] = "L0",
148
  //  L1 LED control
149

    
150
  /* Color Tracking Commands */
151
  [TRACK_COLOR] = "TC",
152
  [TRACK_INVERT] = "TI",
153
  [TRACK_WINDOW] = "TW",
154
  [NOISE_FILTER] = "NF",
155
  [LINE_MODE] = "LM",
156
  [GET_TRACK] = "GT",
157
  [SET_TRACK] = "ST",
158

    
159
  /* Histogram Commands */
160
  [GET_HISTOGRAM] = "GH",
161
  [CONF_HISTOGRAM] = "HC",
162
  //  HT histogram track
163

    
164
  /* Frame Differencing Commands */
165
  [FRAME_DIFF] = "FD",
166
  [LOAD_FRAME] = "LF",
167
  [FRAME_DIFF_CHANNEL] = "DC",
168
  [HIRES_DIFF] = "HD",
169
  //  MD mask difference
170
  //  UD upload difference
171

    
172
  /* Color Statistics Commands */
173
  [GET_MEAN] = "GM",
174

    
175
  /* System Level Commands */
176
  //  SD sleep deeply
177
  //  SL sleep
178
  [RESET] = "RS",
179
  [GET_VERSION] = "GV",
180
  
181
  [COLOR_SPACE] = "CS",
182

    
183
  /* CMUcam3 New Commands */
184
  [SEND_JPEG] = "SJ",
185
};
186

    
187

    
188
static void cmucam2_load_frame (cc3_frame_diff_pkt_t * pkt, bool buf_mode, uint8_t sw_color_space);
189
static void cmucam2_get_histogram (cc3_histogram_pkt_t * h_pkt,
190
                                   bool poll_mode, bool buf_mode, uint8_t sw_color_space, bool quiet);
191
static void cmucam2_get_mean (cc3_color_info_pkt_t * t_pkt, bool poll_mode,
192
                              bool line_mode, bool buf_mode, uint8_t sw_color_space, bool quiet);
193
static void cmucam2_write_s_packet (cc3_color_info_pkt_t * pkt);
194
static void cmucam2_track_color (cc3_track_pkt_t * t_pkt,
195
                                 bool poll_mode,
196
                                 int8_t line_mode, bool auto_led,
197
                                 cmucam2_servo_t * servo_settings,
198
                                 bool buf_mode, uint8_t sw_color_space, bool quiet);
199
static void cmucam2_frame_diff (cc3_frame_diff_pkt_t * pkt,
200
                                bool poll_mode, bool line_mode, bool buf_mode,
201
                                bool auto_led, uint8_t sw_color_space, bool quiet);
202
static int32_t cmucam2_get_command (cmucam2_command_t * cmd,
203
                                    uint32_t arg_list[]);
204
static int32_t cmucam2_get_command_raw (cmucam2_command_t * cmd,
205
                                        uint32_t arg_list[]);
206
static void print_ACK (void);
207
static void print_NCK (void);
208
static void print_prompt (void);
209
static void print_cr (void);
210
static void cmucam2_write_t_packet (cc3_track_pkt_t * pkt,
211
                                    cmucam2_servo_t * servo_settings);
212
static void cmucam2_write_h_packet (cc3_histogram_pkt_t * pkt);
213
static void cmucam2_send_image_direct (bool auto_led,uint8_t sw_color_space);
214

    
215
static void raw_print (uint8_t val);
216

    
217
static bool packet_filter_flag;
218
static uint8_t t_pkt_mask;
219
static uint8_t s_pkt_mask;
220

    
221
static bool raw_mode_output;
222
static bool raw_mode_no_confirmations;
223
static bool raw_mode_input;
224

    
225
int main (void)
226
{
227
  cmucam2_command_t command;
228
  int32_t val, n;
229
  uint32_t arg_list[MAX_ARGS];
230
  uint32_t start_time;
231
  uint8_t sw_color_space;
232
  bool error, poll_mode, auto_led, demo_mode, buf_mode,frame_stream_mode;
233
  int8_t line_mode;
234
  cc3_track_pkt_t t_pkt;
235
  cc3_color_info_pkt_t s_pkt;
236
  cc3_histogram_pkt_t h_pkt;
237
  cc3_frame_diff_pkt_t fd_pkt;
238
  cmucam2_servo_t servo_settings;
239

    
240
  //cc3_filesystem_init ();
241

    
242
  cc3_uart_init (0,
243
                 SERIAL_BAUD_RATE,
244
                 CC3_UART_MODE_8N1, CC3_UART_BINMODE_BINARY);
245
  val = setvbuf (stdout, NULL, _IONBF, 0);
246

    
247
  if (!cc3_camera_init ()) {
248
    cc3_led_set_state (0, true);
249
    exit (1);
250
  }
251

    
252
  servo_settings.x_control = false;
253
  servo_settings.y_control = false;
254
  servo_settings.x_report = false;
255
  servo_settings.y_report = false;
256
  demo_mode = false;
257

    
258
  // Keep this memory in the bank for frame differencing
259
  fd_pkt.previous_template = malloc (16 * 16 * sizeof (uint32_t));
260
  if (fd_pkt.previous_template == NULL)
261
    printf ("Malloc FD startup error!\r");
262
  start_time = cc3_timer_get_current_ms ();
263

    
264
  do {
265
    if (cc3_button_get_state () == 1) {
266
      // Demo Mode flag
267
      demo_mode = true;
268
      servo_settings.x_control = true;
269
      servo_settings.y_control = true;
270
      servo_settings.x_report = true;
271
      servo_settings.y_report = true;
272
      // Debounce Switch
273
      cc3_led_set_state (0, false);
274
      cc3_timer_wait_ms (500);
275
      break;
276
    }
277

    
278
  } while (cc3_timer_get_current_ms () < (start_time + 2000));
279

    
280

    
281
cmucam2_start:
282
  sw_color_space=DEFAULT_COLOR;
283
  auto_led = true;
284
  poll_mode = false;
285
  frame_stream_mode = false;
286
  line_mode = 0;
287
  buf_mode = false;
288
  packet_filter_flag = false;
289
  t_pkt_mask = 0xFF;
290
  s_pkt_mask = 0xFF;
291
  h_pkt.bins = 28;
292
  fd_pkt.coi = 1;
293
  fd_pkt.template_width = 8;
294
  fd_pkt.template_height = 8;
295
  t_pkt.track_invert = false;
296
  t_pkt.noise_filter = 0;
297

    
298
  // set to 0 since cmucam2 appears to initialize to this
299
  t_pkt.lower_bound.channel[0] = 0;
300
  t_pkt.upper_bound.channel[0] = 0;
301
  t_pkt.lower_bound.channel[1] = 0;
302
  t_pkt.upper_bound.channel[1] = 0;
303
  t_pkt.lower_bound.channel[2] = 0;
304
  t_pkt.upper_bound.channel[2] = 0;
305

    
306
  raw_mode_output = false;
307
  raw_mode_no_confirmations = false;
308
  raw_mode_input = false;
309

    
310
  servo_settings.x = SERVO_MID;
311
  servo_settings.y = SERVO_MID;
312
  servo_settings.pan_range_far = 32;
313
  servo_settings.pan_range_near = 20;
314
  servo_settings.pan_step = 20;
315
  servo_settings.tilt_range_far = 32;
316
  servo_settings.tilt_range_near = 20;
317
  servo_settings.tilt_step = 20;
318

    
319

    
320
  cc3_camera_set_power_state (true);
321
  cc3_camera_set_resolution (CC3_CAMERA_RESOLUTION_LOW);
322

    
323
  printf ("%s\r", VERSION_BANNER);
324

    
325
  cc3_gpio_set_mode (0, CC3_GPIO_MODE_SERVO);
326
  cc3_gpio_set_mode (1, CC3_GPIO_MODE_SERVO);
327
  cc3_gpio_set_mode (2, CC3_GPIO_MODE_SERVO);
328
  cc3_gpio_set_mode (3, CC3_GPIO_MODE_SERVO);
329

    
330
  cc3_gpio_set_servo_position (0, SERVO_MID);
331
  cc3_gpio_set_servo_position (1, SERVO_MID);
332
  cc3_gpio_set_servo_position (2, SERVO_MID);
333
  cc3_gpio_set_servo_position (3, SERVO_MID);
334

    
335
  cc3_pixbuf_frame_set_subsample (CC3_SUBSAMPLE_NEAREST, 2, 1);
336

    
337
  if (demo_mode) {
338
    cc3_led_set_state (0, true);
339
    cc3_timer_wait_ms (5000);
340
    cc3_camera_set_auto_exposure (false);
341
    cc3_camera_set_auto_white_balance (false);
342
    // Wait for second button press as target lock
343
    while (1) {
344
      cc3_led_set_state (0, true);
345
      cc3_timer_wait_ms (100);
346
      cc3_led_set_state (0, false);
347
      cc3_timer_wait_ms (100);
348
      if (cc3_button_get_state () == 1)
349
        break;
350
    }
351

    
352
  }
353
  while (true) {
354
    cc3_channel_t old_coi;
355

    
356
    print_prompt ();
357
    error = false;
358
    if (demo_mode == true) {
359
      n = 0;
360
      command = TRACK_WINDOW;
361
    }
362
    else if (raw_mode_input) {
363
      n = cmucam2_get_command_raw (&command, arg_list);
364
    }
365
    else {
366
      n = cmucam2_get_command (&command, arg_list);
367
    }
368
    if (n != -1) {
369
      switch (command) {
370

    
371
      case RESET:
372
        if (n != 0) {
373
          error = true;
374
          break;
375
        }
376

    
377
        print_ACK ();
378
        print_cr ();
379
        goto cmucam2_start;
380
        break;
381

    
382
      case READ_FRAME:
383
        if (n != 0) {
384
          error = true;
385
          break;
386
        }
387
        print_ACK ();
388
        cc3_pixbuf_load ();
389
        break;
390

    
391
      case OUTPUT_MASK:
392
        if (n != 2 || arg_list[0] > 1) {
393
          error = true;
394
          break;
395
        }
396
        if (arg_list[0] == 0)
397
          t_pkt_mask = arg_list[1];
398
        if (arg_list[0] == 1)
399
          s_pkt_mask = arg_list[1];
400
        print_ACK ();
401
        break;
402

    
403

    
404
      case GET_VERSION:
405
        if (n != 0) {
406
          error = true;
407
          break;
408
        }
409

    
410
        print_ACK ();
411
        // no different in raw mode
412
        printf ("%s", VERSION_BANNER);
413
        break;
414

    
415
      case LED_0:
416
        if (n != 1 || arg_list[0] > 2) {
417
          error = true;
418
          break;
419
        }
420

    
421
        print_ACK ();
422
        auto_led = false;
423
        if (arg_list[0] == 0)
424
          cc3_led_set_state (0, false);
425
        if (arg_list[0] == 1)
426
          cc3_led_set_state (0, true);
427
        if (arg_list[0] == 2)
428
          auto_led = true;
429
        break;
430

    
431
      case BUF_MODE:
432
        if (n != 1 || arg_list[0] > 1) {
433
          error = true;
434
          break;
435
        }
436

    
437
        print_ACK ();
438
        if (arg_list[0] == 1)
439
          buf_mode = true;
440
        else
441
          buf_mode = false;
442
        break;
443

    
444
      case PACKET_FILTER:
445
        if (n != 1 || arg_list[0] > 1) {
446
          error = true;
447
          break;
448
        }
449

    
450
        print_ACK ();
451
        if (arg_list[0] == 1)
452
          packet_filter_flag = true;
453
        else
454
          packet_filter_flag = false;
455
        break;
456

    
457
      case POLL_MODE:
458
        if (n != 1 || arg_list[0] > 1) {
459
          error = true;
460
          break;
461
        }
462

    
463
        print_ACK ();
464
        if (arg_list[0] == 1)
465
          poll_mode = true;
466
        else
467
          poll_mode = false;
468
        break;
469

    
470
        
471
      case FRAME_STREAM:
472
        if (n != 1 || arg_list[0] > 1) {
473
          error = true;
474
          break;
475
        }
476

    
477
        print_ACK ();
478
        if (arg_list[0] == 1)
479
          frame_stream_mode = true;
480
        else
481
          frame_stream_mode = false;
482
        break;
483

    
484
      case SERVO_PARAMETERS:
485
        if (n != 6) {
486
          error = true;
487
          break;
488
        }
489

    
490
        print_ACK ();
491
        servo_settings.pan_range_far = arg_list[0];
492
        servo_settings.pan_range_near = arg_list[1];
493
        servo_settings.pan_step = arg_list[2];
494
        servo_settings.tilt_range_far = arg_list[3];
495
        servo_settings.tilt_range_near = arg_list[4];
496
        servo_settings.tilt_step = arg_list[5];
497
        break;
498

    
499
      case SERVO_MASK:
500
        if (n != 1) {
501
          error = true;
502
          break;
503
        }
504

    
505
        print_ACK ();
506
        servo_settings.x_control = !!(arg_list[0] & 0x1);
507
        servo_settings.y_control = !!(arg_list[0] & 0x2);
508
        servo_settings.x_report = !!(arg_list[0] & 0x4);
509
        servo_settings.y_report = !!(arg_list[0] & 0x8);
510
        break;
511

    
512
      case HI_RES:
513
        if (n != 1) {
514
          error = true;
515
          break;
516
        }
517

    
518
        print_ACK ();
519
        if (arg_list[0] == 1)
520
          cc3_camera_set_resolution (CC3_CAMERA_RESOLUTION_HIGH);
521
        else
522
          cc3_camera_set_resolution (CC3_CAMERA_RESOLUTION_LOW);
523
        cc3_pixbuf_frame_set_subsample (CC3_SUBSAMPLE_NEAREST, 2, 1);
524
        break;
525

    
526

    
527

    
528
      case LOAD_FRAME:
529
        if (n != 0) {
530
          error = true;
531
          break;
532
        }
533
        print_ACK ();
534
        fd_pkt.total_x = cc3_g_pixbuf_frame.width;
535
        fd_pkt.total_y = cc3_g_pixbuf_frame.height;
536
        fd_pkt.load_frame = 1;  // load a new frame
537
        cmucam2_load_frame (&fd_pkt, buf_mode, sw_color_space);
538
        break;
539

    
540
      case HIRES_DIFF:
541
        if (n != 1 || arg_list[0] > 1) {
542
          error = true;
543
          break;
544
        }
545
        print_ACK ();
546
        if (arg_list[0] == 0) {
547
          fd_pkt.template_width = 8;
548
          fd_pkt.template_height = 8;
549
        }
550
        else {
551
          fd_pkt.template_width = 16;
552
          fd_pkt.template_height = 16;
553
        }
554
        break;
555

    
556
      case FRAME_DIFF:
557
        if (n != 1) {
558
          error = true;
559
          break;
560
        }
561
        print_ACK ();
562
        fd_pkt.threshold = arg_list[0];
563
        fd_pkt.load_frame = 0;
564
        fd_pkt.total_x = cc3_g_pixbuf_frame.width;
565
        fd_pkt.total_y = cc3_g_pixbuf_frame.height;
566
        cmucam2_frame_diff (&fd_pkt, poll_mode, line_mode, buf_mode, auto_led, sw_color_space,
567
                            0);
568
        break;
569

    
570
      case FRAME_DIFF_CHANNEL:
571
        if (n != 1 || arg_list[0] > 2) {
572
          error = true;
573
          break;
574
        }
575
        print_ACK ();
576
        fd_pkt.coi = arg_list[0];
577
        break;
578

    
579

    
580
      case CONF_HISTOGRAM:
581
        if (n != 1 || arg_list[0] < 1) {
582
          error = true;
583
          break;
584
        }
585
        h_pkt.bins = arg_list[0];
586
        print_ACK ();
587

    
588
        break;
589

    
590

    
591
      case TRACK_INVERT:
592
        if (n != 1 || arg_list[0] > 1) {
593
          error = true;
594
          break;
595
        }
596

    
597
        print_ACK ();
598
        if (arg_list[0] == 0)
599
          t_pkt.track_invert = 0;
600
        else
601
          t_pkt.track_invert = 1;
602
        break;
603

    
604
      case COLOR_SPACE:
605
        if (n != 1) {
606
          error = true;
607
          break;
608
        }
609

    
610
        print_ACK ();
611
        sw_color_space= arg_list[0];
612
        break;
613

    
614
      case NOISE_FILTER:
615
        if (n != 1) {
616
          error = true;
617
          break;
618
        }
619

    
620
        print_ACK ();
621
        t_pkt.noise_filter = arg_list[0];
622
        break;
623

    
624
      case LINE_MODE:
625
        if (n != 2) {
626
          error = true;
627
          break;
628
        }
629

    
630
        print_ACK ();
631
        // FIXME: Make bitmasks later
632
        if (arg_list[0] == 0) {
633
          if (arg_list[1] == 1)
634
            line_mode = 1;
635
          else if (arg_list[1] == 2)
636
                  line_mode=2;
637
          else
638
            line_mode = 0;
639
        }
640
        break;
641

    
642

    
643
      case SEND_JPEG:
644
        if (n != 0 && n != 1) {
645
          error = true;
646
          break;
647
        }
648

    
649
        print_ACK ();
650
        // ignore raw mode
651
        cc3_jpeg_send_simple ();
652
        printf ("JPG_END\r");
653
        break;
654

    
655

    
656
      case SEND_FRAME:
657
        old_coi = cc3_g_pixbuf_frame.coi;
658
        if (n == 1) {
659
          if (arg_list[0] > 4) {
660
            error = true;
661
            break;
662
          }
663
          cc3_pixbuf_frame_set_coi (arg_list[0]);
664
        }
665
        else if (n > 1) {
666
          error = true;
667
          break;
668
        }
669

    
670
        print_ACK ();
671
        do {
672
                cmucam2_send_image_direct (auto_led,sw_color_space);
673
                // Check to see if data is on the UART to break from frame_stream_mode
674
                   if (!cc3_uart_has_data (0)) {
675
                      if (getchar () == '\r')
676
                        break;
677
                    }
678
          } while (frame_stream_mode);
679
        
680
        cc3_pixbuf_frame_set_coi (old_coi);
681
        break;
682

    
683
      case RAW_MODE:
684
        if (n != 1) {
685
          error = true;
686
          break;
687
        }
688

    
689
        raw_mode_output = arg_list[0] & 1;
690
        raw_mode_no_confirmations = arg_list[0] & 2;
691
        raw_mode_input = arg_list[0] & 4;
692
        print_ACK ();           // last because ACK may be supressed
693
        break;
694

    
695
      case CAMERA_REG:
696
        if (n % 2 != 0 || n < 2) {
697
          error = true;
698
          break;
699
        }
700

    
701
        print_ACK ();
702
        for (int i = 0; i < n; i += 2)
703
          cc3_camera_set_raw_register (arg_list[i], arg_list[i + 1]);
704
        break;
705

    
706
      case CAMERA_POWER:
707
        if (n != 1) {
708
          error = true;
709
          break;
710
        }
711

    
712
        print_ACK ();
713
        {
714
          // save
715
          uint16_t x_0 = cc3_g_pixbuf_frame.x0;
716
          uint16_t y_0 = cc3_g_pixbuf_frame.y0;
717
          uint16_t x_1 = cc3_g_pixbuf_frame.x1;
718
          uint16_t y_1 = cc3_g_pixbuf_frame.y1;
719
          uint8_t x_step = cc3_g_pixbuf_frame.x_step;
720
          uint8_t y_step = cc3_g_pixbuf_frame.y_step;
721
          cc3_subsample_mode_t ss_mode = cc3_g_pixbuf_frame.subsample_mode;
722

    
723
          cc3_camera_set_power_state (arg_list[0]);
724

    
725
          // restore
726
          cc3_pixbuf_frame_set_roi (x_0, y_0, x_1, y_1);
727
          cc3_pixbuf_frame_set_subsample (ss_mode, x_step, y_step);
728
        }
729
        break;
730

    
731
      case VIRTUAL_WINDOW:
732
        if (n != 4) {
733
          error = true;
734
          break;
735
        }
736

    
737
        print_ACK ();
738
        cc3_pixbuf_frame_set_roi (arg_list[0] * 2,
739
                                  arg_list[1], arg_list[2] * 2, arg_list[3]);
740
        break;
741

    
742
      case GET_TRACK:
743
        if (n != 0) {
744
          error = true;
745
          break;
746
        }
747

    
748
        print_ACK ();
749
        if (raw_mode_output) {
750
          putchar (255);
751
          raw_print (t_pkt.lower_bound.channel[0]);
752
          raw_print (t_pkt.lower_bound.channel[1]);
753
          raw_print (t_pkt.lower_bound.channel[2]);
754
          raw_print (t_pkt.upper_bound.channel[0]);
755
          raw_print (t_pkt.upper_bound.channel[1]);
756
          raw_print (t_pkt.upper_bound.channel[2]);
757
        }
758
        else {
759
          printf ("%d %d %d %d %d %d\r", t_pkt.lower_bound.channel[0],
760
                  t_pkt.lower_bound.channel[1], t_pkt.lower_bound.channel[2],
761
                  t_pkt.upper_bound.channel[0], t_pkt.upper_bound.channel[1],
762
                  t_pkt.upper_bound.channel[2]);
763
        }
764
        break;
765

    
766
      case GET_WINDOW:
767
        if (n != 0) {
768
          error = true;
769
          break;
770
        }
771

    
772
        print_ACK ();
773
        if (raw_mode_output) {
774
          putchar (255);
775
          raw_print (cc3_g_pixbuf_frame.x0 / 2);
776
          raw_print (cc3_g_pixbuf_frame.y0);
777
          raw_print (cc3_g_pixbuf_frame.x1 / 2);
778
          raw_print (cc3_g_pixbuf_frame.y1);
779
        }
780
        else {
781
          printf ("%d %d %d %d\r", cc3_g_pixbuf_frame.x0 / 2,
782
                  cc3_g_pixbuf_frame.y0, cc3_g_pixbuf_frame.x1 / 2,
783
                  cc3_g_pixbuf_frame.y1);
784
        }
785
        break;
786

    
787
      case DOWN_SAMPLE:
788
        if (n != 2) {
789
          error = true;
790
          break;
791
        }
792

    
793
        print_ACK ();
794
        cc3_pixbuf_frame_set_subsample (CC3_SUBSAMPLE_NEAREST,
795
                                        arg_list[0] * 2, arg_list[1]);
796
        break;
797

    
798
      case SET_TRACK:
799
        if (n != 0 && n != 6) {
800
          error = true;
801
          break;
802
        }
803
        print_ACK ();
804
        if (n == 6) {
805
          t_pkt.lower_bound.channel[0] = arg_list[0];
806
          t_pkt.upper_bound.channel[0] = arg_list[1];
807
          t_pkt.lower_bound.channel[1] = arg_list[2];
808
          t_pkt.upper_bound.channel[1] = arg_list[3];
809
          t_pkt.lower_bound.channel[2] = arg_list[4];
810
          t_pkt.upper_bound.channel[2] = arg_list[5];
811
        }
812
        break;
813

    
814
      case TRACK_COLOR:
815
        if (n != 0 && n != 6) {
816
          error = true;
817
          break;
818
        }
819

    
820
        print_ACK ();
821
        if (n == 6) {
822
          t_pkt.lower_bound.channel[0] = arg_list[0];
823
          t_pkt.upper_bound.channel[0] = arg_list[1];
824
          t_pkt.lower_bound.channel[1] = arg_list[2];
825
          t_pkt.upper_bound.channel[1] = arg_list[3];
826
          t_pkt.lower_bound.channel[2] = arg_list[4];
827
          t_pkt.upper_bound.channel[2] = arg_list[5];
828
        }
829
        cmucam2_track_color (&t_pkt, poll_mode, line_mode, auto_led,
830
                             &servo_settings, buf_mode, sw_color_space, 0);
831
        break;
832

    
833
      case TRACK_WINDOW:
834
        if (n != 0 && n != 1) {
835
          error = true;
836
          break;
837
        }
838
        else {
839
          uint32_t threshold, x0, y0, x1, y1;
840
          int32_t tmp;
841
          threshold = 30;
842
          if (n == 1)
843
            threshold = arg_list[0];
844
          print_ACK ();
845
          // set window to 1/2 size
846
          x0 = cc3_g_pixbuf_frame.x0 + cc3_g_pixbuf_frame.width / 4;
847
          x1 = cc3_g_pixbuf_frame.x1 - cc3_g_pixbuf_frame.width / 4;
848
          y0 = cc3_g_pixbuf_frame.y0 + cc3_g_pixbuf_frame.width / 4;
849
          y1 = cc3_g_pixbuf_frame.y1 - cc3_g_pixbuf_frame.width / 4;
850
          cc3_pixbuf_frame_set_roi (x0, y0, x1, y1);
851
          // call get mean
852
          cmucam2_get_mean (&s_pkt, 1, line_mode, buf_mode,sw_color_space, 1);
853
          // set window back to full size
854
          x0 = 0;
855
          x1 = cc3_g_pixbuf_frame.raw_width;
856
          y0 = 0;
857
          y1 = cc3_g_pixbuf_frame.raw_height;
858
          cc3_pixbuf_frame_set_roi (x0, y0, x1, y1);
859
          // fill in parameters and call track color
860
          tmp = s_pkt.mean.channel[0] - threshold;
861
          if (tmp < 16)
862
            tmp = 16;
863
          if (tmp > 240)
864
            tmp = 240;
865
          t_pkt.lower_bound.channel[0] = tmp;
866
          tmp = s_pkt.mean.channel[0] + threshold;
867
          if (tmp < 16)
868
            tmp = 16;
869
          if (tmp > 240)
870
            tmp = 240;
871
          t_pkt.upper_bound.channel[0] = tmp;
872
          tmp = s_pkt.mean.channel[1] - threshold;
873
          if (tmp < 16)
874
            tmp = 16;
875
          if (tmp > 240)
876
            tmp = 240;
877
          t_pkt.lower_bound.channel[1] = tmp;
878
          tmp = s_pkt.mean.channel[1] + threshold;
879
          if (tmp < 16)
880
            tmp = 16;
881
          if (tmp > 240)
882
            tmp = 240;
883
          t_pkt.upper_bound.channel[1] = tmp;
884
          tmp = s_pkt.mean.channel[2] - threshold;
885
          if (tmp < 16)
886
            tmp = 16;
887
          if (tmp > 240)
888
            tmp = 240;
889
          t_pkt.lower_bound.channel[2] = tmp;
890
          tmp = s_pkt.mean.channel[2] + threshold;
891
          if (tmp < 16)
892
            tmp = 16;
893
          if (tmp > 240)
894
            tmp = 240;
895
          t_pkt.upper_bound.channel[2] = tmp;
896
          cmucam2_track_color (&t_pkt, poll_mode, line_mode, auto_led,
897
                               &servo_settings, buf_mode,sw_color_space, 0);
898
        }
899
        demo_mode = false;
900
        break;
901

    
902

    
903
      case GET_MEAN:
904
        if (n != 0) {
905
          error = true;
906
          break;
907
        }
908

    
909
        print_ACK ();
910
        cmucam2_get_mean (&s_pkt, poll_mode, line_mode, buf_mode,sw_color_space, 0);
911
        break;
912

    
913

    
914
      case GET_HISTOGRAM:
915
        if (n != 1 || arg_list[0] > 2) {
916
          error = true;
917
          break;
918
        }
919

    
920
        print_ACK ();
921
        h_pkt.channel = arg_list[0];
922
        cmucam2_get_histogram (&h_pkt, poll_mode, buf_mode,sw_color_space, 0);
923
        break;
924

    
925

    
926
      case SET_SERVO:
927
        if (n != 2 || arg_list[0] > 4) {
928
          error = true;
929
          break;
930
        }
931

    
932
        print_ACK ();
933
        cc3_gpio_set_mode (arg_list[0], CC3_GPIO_MODE_SERVO);
934
        cc3_gpio_set_servo_position (arg_list[0], arg_list[1]);
935
        if (arg_list[0] == 0)
936
          servo_settings.x = arg_list[1];
937
        if (arg_list[0] == 1)
938
          servo_settings.y = arg_list[1];
939
        break;
940

    
941
      case GET_SERVO:
942
        if (n != 1) {
943
          error = true;
944
          break;
945
        }
946

    
947
        print_ACK ();
948

    
949
        {
950
          uint8_t servo = cc3_gpio_get_servo_position (arg_list[0]);
951
          if (raw_mode_output) {
952
            putchar (255);
953
            raw_print (servo);
954
          }
955
          else {
956
            printf ("%d\r", servo);
957
          }
958
          break;
959
        }
960

    
961
      case GET_INPUT:
962
        if (n != 0) {
963
          error = true;
964
          break;
965
        }
966

    
967
        print_ACK ();
968
        {
969
          uint8_t input =
970
            (cc3_gpio_get_value (arg_list[0])) |
971
            (cc3_gpio_get_value (arg_list[1]) << 1) |
972
            (cc3_gpio_get_value (arg_list[2]) << 2) |
973
            (cc3_gpio_get_value (arg_list[3]) << 3);
974
          if (raw_mode_output) {
975
            putchar (255);
976
            raw_print (input);
977
          }
978
          else {
979
            printf ("%d\r", input);
980
          }
981
        }
982
        break;
983

    
984

    
985
      case SET_INPUT:
986
        if (n != 1) {
987
          error = true;
988
          break;
989
        }
990
        print_ACK ();
991
        cc3_gpio_set_mode (arg_list[0], CC3_GPIO_MODE_INPUT);
992

    
993
        break;
994

    
995
      case GET_BUTTON:
996
        if (n != 0) {
997
          error = true;
998
          break;
999
        }
1000

    
1001
        print_ACK ();
1002

    
1003
        {
1004
          int button = cc3_button_get_and_reset_trigger ()? 1 : 0;
1005
          if (raw_mode_output) {
1006
            putchar (255);
1007
            raw_print (button);
1008
          }
1009
          else {
1010
            printf ("%d\r", button);
1011
          }
1012
        }
1013
        break;
1014

    
1015
      case SERVO_OUTPUT:
1016
        if (n != 2) {
1017
          error = true;
1018
          break;
1019
        }
1020
        print_ACK ();
1021
        cc3_gpio_set_mode (arg_list[0], CC3_GPIO_MODE_OUTPUT);
1022
        cc3_gpio_set_value (arg_list[0], arg_list[1]);
1023

    
1024
        break;
1025

    
1026
      default:
1027
        print_ACK ();
1028
        break;
1029
      }
1030
    }
1031
    else
1032
      error = true;
1033

    
1034
    if (error)
1035
      print_NCK ();
1036
  }
1037

    
1038

    
1039
  return 0;
1040
}
1041

    
1042

    
1043
void cmucam2_send_image_direct (bool auto_led, uint8_t sw_color_space)
1044
{
1045
  cc3_pixbuf_load ();
1046

    
1047
  uint32_t x, y;
1048
  uint32_t size_x, size_y;
1049
  uint8_t *row = cc3_malloc_rows (1);
1050
  uint8_t num_channels = cc3_g_pixbuf_frame.coi == CC3_CHANNEL_ALL ? 3 : 1;
1051

    
1052

    
1053
  size_x = cc3_g_pixbuf_frame.width;
1054
  size_y = cc3_g_pixbuf_frame.height;
1055

    
1056
  putchar (1);
1057
  putchar (size_x);
1058
  if (size_y > 255)
1059
    size_y = 255;
1060
  putchar (size_y);
1061
  for (y = 0; y < size_y; y++) {
1062
    putchar (2);
1063
    if (auto_led) {
1064
      if (y % 4 == 0)
1065
        cc3_led_set_state (0, true);
1066
      else
1067
        cc3_led_set_state (0, false);
1068
    }
1069
    cc3_pixbuf_read_rows (row, 1);
1070
    if(sw_color_space==HSV_COLOR && num_channels==CC3_CHANNEL_ALL )  cc3_rgb2hsv_row(row,size_x);
1071
    for (x = 0; x < size_x * num_channels; x++) {
1072
      uint8_t p = row[x];
1073

    
1074
      // avoid confusion from FIFO corruptions
1075
      if (p < 16) {
1076
        p = 16;
1077
      }
1078
      else if (p > 240) {
1079
        p = 240;
1080
      }
1081
      putchar (p);
1082
    }
1083
  }
1084
  putchar (3);
1085

    
1086
  cc3_led_set_state (0, false);
1087
  free (row);
1088
}
1089

    
1090

    
1091

    
1092
void cmucam2_get_histogram (cc3_histogram_pkt_t * h_pkt, bool poll_mode,
1093
                            bool buf_mode, uint8_t sw_color_space, bool quiet)
1094
{
1095
  cc3_image_t img;
1096
  img.channels = 3;
1097
  img.width = cc3_g_pixbuf_frame.width;
1098
  img.height = 1;               // image will hold just 1 row for scanline processing
1099
  img.pix = malloc (3 * img.width);
1100
  h_pkt->hist = malloc (h_pkt->bins * sizeof (uint32_t));
1101
  if (img.pix == NULL || h_pkt->hist == NULL) {
1102
    printf ("INTERNAL ERROR\r");
1103
    return;
1104
  }
1105
  do {
1106
    if (!buf_mode)
1107
      cc3_pixbuf_load ();
1108
    else
1109
      cc3_pixbuf_rewind ();
1110
    if (cc3_histogram_scanline_start (h_pkt) != 0) {
1111
      while (cc3_pixbuf_read_rows (img.pix, 1)) {
1112
        if(sw_color_space==HSV_COLOR  && img.channels==CC3_CHANNEL_ALL)  cc3_rgb2hsv_row(img.pix,img.width);
1113
        cc3_histogram_scanline (&img, h_pkt);
1114
      }
1115
      cc3_histogram_scanline_finish (h_pkt);
1116
      while (!cc3_uart_has_data (0)) {
1117
        if (getchar () == '\r') {
1118
          free (img.pix);
1119
          free (h_pkt->hist);
1120
          return;
1121
        }
1122
      }
1123
      if (!quiet)
1124
        cmucam2_write_h_packet (h_pkt);
1125
    }
1126
    if (!cc3_uart_has_data (0)) {
1127
      if (getchar () == '\r')
1128
        break;
1129
    }
1130
  } while (!poll_mode);
1131

    
1132
  free (img.pix);
1133
  free (h_pkt->hist);
1134

    
1135

    
1136
}
1137

    
1138
void cmucam2_load_frame (cc3_frame_diff_pkt_t * pkt, bool buf_mode, uint8_t sw_color_space)
1139
{
1140
  cc3_image_t img;
1141
  uint8_t old_coi;
1142

    
1143
  old_coi = cc3_g_pixbuf_frame.coi;
1144
  cc3_pixbuf_frame_set_coi (pkt->coi);
1145

    
1146
  img.channels = 1;
1147
  img.width = cc3_g_pixbuf_frame.width;
1148
  img.height = 1;               // image will hold just 1 row for scanline processing
1149
  img.pix = malloc (img.width);
1150

    
1151
  if (!buf_mode)
1152
    cc3_pixbuf_load ();
1153
  else
1154
    cc3_pixbuf_rewind ();
1155

    
1156
  if (cc3_frame_diff_scanline_start (pkt) != 0) {
1157
    while (cc3_pixbuf_read_rows (img.pix, 1)) {
1158
      if(sw_color_space==HSV_COLOR  && img.channels==CC3_CHANNEL_ALL)  cc3_rgb2hsv_row(img.pix,img.width);
1159
      cc3_frame_diff_scanline (&img, pkt);
1160
    }
1161
    cc3_frame_diff_scanline_finish (pkt);
1162
  }
1163
  else
1164
    printf ("frame diff start error\r");
1165

    
1166
  cc3_pixbuf_frame_set_coi (old_coi);
1167
  free (img.pix);
1168

    
1169

    
1170
}
1171

    
1172
void cmucam2_frame_diff (cc3_frame_diff_pkt_t * pkt,
1173
                         bool poll_mode, bool line_mode, bool buf_mode, 
1174
                         bool auto_led, uint8_t sw_color_space, bool quiet)
1175
{
1176
  cc3_track_pkt_t t_pkt;
1177
  cc3_image_t img;
1178
  uint8_t old_coi;
1179

    
1180
  bool prev_packet_empty = true;
1181

    
1182
  old_coi = cc3_g_pixbuf_frame.coi;
1183
  cc3_pixbuf_frame_set_coi (pkt->coi);
1184
  img.channels = 1;
1185
  img.width = cc3_g_pixbuf_frame.width;
1186
  img.height = 1;               // image will hold just 1 row for scanline processing
1187
  img.pix = malloc (img.width);
1188
  pkt->current_template =
1189
    malloc (pkt->template_width * pkt->template_height * sizeof (uint32_t));
1190
  if (pkt->current_template == NULL)
1191
    printf ("Malloc failed in frame diff\r");
1192
  do {
1193
    if (!buf_mode)
1194
      cc3_pixbuf_load ();
1195
    else
1196
      cc3_pixbuf_rewind ();
1197

    
1198
    if (cc3_frame_diff_scanline_start (pkt) != 0) {
1199

    
1200
      while (cc3_pixbuf_read_rows (img.pix, 1)) {
1201
        if(sw_color_space==HSV_COLOR && img.channels==CC3_CHANNEL_ALL)  cc3_rgb2hsv_row(img.pix,img.width);
1202
        cc3_frame_diff_scanline (&img, pkt);
1203
      }
1204
      cc3_frame_diff_scanline_finish (pkt);
1205

    
1206
      while (!cc3_uart_has_data (0)) {
1207
        if (getchar () == '\r') {
1208
          cc3_pixbuf_frame_set_coi (old_coi);
1209
          free (pkt->current_template);
1210
          free (img.pix);
1211
          return;
1212
        }
1213
      }
1214
      if (!quiet) {
1215
        t_pkt.x0 = pkt->x0 + 1;
1216
        t_pkt.y0 = pkt->y0 + 1;
1217
        t_pkt.x1 = pkt->x1 + 1;
1218
        t_pkt.y1 = pkt->y1 + 1;
1219
        t_pkt.num_pixels = pkt->num_pixels;
1220
        t_pkt.centroid_x = pkt->centroid_x + 1;
1221
        t_pkt.centroid_y = pkt->centroid_y + 1;
1222
        t_pkt.int_density = pkt->int_density;
1223
        if (auto_led) {
1224
          if (t_pkt.num_pixels > 2)
1225
            cc3_led_set_state (0, true);
1226
          else
1227
            cc3_led_set_state (0, false);
1228
        }
1229

    
1230
        if (!(packet_filter_flag &&
1231
              t_pkt.num_pixels == 0 && prev_packet_empty)) {
1232
          cmucam2_write_t_packet (&t_pkt, NULL);
1233
        }
1234
        prev_packet_empty = t_pkt.num_pixels == 0;
1235
      }
1236
    }
1237

    
1238

    
1239
    if (!cc3_uart_has_data (0)) {
1240
      if (getchar () == '\r')
1241
        break;
1242
    }
1243
  } while (!poll_mode);
1244

    
1245
  cc3_pixbuf_frame_set_coi (old_coi);
1246
  free (pkt->current_template);
1247
  free (img.pix);
1248
}
1249

    
1250
void cmucam2_get_mean (cc3_color_info_pkt_t * s_pkt,
1251
                       bool poll_mode, bool line_mode, bool buf_mode, uint8_t sw_color_space,
1252
                       bool quiet)
1253
{
1254
  cc3_image_t img;
1255
  img.channels = 3;
1256
  img.width = cc3_g_pixbuf_frame.width;
1257
  img.height = 1;               // image will hold just 1 row for scanline processing
1258
  img.pix = malloc (3 * img.width);
1259
  do {
1260
    if (!buf_mode)
1261
      cc3_pixbuf_load ();
1262
    else
1263
      cc3_pixbuf_rewind ();
1264
    if (cc3_color_info_scanline_start (s_pkt) != 0) {
1265
      while (cc3_pixbuf_read_rows (img.pix, 1)) {
1266
        if(sw_color_space==HSV_COLOR && img.channels==CC3_CHANNEL_ALL)  cc3_rgb2hsv_row(img.pix,img.width);
1267
        cc3_color_info_scanline (&img, s_pkt);
1268
      }
1269
      cc3_color_info_scanline_finish (s_pkt);
1270
      while (!cc3_uart_has_data (0)) {
1271
        if (getchar () == '\r') {
1272
          free (img.pix);
1273
          return;
1274
        }
1275
      }
1276
      if (!quiet)
1277
        cmucam2_write_s_packet (s_pkt);
1278
    }
1279
    if (!cc3_uart_has_data (0)) {
1280
      if (getchar () == '\r')
1281
        break;
1282
    }
1283
  } while (!poll_mode);
1284

    
1285
  free (img.pix);
1286
}
1287

    
1288
void cmucam2_track_color (cc3_track_pkt_t * t_pkt,
1289
                          bool poll_mode,
1290
                          int8_t line_mode, bool auto_led,
1291
                          cmucam2_servo_t * servo_settings, bool buf_mode, uint8_t sw_color_space,
1292
                          bool quiet)
1293
{
1294
  cc3_image_t img;
1295
  uint16_t x_mid, y_mid;
1296
  int8_t t_step;
1297

    
1298
  bool prev_packet_empty = true;
1299

    
1300
  img.channels = 3;
1301
  img.width = cc3_g_pixbuf_frame.width;
1302
  img.height = 1;               // image will hold just 1 row for scanline processing
1303
  img.pix = cc3_malloc_rows (1);
1304
  if (img.pix == NULL) {
1305
    return;
1306
  }
1307

    
1308
  x_mid = cc3_g_pixbuf_frame.x0 + (cc3_g_pixbuf_frame.width / 2);
1309
  y_mid = cc3_g_pixbuf_frame.y0 + (cc3_g_pixbuf_frame.height / 2);
1310

    
1311

    
1312
  do {
1313
    if (!buf_mode)
1314
      cc3_pixbuf_load ();
1315
    else
1316
      cc3_pixbuf_rewind ();
1317
    if (cc3_track_color_scanline_start (t_pkt) != 0) {
1318
      uint8_t lm_width, lm_height;
1319
      uint8_t *lm;
1320
      lm_width = 0;
1321
      lm_height = 0;
1322
      if (line_mode==1) {
1323
        // FIXME: This doesn't make sense
1324
        lm = &t_pkt->binary_scanline;
1325
        lm_width = img.width / 8;
1326
        if (img.width % 8 != 0)
1327
          lm_width++;
1328
        if (!quiet)
1329
          putchar (0xAA);
1330
        if (cc3_g_pixbuf_frame.height > 255)
1331
          lm_height = 255;
1332
        else
1333
          lm_height = cc3_g_pixbuf_frame.height;
1334
        if (!quiet)
1335
          putchar (img.width);
1336
        if (!quiet)
1337
          putchar (lm_height);
1338
      }
1339

    
1340
    if (line_mode==2) {
1341
        // FIXME: This still doesn't make sense
1342
        lm = &t_pkt->binary_scanline;
1343
        lm_width = img.width / 8;
1344
        if (img.width % 8 != 0)
1345
          lm_width++;
1346
        if (!quiet)
1347
          putchar (0xFE);
1348
        if (cc3_g_pixbuf_frame.height > 255)
1349
          lm_height = 255;
1350
        else
1351
          lm_height = cc3_g_pixbuf_frame.height;
1352
        //if (!quiet)
1353
          //putchar (img.width);
1354
        if (!quiet)
1355
          putchar (lm_height);
1356
      }
1357

    
1358
      
1359
      while (cc3_pixbuf_read_rows (img.pix, 1)) {
1360
        if(sw_color_space==HSV_COLOR && img.channels==CC3_CHANNEL_ALL)  cc3_rgb2hsv_row(img.pix,img.width);
1361
        cc3_track_color_scanline (&img, t_pkt);
1362

    
1363
        if (line_mode==1) {
1364
          // keep this check here if you don't want the CMUcam2 GUI to hang after exiting a command in line mode
1365
          while (!cc3_uart_has_data (0)) {
1366
            if (getchar () == '\r') {
1367
              free (img.pix);
1368
              return;
1369
            }
1370
          }
1371
          for (int j = 0; j < lm_width; j++) {
1372
            if (lm[j] == 0xAA) {
1373
              if (!quiet)
1374
                putchar (0xAB);
1375
            }
1376
            else {
1377
              if (!quiet)
1378
                putchar (lm[j]);
1379
            }
1380
          }
1381
        }
1382

    
1383

    
1384
        if (line_mode==2) {
1385
          uint8_t min,max,p_count,conf;
1386
          uint32_t mean;
1387
          // keep this check here if you don't want the CMUcam2 GUI to hang after exiting a command in line mode
1388
          while (!cc3_uart_has_data (0)) {
1389
            if (getchar () == '\r') {
1390
              free (img.pix);
1391
              return;
1392
            }
1393
          }
1394
          mean=0;
1395
          min=255;
1396
          max=0;
1397
          p_count=0;
1398
          for (int j = 0; j < img.width; j++) {
1399
                 uint8_t block, offset;
1400
                block = j / 8;
1401
                offset = j % 8;
1402
                offset = 7 - offset;        
1403
                if((lm[block] & (1<<offset))!=0) 
1404
                        {
1405
                        // bit detected
1406
                        if(j<min) min=j;
1407
                        if(j>max) max=j;
1408
                        p_count++;
1409
                        mean+=j;
1410
                        }
1411
          }
1412
        mean=mean/p_count;
1413
        conf=((max-min)*100)/p_count;
1414
        if (!quiet)
1415
                printf( "%c%c%c%c%c",(uint8_t)mean,min,max,p_count,conf);
1416
        }
1417

    
1418

    
1419
        
1420
      }
1421
      // keep this check here if you don't want the CMUcam2 GUI to hang after exiting a command in line mode
1422
      while (!cc3_uart_has_data (0)) {
1423
        if (getchar () == '\r') {
1424
          free (img.pix);
1425
          return;
1426
        }
1427
      }
1428
      cc3_track_color_scanline_finish (t_pkt);
1429
      if (line_mode==1) {
1430
        if (!quiet)
1431
          putchar (0xAA);
1432
        if (!quiet)
1433
          putchar (0xAA);
1434
      }
1435

    
1436
     if (line_mode==2) {
1437
        if (!quiet)
1438
          putchar (0xFD);
1439
      }
1440

    
1441
      if (auto_led) {
1442
        if (t_pkt->int_density > 2)
1443
          cc3_led_set_state (0, true);
1444
        else
1445
          cc3_led_set_state (0, false);
1446
      }
1447

    
1448
      if (t_pkt->int_density > 5 && servo_settings != NULL) {
1449
        if (servo_settings->x_control) {
1450
          t_step = 0;
1451
          if (t_pkt->centroid_x > x_mid + servo_settings->pan_range_far)
1452
            t_step = servo_settings->pan_step;
1453
          else if (t_pkt->centroid_x > x_mid + servo_settings->pan_range_near)
1454
            t_step = (servo_settings->pan_step / 2);
1455

    
1456
          if (t_pkt->centroid_x < x_mid - servo_settings->pan_range_far)
1457
            t_step = -servo_settings->pan_step;
1458
          else if (t_pkt->centroid_x < x_mid - servo_settings->pan_range_near)
1459
            t_step = -(servo_settings->pan_step / 2);
1460

    
1461

    
1462
#ifdef SERVO_REVERSE_DIRECTION_PAN
1463
          servo_settings->x -= t_step;
1464
#else
1465
          servo_settings->x += t_step;
1466
#endif
1467
          t_step = 0;
1468
          if (servo_settings->x > SERVO_MAX)
1469
            servo_settings->x = SERVO_MAX;
1470
          if (servo_settings->x < SERVO_MIN)
1471
            servo_settings->x = SERVO_MIN;
1472
          cc3_gpio_set_servo_position (0, servo_settings->x);
1473
        }
1474
        if (servo_settings->y_control) {
1475
          if (t_pkt->centroid_y > y_mid + servo_settings->tilt_range_far)
1476
            t_step = servo_settings->tilt_step;
1477
          else if (t_pkt->centroid_y >
1478
                   y_mid + servo_settings->tilt_range_near)
1479
            t_step = servo_settings->tilt_step / 2;
1480

    
1481
          if (t_pkt->centroid_y < y_mid - servo_settings->tilt_range_far)
1482
            t_step = -(servo_settings->tilt_step);
1483
          else if (t_pkt->centroid_y <
1484
                   y_mid - servo_settings->tilt_range_near)
1485
            t_step = -(servo_settings->tilt_step / 2);
1486

    
1487
#ifdef SERVO_REVERSE_DIRECTION_TILT
1488
          servo_settings->y -= t_step;
1489
#else
1490
          servo_settings->y += t_step;
1491
#endif
1492

    
1493
          if (servo_settings->y > SERVO_MAX)
1494
            servo_settings->y = SERVO_MAX;
1495
          if (servo_settings->y < SERVO_MIN)
1496
            servo_settings->y = SERVO_MIN;
1497
          cc3_gpio_set_servo_position (1, servo_settings->y);
1498
        }
1499
      }
1500

    
1501
      if (!quiet) {
1502
        if (!(packet_filter_flag &&
1503
              t_pkt->num_pixels == 0 && prev_packet_empty)) {
1504
          cmucam2_write_t_packet (t_pkt, servo_settings);
1505
        }
1506
      }
1507
      prev_packet_empty = t_pkt->num_pixels == 0;
1508
    }
1509
    else
1510
      return;
1511

    
1512
    while (!cc3_uart_has_data (0)) {
1513
      if (getchar () == '\r')
1514
        break;
1515
    }
1516
  } while (!poll_mode);
1517
  free (img.pix);
1518
  return;
1519
}
1520

    
1521
void cmucam2_write_t_packet (cc3_track_pkt_t * pkt,
1522
                             cmucam2_servo_t * servo_settings)
1523
{
1524

    
1525
  // cap at 255
1526
  if (pkt->centroid_x > 255)
1527
    pkt->centroid_x = 255;
1528
  if (pkt->centroid_y > 255)
1529
    pkt->centroid_y = 255;
1530
  if (pkt->x0 > 255)
1531
    pkt->x0 = 255;
1532
  if (pkt->x1 > 255)
1533
    pkt->x1 = 255;
1534
  if (pkt->y1 > 255)
1535
    pkt->y1 = 255;
1536
  if (pkt->y0 > 255)
1537
    pkt->y0 = 255;
1538
  if (pkt->num_pixels > 255)
1539
    pkt->num_pixels = 255;
1540
  if (pkt->int_density > 255)
1541
    pkt->int_density = 255;
1542

    
1543
  // values to print
1544
  uint8_t p[8];
1545

    
1546
  if (pkt->num_pixels == 0) {
1547
    p[0] = p[1] = p[2] = p[3] = p[4] = p[5] = p[6] = p[7] = 0;
1548
  }
1549
  else {
1550
    p[0] = pkt->centroid_x;
1551
    p[1] = pkt->centroid_y;
1552
    p[2] = pkt->x0;
1553
    p[3] = pkt->y0;
1554
    p[4] = pkt->x1;
1555
    p[5] = pkt->y1;
1556
    p[6] = pkt->num_pixels;
1557
    p[7] = pkt->int_density;
1558
  }
1559

    
1560
  uint8_t mask = t_pkt_mask;
1561
  if (raw_mode_output) {
1562
    putchar (255);
1563
  }
1564
  printf ("T");
1565

    
1566
  // print out fields using mask
1567
  for (int i = 0; i < 8; i++) {
1568
    if (mask & 0x1) {
1569
      if (raw_mode_output) {
1570
        raw_print (p[i]);
1571
      }
1572
      else {
1573
        printf (" %d", p[i]);
1574
      }
1575
    }
1576
    mask >>= 1;
1577
  }
1578

    
1579
  // print servo settings?
1580
  if (servo_settings != NULL) {
1581
    uint8_t sx = servo_settings->x;
1582
    uint8_t sy = servo_settings->y;
1583

    
1584
    if (servo_settings->x_report) {
1585
      if (raw_mode_output) {
1586
        raw_print (sx);
1587
      }
1588
      else {
1589
        printf (" %d", sx);
1590
      }
1591
    }
1592
    if (servo_settings->y_report) {
1593
      if (raw_mode_output) {
1594
        raw_print (sy);
1595
      }
1596
      else {
1597
        printf (" %d", sy);
1598
      }
1599
    }
1600
  }
1601

    
1602
  print_cr ();
1603
}
1604

    
1605
void cmucam2_write_h_packet (cc3_histogram_pkt_t * pkt)
1606
{
1607
  uint32_t i;
1608
  uint32_t total_pix;
1609

    
1610
  total_pix = cc3_g_pixbuf_frame.width * cc3_g_pixbuf_frame.height;
1611

    
1612
  if (raw_mode_output) {
1613
    putchar (255);
1614
  }
1615
  printf ("H");
1616

    
1617
  for (i = 0; i < pkt->bins; i++) {
1618
    pkt->hist[i] = (pkt->hist[i] * 256) / total_pix;
1619
    if (pkt->hist[i] > 255)
1620
      pkt->hist[i] = 255;
1621

    
1622
    if (raw_mode_output) {
1623
      raw_print (pkt->hist[i]);
1624
    }
1625
    else {
1626
      printf (" %d", pkt->hist[i]);
1627
    }
1628
  }
1629

    
1630
  print_cr ();
1631
}
1632

    
1633
void cmucam2_write_s_packet (cc3_color_info_pkt_t * pkt)
1634
{
1635
  uint8_t pkt2[6];
1636
  uint8_t mask = s_pkt_mask;
1637

    
1638
  if (raw_mode_output) {
1639
    putchar (255);
1640
  }
1641

    
1642
  pkt2[0] = pkt->mean.channel[0];
1643
  pkt2[1] = pkt->mean.channel[1];
1644
  pkt2[2] = pkt->mean.channel[2];
1645
  pkt2[3] = pkt->deviation.channel[0];
1646
  pkt2[4] = pkt->deviation.channel[1];
1647
  pkt2[5] = pkt->deviation.channel[2];
1648

    
1649
  printf ("S");
1650

    
1651
  for (int i = 0; i < 6; i++) {
1652
    if (mask & 0x1) {
1653
      if (raw_mode_output) {
1654
        putchar (pkt2[i]);
1655
      }
1656
      else {
1657
        printf (" %d", pkt2[i]);
1658
      }
1659
    }
1660
    mask >>= 1;
1661
  }
1662

    
1663
  print_cr ();
1664
}
1665

    
1666
void print_ACK ()
1667
{
1668
  if (!raw_mode_no_confirmations)
1669
    printf ("ACK\r");
1670
}
1671

    
1672
void print_NCK ()
1673
{
1674
  if (!raw_mode_no_confirmations)
1675
    printf ("NCK\r");
1676
}
1677

    
1678
void print_prompt ()
1679
{
1680
  printf (":");
1681
}
1682

    
1683
void print_cr ()
1684
{
1685
  if (!raw_mode_output) {
1686
    printf ("\r");
1687
  }
1688
}
1689

    
1690
int32_t cmucam2_get_command (cmucam2_command_t * cmd, uint32_t * arg_list)
1691
{
1692
  char line_buf[MAX_LINE];
1693
  int c;
1694
  char *token;
1695
  bool fail = false;
1696
  uint32_t length, argc;
1697
  uint32_t i;
1698

    
1699
  length = 0;
1700
  c = 0;
1701
  while (c != '\r') {
1702
    c = getchar ();
1703

    
1704
    if (length < (MAX_LINE - 1) && c != EOF) {
1705
      line_buf[length] = c;
1706
      length++;
1707
    }
1708
    else {
1709
      // too long or EOF
1710
      return -1;
1711
    }
1712
  }
1713
  // null terminate
1714
  line_buf[length] = '\0';
1715

    
1716
  // check for empty command
1717
  if (line_buf[0] == '\r' || line_buf[0] == '\n') {
1718
    *cmd = RETURN;
1719
    return 0;
1720
  }
1721

    
1722
  // start looking for command
1723
  token = strtok (line_buf, " \r\n");
1724

    
1725
  if (token == NULL) {
1726
    // no command ?
1727
    return -1;
1728
  }
1729

    
1730
  // get name of the command
1731
  for (i = 0; i < strlen (token); i++) {
1732
    token[i] = toupper (token[i]);
1733
  }
1734

    
1735
  // do lookup of command
1736
  fail = true;
1737
  for (i = 0; i < CMUCAM2_CMDS_COUNT; i++) {
1738
    if (strcmp (token, cmucam2_cmds[i]) == 0) {
1739
      fail = false;
1740
      *cmd = i;
1741
      break;
1742
    }
1743
  }
1744
  if (fail) {
1745
    return -1;
1746
  }
1747

    
1748
  // now get the arguments
1749
  argc = 0;
1750
  while (true) {
1751
    // extract string from string sequence
1752
    token = strtok (NULL, " \r\n");
1753
    // check if there is nothing else to extract
1754
    if (token == NULL) {
1755
      // printf("Tokenizing complete\n");
1756
      return argc;
1757
    }
1758

    
1759
    // make sure the argument is fully numeric
1760
    for (i = 0; i < strlen (token); i++) {
1761
      if (!isdigit (token[i]))
1762
        return -1;
1763
    }
1764

    
1765
    // we have a valid token, add it
1766
    arg_list[argc] = atoi (token);
1767
    argc++;
1768
  }
1769

    
1770
  return -1;
1771
}
1772

    
1773
int32_t cmucam2_get_command_raw (cmucam2_command_t * cmd, uint32_t * arg_list)
1774
{
1775
  bool fail;
1776
  int c;
1777
  unsigned int i;
1778
  uint32_t argc;
1779

    
1780
  char cmd_str[3];
1781
  cmd_str[2] = '\0';
1782

    
1783
  // read characters
1784
  for (i = 0; i < 2; i++) {
1785
    c = getchar ();
1786
    if (c == EOF) {
1787
      return -1;
1788
    }
1789

    
1790
    cmd_str[i] = c;
1791
  }
1792

    
1793
  // do lookup of command
1794
  fail = true;
1795
  for (i = 0; i < CMUCAM2_CMDS_COUNT; i++) {
1796
    if (strcmp (cmd_str, cmucam2_cmds[i]) == 0) {
1797
      fail = false;
1798
      *cmd = i;
1799
      break;
1800
    }
1801
  }
1802
  if (fail) {
1803
    return -1;
1804
  }
1805

    
1806
  // read argc
1807
  c = getchar ();
1808
  if (c == EOF) {
1809
    return -1;
1810
  }
1811
  argc = c;
1812
  if (argc > MAX_ARGS) {
1813
    return -1;
1814
  }
1815

    
1816
  // read args
1817
  for (i = 0; i < argc; i++) {
1818
    c = getchar ();
1819
    if (c == EOF) {
1820
      return -1;
1821
    }
1822

    
1823
    arg_list[i] = toupper (c);
1824
  }
1825

    
1826
  // done
1827
  return argc;
1828
}
1829

    
1830

    
1831
void raw_print (uint8_t val)
1832
{
1833
  if (val == 255) {
1834
    putchar (254);              // avoid confusion
1835
  }
1836
  else {
1837
    putchar (val);
1838
  }
1839
}