Add camera snapshot of blackline
[brmpuk.git] / host / camnav / brmbotimg.c
blob4f67459a6a9ff4acc77ace63ca70bc7f3fe42928
1 /*
2 * brmbotimg.c
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
12 #include <math.h>
13 #include <stdbool.h>
14 #include <stdio.h>
15 #include <stdlib.h>
16 #include <sys/time.h>
17 #include <time.h>
18 #include <unistd.h>
19 #include <linux/videodev2.h>
22 #define SPEED 17
23 #define FRAMEDROP 3 // only each FRAMEDROP-th frame is examined
25 #define FOLLOW_THRES 96
26 #define SIDE_THRES 96
28 #define CAM_Y_OFS 25
30 FILE *debugout = NULL;
33 struct dpix {
34 unsigned char y1, u, y2, v;
35 //char u, y1, v, y2;
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;
49 int j, i;
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
64 int j, i;
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];
68 pixel->u = u;
69 pixel->v = v;
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)
103 #if 0
104 static FILE *f;
105 if (!f) f = fopen("stream.raw", "wb");
106 fwrite(p,size,1,f);
107 #endif
109 struct timeval tv;
110 gettimeofday(&tv, NULL);
111 static int framei;
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);
116 framei++;
118 if (framei%FRAMEDROP)
119 return;
121 int m1, m2;
122 int dir = 0;
123 static int ldir = 0;
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);
132 if (f) {
133 /* Happy! */
134 m1 = SPEED;
135 m2 = SPEED;
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
138 * sharp turn. */
139 if (left ^ right)
140 ldir = left ? -1 : 1;
141 goto next;
144 if (left ^ right) {
145 dir = left ? -1 : 1;
146 ldir = dir;
147 } else {
148 if (ldir)
149 dir = ldir;
150 else
151 dir = ldir = 1 - 2 * (random() % 2);
154 m1 = dir > 0 ? SPEED : 0;
155 m2 = dir < 0 ? SPEED : 0;
157 next:;
158 static int lm1, lm2;
159 if (m1 != lm1 || m2 != lm2) {
160 printf("%d %d\n", m1, m2);
161 lm1 = m1;
162 lm2 = 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);
172 #if 0
173 int main(void)
175 struct v4l2_format fmt = { .fmt = { .pix = { .width = 640, .height = 480, .bytesperline = 1280 } } };
176 char frame[614400];
177 while (!feof(stdin)) {
178 int i = fread(frame, 614400, 1, stdin);
179 if (i < 1) break;
180 process_image(frame, 614400, &fmt);
182 return 0;
184 #endif