host/camnav/brmbotimg.c

   1 /*
   2  * brmbotimg.c
   3  *
   4  * Camera image processing and filtering.
   5  *
   6  * gcc -Wall -O3 -ggdb3 -std=gnu99 -o brmbotimg brmbotimg.c apc.c -lm
   7  *
   8  * To playback raw streams, use:
   9  *
  10  * mplayer -demuxer rawvideo -noframedrop -rawvideo fps=1:w=640:h=480:yuy2 out.raw
  11  */
  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>
  20
  21
  22 #define SPEED 17
  23 #define FRAMEDROP 3 // only each FRAMEDROP-th frame is examined
  24
  25 #define FOLLOW_THRES 96
  26 #define SIDE_THRES 96
  27
  28 #define CAM_Y_OFS 25
  29
  30 FILE *debugout = NULL;
  31
  32
  33 struct dpix {
  34         unsigned char y1, u, y2, v;
  35         //char u, y1, v, y2;
  36 } __attribute__((packed));
  37
  38 /* Near zero: likely black. Higher: likely not black. */
  39 int assess_pixel(const struct dpix *pixel)
  40 {
  41         return (pixel->y1 + pixel->y2) / 2;
  42 }
  43
  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)
  46 {
  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;
  57                 }
  58         return assess_max - assess_min;
  59 }
  60
  61 void color_square(struct dpix *image, struct v4l2_format *fmt, int x, int y, int w, int h, int u, int v)
  62 {
  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;
  70                 }
  71 }
  72
  73 bool following(struct dpix *image, struct v4l2_format *fmt)
  74 {
  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;
  80 }
  81
  82 bool on_right(struct dpix *image, struct v4l2_format *fmt)
  83 {
  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;
  89 }
  90
  91 bool on_left(struct dpix *image, struct v4l2_format *fmt)
  92 {
  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;
  98 }
  99
 100
 101 void process_image(void *p, int size /* 614400 */, struct v4l2_format *fmt)
 102 {
 103 #if 0
 104         static FILE *f;
 105         if (!f) f = fopen("stream.raw", "wb");
 106         fwrite(p,size,1,f);
 107 #endif
 108
 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++;
 117
 118         if (framei%FRAMEDROP)
 119                 return;
 120
 121         int m1, m2;
 122         int dir = 0;
 123         static int ldir = 0;
 124
 125         /* Each two bytes are (Y, U, Y, V) representing two
 126          * adjecent pixels. */
 127         struct dpix *image = p;
 128
 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;
 142         }
 143
 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);
 152         }
 153
 154         m1 = dir > 0 ? SPEED : 0;
 155         m2 = dir < 0 ? SPEED : 0;
 156
 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;
 163         }
 164
 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);
 170 }
 171
 172 #if 0
 173 int main(void)
 174 {
 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);
 181         }
 182         return 0;
 183 }
 184 #endif