4 * Camera image processing and filtering.
6 * gcc -Wall -O3 -ggdb3 -std=gnu99 -o brmbotimg brmbotimg.c apc.c -lm
8 * To playback raw streams, use:
10 * mplayer -demuxer rawvideo -noframedrop -rawvideo fps=1:w=640:h=480:yuy2 out.raw
19 #include <linux/videodev2.h>
23 #define FRAMEDROP 3 // only each FRAMEDROP-th frame is examined
25 #define FOLLOW_THRES 96
30 FILE *debugout
= NULL
;
34 unsigned char y1
, u
, y2
, v
;
36 } __attribute__((packed
));
38 /* Near zero: likely black. Higher: likely not black. */
39 int assess_pixel(const struct dpix
*pixel
)
41 return (pixel
->y1
+ pixel
->y2
) / 2;
44 /* w,h is square radius, not diameter! */
45 int assess_square(const struct dpix
*image
, struct v4l2_format
*fmt
, int x
, int y
, int w
, int h
)
47 x
/= 2; w
/= 2; // each dpix is two physical pixels
48 int assess_min
= 255, assess_max
= 0;
50 for (j
= y
- h
; j
< y
+ h
; j
++)
51 for (i
= x
- w
; i
< x
+ w
; i
++) {
52 const struct dpix
*pixel
= &image
[j
* fmt
->fmt
.pix
.width
/ 2 + i
];
53 //fprintf(stderr, "%d - %d,%d: Y %d, U %d, V %d\n", assess_pixel(pixel), i*2, j, (pixel->y1 + pixel->y2) / 2, pixel->u, pixel->v);
54 int a
= assess_pixel(pixel
);
55 if (a
< assess_min
) assess_min
= a
;
56 else if (a
> assess_max
) assess_max
= a
;
58 return assess_max
- assess_min
;
61 void color_square(struct dpix
*image
, struct v4l2_format
*fmt
, int x
, int y
, int w
, int h
, int u
, int v
)
63 x
/= 2; w
/= 2; // each dpix is two physical pixels
65 for (j
= y
- h
; j
< y
+ h
; j
++)
66 for (i
= x
- w
; i
< x
+ w
; i
++) {
67 struct dpix
*pixel
= &image
[j
* fmt
->fmt
.pix
.width
/ 2 + i
];
73 bool following(struct dpix
*image
, struct v4l2_format
*fmt
)
75 int x
= 230, y
= 240 + CAM_Y_OFS
, xr
= 15, yr
= 20;
76 double value
= assess_square(image
, fmt
, x
, y
, xr
, yr
);
77 color_square(image
, fmt
, x
, y
, xr
, yr
, 0, value
> FOLLOW_THRES
? 255 : 0);
78 fprintf(stderr
, "Following check: %d,%d %dx%d %f >? %d\n", x
, y
, xr
, yr
, value
, FOLLOW_THRES
);
79 return value
> FOLLOW_THRES
;
82 bool on_right(struct dpix
*image
, struct v4l2_format
*fmt
)
84 int x
= 230, y
= 200 + CAM_Y_OFS
, xr
= 15, yr
= 20;
85 double value
= assess_square(image
, fmt
, x
, y
, xr
, yr
);
86 color_square(image
, fmt
, x
, y
, xr
, yr
, 255, value
> SIDE_THRES
? 255 : 0);
87 fprintf(stderr
, "On-right check: %d,%d %dx%d %f >? %d\n", x
, y
, xr
, yr
, value
, SIDE_THRES
);
88 return value
> SIDE_THRES
;
91 bool on_left(struct dpix
*image
, struct v4l2_format
*fmt
)
93 int x
= 230, y
= 280 + CAM_Y_OFS
, xr
= 15, yr
= 20;
94 double value
= assess_square(image
, fmt
, x
, y
, xr
, yr
);
95 color_square(image
, fmt
, x
, y
, xr
, yr
, 255, value
> SIDE_THRES
? 255 : 0);
96 fprintf(stderr
, "On-left check: %d,%d %dx%d %f >? %d\n", x
, y
, xr
, yr
, value
, SIDE_THRES
);
97 return value
> SIDE_THRES
;
101 void process_image(void *p
, int size
/* 614400 */, struct v4l2_format
*fmt
)
105 if (!f
) f
= fopen("stream.raw", "wb");
110 gettimeofday(&tv
, NULL
);
112 fprintf(stderr
, "\n----------------------------------------------------------------------\n"
113 "\n[%d %ld,%ld] frame (size %d w %d h %d bpl %d)\n",
114 framei
, tv
.tv_sec
, tv
.tv_usec
,
115 size
, fmt
->fmt
.pix
.width
, fmt
->fmt
.pix
.height
, fmt
->fmt
.pix
.bytesperline
);
118 if (framei
%FRAMEDROP
)
125 /* Each two bytes are (Y, U, Y, V) representing two
126 * adjecent pixels. */
127 struct dpix
*image
= p
;
129 bool f
= following(image
, fmt
);
130 bool right
= on_right(image
, fmt
);
131 bool left
= on_left(image
, fmt
);
136 /* In case we lose the line, try to remember the last
137 * side area where we've seen it in case it was very
140 ldir
= left
? -1 : 1;
151 dir
= ldir
= 1 - 2 * (random() % 2);
154 m1
= dir
> 0 ? SPEED
: 0;
155 m2
= dir
< 0 ? SPEED
: 0;
159 if (m1
!= lm1
|| m2
!= lm2
) {
160 printf("%d %d\n", m1
, m2
);
165 fprintf(stderr
, "dir=%d => %d %d\n", dir
, m1
, m2
);
166 fflush(stdout
); fflush(stderr
);
167 if (!debugout
) debugout
= fopen("out.raw", "wb");
168 // comment out below for no debugging
169 fwrite((char*) p
, size
, 1, debugout
);
175 struct v4l2_format fmt
= { .fmt
= { .pix
= { .width
= 640, .height
= 480, .bytesperline
= 1280 } } };
177 while (!feof(stdin
)) {
178 int i
= fread(frame
, 614400, 1, stdin
);
180 process_image(frame
, 614400, &fmt
);