cyclabile.c

   1 /*
   2  * cyclabile - CYC Light Augmented Bike Light Emitter
   3  *
   4  * Copyright (C) 2018  Antonio Ospite <ao2@ao2.it>
   5  *
   6  * This program is free software: you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation, either version 3 of the License, or
   9  * (at your option) any later version.
  10  *
  11  * This program is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  * GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public License
  17  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  18  */
  19
  20 #include <stdio.h>
  21 #include <sys/types.h>
  22 #include <sys/stat.h>
  23 #include <fcntl.h>
  24 #include <unistd.h>
  25 #include <errno.h>
  26 #include <signal.h>
  27 #include <linux/input.h>
  28 #include <leptonica/allheaders.h>
  29 #include <turbojpeg.h>
  30 #include <am7xxx.h>
  31
  32 #include "projective_split.h"
  33
  34 #include "fps-meter.h"
  35
  36 #define M_PI            3.14159265358979323846  /* pi */
  37
  38 /* Rotary encoder properties. */
  39 #define PULSES_PER_REVOLUTION 120
  40
  41 /* Video projector property. */
  42 #define HEIGHT_IN_PIXELS 480
  43
  44 /* Bike setup properties */
  45 #define WHEEL_RADIUS_IN_METERS 0.33
  46 #define HEIGHT_IN_METERS 0.92
  47
  48 #define PIXELS_PER_METER (HEIGHT_IN_PIXELS / HEIGHT_IN_METERS)
  49
  50
  51 static unsigned int run = 1;
  52
  53 static struct {
  54         char *image_file;
  55         char *event_device;
  56         unsigned int width;
  57         unsigned int height;
  58         unsigned int x_correction;
  59         unsigned int y_correction;
  60         unsigned int zoom_mode;
  61         unsigned int power_mode;
  62 } config = {
  63         .image_file = NULL,
  64         .event_device = NULL,
  65         .width = 800,
  66         .height = 480,
  67         .x_correction = 200,
  68         .y_correction = 0,
  69         .zoom_mode = AM7XXX_ZOOM_ORIGINAL,
  70         .power_mode = AM7XXX_POWER_LOW,
  71 };
  72
  73 #ifdef USE_EQEP
  74 #define POSITION_FILE "/sys/devices/platform/ocp/48304000.epwmss/48304180.eqep/position"
  75 static int read_position(int *position)
  76 {
  77         FILE *file;
  78         int ret = 0;
  79
  80         file = fopen(POSITION_FILE, "r");
  81         if (file == NULL) {
  82                 fprintf(stderr, "Cannot open %s\n", POSITION_FILE);
  83                 return -errno;
  84         }
  85
  86         ret = fscanf(file, "%d", position);
  87         if (ret != 1)
  88                 ret = -EINVAL;
  89         else
  90                 ret = 0;
  91
  92         fclose(file);
  93         return ret;
  94 }
  95 #else
  96 static int read_position(int *position)
  97 {
  98         (void)position;
  99         return 0;
 100 }
 101 #endif
 102
 103 static int mod(int a, int b)
 104 {
 105         int c = a % b;
 106         return (c < 0) ? c + b : c;
 107 }
 108
 109 static int projector_device_init(am7xxx_context **ctx, am7xxx_device **dev, unsigned int device_index)
 110 {
 111         int log_level = AM7XXX_LOG_INFO;
 112         int ret;
 113
 114         ret = am7xxx_init(ctx);
 115         if (ret < 0) {
 116                 perror("am7xxx_init");
 117                 goto err;
 118         }
 119
 120         am7xxx_set_log_level(*ctx, log_level);
 121
 122         ret = am7xxx_open_device(*ctx, dev, device_index);
 123         if (ret < 0) {
 124                 perror("am7xxx_open_device");
 125                 goto err;
 126         }
 127
 128         ret = am7xxx_set_zoom_mode(*dev, config.zoom_mode);
 129         if (ret < 0) {
 130                 perror("am7xxx_set_zoom_mode");
 131                 goto err_close_device;
 132         }
 133
 134         ret = am7xxx_set_power_mode(*dev, config.power_mode);
 135         if (ret < 0) {
 136                 perror("am7xxx_set_power_mode");
 137                 goto err_close_device;
 138         }
 139
 140         return 0;
 141
 142 err_close_device:
 143         am7xxx_close_device(*dev);
 144 err:
 145         am7xxx_shutdown(*ctx);
 146         return ret;
 147 }
 148
 149 static void unset_run(int signo)
 150 {
 151         (void) signo;
 152         run = 0;
 153 }
 154
 155 static int set_signal_handler(int signum, void (*signal_handler)(int))
 156 {
 157         struct sigaction new_action;
 158         struct sigaction old_action;
 159         int ret;
 160
 161         new_action.sa_handler = signal_handler;
 162         sigemptyset(&new_action.sa_mask);
 163         new_action.sa_flags = 0;
 164
 165         ret = sigaction(signum, NULL, &old_action);
 166         if (ret < 0) {
 167                 perror("sigaction on old_action");
 168                 goto out;
 169         }
 170
 171         if (old_action.sa_handler != SIG_IGN) {
 172                 ret = sigaction(signum, &new_action, NULL);
 173                 if (ret < 0) {
 174                         perror("sigaction on new_action");
 175                         goto out;
 176                 }
 177         }
 178
 179 out:
 180         return ret;
 181 }
 182
 183 static struct p *calc_perspective_map(void)
 184 {
 185         PTA *src;
 186         PTA *dst;
 187         BOX *viewport;
 188         l_float32 *transform_coeffs;
 189         struct p *perspective_map = NULL;
 190         int ret;
 191
 192         src = ptaCreate(4);
 193         if (src == NULL)
 194                 return NULL;
 195
 196         ptaAddPt(src, 0, 0);
 197         ptaAddPt(src, config.width, 0);
 198         ptaAddPt(src, config.width, config.height);
 199         ptaAddPt(src, 0, config.height);
 200
 201         dst = ptaCreate(4);
 202         if (dst == NULL)
 203                 goto out_destroy_pta_src;
 204
 205         ptaAddPt(dst, config.x_correction,                config.y_correction);
 206         ptaAddPt(dst, config.width - config.x_correction, config.y_correction);
 207         ptaAddPt(dst, config.width,                       config.height);
 208         ptaAddPt(dst, 0,                                  config.height);
 209
 210         transform_coeffs = NULL;
 211         ret = getProjectiveXformCoeffs(dst, src, &transform_coeffs);
 212         if (ret != 0)
 213                 goto out_destroy_pta_dst;
 214
 215         viewport = boxCreateValid(0, 0, config.width, config.height);
 216         if (viewport == NULL)
 217                 goto out_free_transform_coeffs;
 218
 219         perspective_map = _pixProjectiveSampled_precalc_map(viewport, transform_coeffs);
 220
 221         boxDestroy(&viewport);
 222
 223 out_free_transform_coeffs:
 224         LEPT_FREE(transform_coeffs);
 225 out_destroy_pta_dst:
 226         ptaDestroy(&dst);
 227 out_destroy_pta_src:
 228         ptaDestroy(&src);
 229
 230         return perspective_map;
 231 }
 232
 233 static void usage(char *name)
 234 {
 235         printf("usage: %s [OPTIONS]\n\n", name);
 236         printf("OPTIONS:\n");
 237         printf("\t-f <image_file>\t\tthe image file to upload\n");
 238         printf("\t-e <event device>\t\tthe event device to get input from\n");
 239         printf("\t-x <X correction>\t\tthe perspective correction in the X direction, in pixels (default 200)\n");
 240         printf("\t-y <Y correction>\t\tthe perspective correction in the Y direction, in pixels (default 50)\n");
 241         printf("\t-Z <zoom mode>\t\tthe display zoom mode, between %d (original) and %d (test)\n",
 242                AM7XXX_ZOOM_ORIGINAL, AM7XXX_ZOOM_TEST);
 243         printf("\t-h \t\t\tthis help message\n");
 244         printf("\n\nEXAMPLES OF USE:\n");
 245         printf("\t%s -f road.png -e /dev/input/event5\n", name);
 246         printf("\t%s -f narrow-road.png -e /dev/input/event0 -x 130 -Z 1\n", name);
 247 }
 248
 249 int main(int argc, char *argv[])
 250 {
 251         PIX *image;
 252         PIX *transformed_image;
 253         BOX *viewport;
 254         struct p *perspective_map;
 255
 256         tjhandle jpeg_compressor;
 257         unsigned char *out_buf;
 258         unsigned long out_buf_size;
 259
 260         am7xxx_context *projector_ctx = NULL;
 261         am7xxx_device *projector_dev = NULL;
 262
 263         int input_fd;
 264         struct input_event ev[64];
 265         unsigned int i;
 266
 267         int position = 0;
 268         int old_position = 0;
 269         int position_delta;
 270         float distance;
 271
 272         struct fps_meter_stats fps_stats;
 273
 274         int y = 0;
 275         int lastframe_limit;
 276
 277         int ret;
 278         int opt;
 279
 280         while ((opt = getopt(argc, argv, "e:f:x:y:P:Z:h")) != -1) {
 281                 switch (opt) {
 282                 case 'e':
 283                         if (config.event_device != NULL)
 284                                 fprintf(stderr, "Warning: event device already specified\n");
 285                         config.event_device = optarg;
 286                         break;
 287                 case 'f':
 288                         if (config.image_file != NULL)
 289                                 fprintf(stderr, "Warning: image file already specified\n");
 290                         config.image_file = optarg;
 291                         break;
 292                 case 'x':
 293                         config.x_correction = atoi(optarg); /* atoi() is as nasty as it is easy */
 294                         break;
 295                 case 'y':
 296                         config.y_correction = atoi(optarg); /* atoi() is as nasty as it is easy */
 297                         break;
 298                 case 'Z':
 299                         config.zoom_mode = atoi(optarg); /* atoi() is as nasty as it is easy */
 300                         switch(config.zoom_mode) {
 301                         case AM7XXX_ZOOM_ORIGINAL:
 302                         case AM7XXX_ZOOM_H:
 303                         case AM7XXX_ZOOM_H_V:
 304                         case AM7XXX_ZOOM_TEST:
 305                                 break;
 306                         default:
 307                                 fprintf(stderr, "Invalid zoom mode value, must be between %d and %d\n",
 308                                         AM7XXX_ZOOM_ORIGINAL, AM7XXX_ZOOM_TEST);
 309                                 ret = -EINVAL;
 310                                 goto out;
 311                         }
 312                         break;
 313                 case 'P':
 314                         config.power_mode = atoi(optarg); /* atoi() is as nasty as it is easy */
 315                         switch(config.power_mode) {
 316                         case AM7XXX_POWER_OFF:
 317                         case AM7XXX_POWER_LOW:
 318                         case AM7XXX_POWER_MIDDLE:
 319                         case AM7XXX_POWER_HIGH:
 320                         case AM7XXX_POWER_TURBO:
 321                                 break;
 322                         default:
 323                                 fprintf(stderr, "Invalid power mode value, must be between %d and %d\n",
 324                                         AM7XXX_POWER_OFF, AM7XXX_POWER_TURBO);
 325                                 ret = -EINVAL;
 326                                 goto out;
 327                         }
 328                         break;
 329                 case 'h':
 330                         usage(argv[0]);
 331                         ret = 0;
 332                         goto out;
 333                 default: /* '?' */
 334                         usage(argv[0]);
 335                         ret = -EINVAL;
 336                         goto out;
 337                 }
 338         }
 339
 340         if (config.image_file == NULL) {
 341                 fprintf(stderr, "An image file MUST be specified with the -f option.\n\n");
 342                 usage(argv[0]);
 343                 ret = -EINVAL;
 344                 goto out;
 345         }
 346
 347         if (config.event_device == NULL) {
 348                 fprintf(stderr, "An event device MUST be specified with the -e option.\n\n");
 349                 usage(argv[0]);
 350                 ret = -EINVAL;
 351                 goto out;
 352         }
 353
 354         ret = set_signal_handler(SIGINT, unset_run);
 355         if (ret < 0) {
 356                 perror("setting SIGINT");
 357                 goto out;
 358         }
 359
 360         ret = set_signal_handler(SIGTERM, unset_run);
 361         if (ret < 0) {
 362                 perror("setting SIGTERM");
 363                 goto out;
 364         }
 365
 366         image = pixRead(config.image_file);
 367         if (image == NULL) {
 368                 fprintf(stderr, "Cannot load image\n");
 369                 ret = -EINVAL;
 370                 goto out;
 371         }
 372         lastframe_limit = (pixGetHeight(image) - config.height);
 373
 374         config.width = pixGetWidth(image);
 375
 376         transformed_image = pixCreate(config.width, config.height, pixGetDepth(image));
 377         if (transformed_image == NULL) {
 378                 fprintf(stderr, "Cannot create image\n");
 379                 ret = -EINVAL;
 380                 goto out_destroy_image;
 381         }
 382
 383         /* Transform only a part of the whole image */
 384         viewport = boxCreateValid(0, y, config.width, config.height);
 385         if (viewport == NULL) {
 386                 ret = -EINVAL;
 387                 goto out_destroy_transformed_image;
 388         }
 389
 390         perspective_map = calc_perspective_map();
 391         if (perspective_map == NULL) {
 392                 ret = -ENOMEM;
 393                 goto out_destroy_viewport;
 394         }
 395
 396         /* jpeg encoder init */
 397         jpeg_compressor = tjInitCompress();
 398         if (jpeg_compressor == NULL) {
 399                 fprintf(stderr, "tjInitCompress failed: %s\n", tjGetErrorStr());
 400                 ret = -ENOMEM;
 401                 goto out_free_perspective_map;
 402         }
 403
 404         out_buf_size = tjBufSize(config.width, config.height, TJSAMP_420);
 405         ret = (int) out_buf_size;
 406         if (ret < 0) {
 407                 fprintf(stderr, "tjBufSize failed: %s\n", tjGetErrorStr());
 408                 goto out_jpeg_destroy;
 409         }
 410         out_buf = malloc(out_buf_size);
 411         if (out_buf == NULL) {
 412                 ret = -ENOMEM;
 413                 goto out_jpeg_destroy;
 414         }
 415
 416         /* output init */
 417         ret = projector_device_init(&projector_ctx, &projector_dev, 0);
 418         if (ret < 0)
 419                 goto out_free_out_buf;
 420
 421         /* input event init */
 422         input_fd = open(config.event_device, O_RDONLY | O_NONBLOCK);
 423         if (input_fd < 0) {
 424                 perror("open");
 425                 ret = input_fd;
 426                 goto out_am7xxx_shutdown;
 427         }
 428
 429         ret = read_position(&old_position);
 430         if (ret < 0)
 431                 goto out_cleanup;
 432
 433
 434         fps_meter_init(&fps_stats);
 435         while (run) {
 436
 437                 fps_meter_update(&fps_stats);
 438
 439                 errno = 0;
 440                 ret = read(input_fd, ev, sizeof(ev));
 441                 if (ret < (int)sizeof(ev[0]) && errno != EAGAIN && errno != EWOULDBLOCK) {
 442                         perror("read");
 443                         ret = -errno;
 444                         goto out_cleanup;
 445                 }
 446
 447                 /*
 448                  * Handle keypad input to adjust the perspective map.
 449                  * This can go in a preliminar "setup" stage instead of the
 450                  * main loop.
 451                  */
 452                 if (ret >= (int)sizeof(ev[0])) {
 453                         for (i = 0; i < ret / sizeof(ev[0]); i++) {
 454                                 /*
 455                                  * This is for testing the software with the
 456                                  * mouse wheel...
 457                                  */
 458                                 if (ev[i].type == EV_REL && ev[i].code == REL_WHEEL) {
 459                                         position += ev[i].value;
 460                                 }
 461                                 else if (ev[i].type == EV_KEY && ev[i].value > 0) {
 462                                         switch(ev[i].code) {
 463                                         case KEY_UP:
 464                                                 if (config.y_correction < config.height)
 465                                                         config.y_correction += 1;
 466                                                 break;
 467                                         case KEY_DOWN:
 468                                                 if (config.y_correction > 0)
 469                                                         config.y_correction -= 1;
 470                                                 break;
 471                                         case KEY_LEFT:
 472                                                 if (config.x_correction > 0)
 473                                                         config.x_correction -= 1;
 474                                                 break;
 475                                         case KEY_RIGHT:
 476                                                 if (config.x_correction < config.width)
 477                                                         config.x_correction += 1;
 478                                                 break;
 479                                         default:
 480                                                 break;
 481
 482                                         }
 483                                         free(perspective_map);
 484                                         perspective_map = calc_perspective_map();
 485                                         if (perspective_map == NULL) {
 486                                                 ret = -ENOMEM;
 487                                                 goto out_cleanup;
 488                                         }
 489                                         pixClearAll(transformed_image);
 490                                         break;
 491                                 }
 492                         }
 493                 }
 494
 495                 ret = read_position(&position);
 496                 if (ret < 0)
 497                         break;
 498                 position_delta = old_position - position;
 499                 old_position = position;
 500
 501                 /* distance in meters */
 502                 distance = 2 * WHEEL_RADIUS_IN_METERS * M_PI / PULSES_PER_REVOLUTION * position_delta;
 503 #ifdef DEBUG
 504                 fprintf(stderr, "position_delta: %d distance: %f pixels: %d\n", position_delta, distance, (int) (distance * PIXELS_PER_METER));
 505 #endif
 506
 507                 /* convert distance to pixels */
 508                 y += distance * PIXELS_PER_METER;
 509                 y = mod(y, lastframe_limit);
 510
 511                 boxSetGeometry(viewport, -1, y, -1, -1);
 512
 513                 /* Apply the perspective transformation */
 514                 transformed_image = _pixProjectiveSampled_apply_map_dest_roi(image,
 515                                                                    perspective_map,
 516                                                                    L_BRING_IN_BLACK,
 517                                                                    transformed_image,
 518                                                                    viewport);
 519                 ret = tjCompress2(jpeg_compressor, (unsigned char *)pixGetData(transformed_image),
 520                                   config.width, 0, config.height, TJPF_XBGR, &out_buf, &out_buf_size,
 521                                   TJSAMP_420, 70, TJFLAG_NOREALLOC | TJFLAG_FASTDCT);
 522                 if (ret < 0) {
 523                         tjGetErrorStr();
 524                         fprintf(stderr, "tjCompress2 failed: %s\n", tjGetErrorStr());
 525                         break;
 526                 }
 527
 528                 ret = am7xxx_send_image_async(projector_dev,
 529                                         AM7XXX_IMAGE_FORMAT_JPEG,
 530                                         config.width,
 531                                         config.height,
 532                                         out_buf,
 533                                         out_buf_size);
 534
 535                 if (ret < 0) {
 536                         perror("am7xxx_send_image");
 537                         break;
 538                 }
 539
 540         }
 541
 542         ret = 0;
 543
 544 out_cleanup:
 545         close(input_fd);
 546 out_am7xxx_shutdown:
 547         am7xxx_set_zoom_mode(projector_dev, AM7XXX_ZOOM_TEST);
 548         am7xxx_set_power_mode(projector_dev, AM7XXX_POWER_OFF);
 549         am7xxx_close_device(projector_dev);
 550         am7xxx_shutdown(projector_ctx);
 551 out_free_out_buf:
 552         free(out_buf);
 553 out_jpeg_destroy:
 554         tjDestroy(jpeg_compressor);
 555 out_free_perspective_map:
 556         free(perspective_map);
 557 out_destroy_viewport:
 558         boxDestroy(&viewport);
 559 out_destroy_transformed_image:
 560         pixDestroy(&transformed_image);
 561 out_destroy_image:
 562         pixDestroy(&image);
 563 out:
 564         return ret;
 565 }