SHARE
TWEET

raspberry camera sync

a guest Apr 19th, 2016 369 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /***
  2.  *
  3.  * This example is just a proof of a concept 6by9 gave in the raspi forum:
  4.  * that one could synchronize the raspi cam to an external periodical event just
  5.  * fiddling with the framerates.
  6.  *
  7.  * So if you have an external periodical event like:
  8.  * t_event = start_time + k * period, for k being an integer
  9.  *
  10.  * what we want here is to have the frame taken always at the same controlled t_dist from t_event.
  11.  *
  12.  * One could sync many raspis together using PTP, for instance, and get stereo vision synced.
  13.  * Or sync the raspi to the internal (or external) PWM, like done here.
  14.  *
  15.  * This is not a solution for imaging at the exact external trigger event.
  16.  *
  17.  * and is just a hack. it is not like I am caring for mutexes, return status of mmal calls, polluting globals.
  18.  * It it just to show that the concept works.
  19.  *
  20.  *
  21. ***/
  22.  
  23. #include <iostream>
  24. #include <thread>
  25.  
  26. #include <time.h>
  27.  
  28. #include <wiringPi.h>
  29.  
  30. // if one is compiling this, will need to add a few include directories manually in the gcc comamnd.
  31. // at least this is needed when compiling for a buildroot generated sysroot.
  32. // since inside the include files there are many other includes without paths.
  33. #include "mmal/mmal.h"
  34. #include "mmal/util/mmal_util.h"
  35. #include "mmal/util/mmal_default_components.h"
  36. #include "mmal/util/mmal_connection.h"
  37. #include "mmal/util/mmal_util_params.h"
  38. #include "bcm_host.h"
  39.  
  40. #define WIDTH 1296
  41. #define HEIGHT 972
  42.  
  43. #define FRAME_RATE_NUM_DEFAULT (40*256)
  44. #define FRAME_RATE_DEN_DEFAULT 256
  45. #define T_DEAD_ZONE 10
  46. #define FR_CORRECTION 32
  47.  
  48.  
  49. #define MMAL_CAMERA_PREVIEW_PORT 0
  50. #define MMAL_CAMERA_VIDEO_PORT 1
  51. #define MMAL_CAMERA_CAPTURE_PORT 2
  52.  
  53.  
  54. typedef struct {
  55.     MMAL_POOL_T *camera_video_port_pool;
  56. } PORT_USERDATA;
  57.  
  58. static PORT_USERDATA userdata;
  59. static MMAL_PORT_T * camera_video_port = nullptr;
  60.  
  61. uint64_t pwm_counter;
  62. uint64_t pwm_start_time;
  63. double pwm_period;
  64. uint64_t pwm_offset;
  65.  
  66. static void video_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
  67. {
  68.     static uint64_t frame_count = 0;
  69.     static struct timespec ts;
  70.  
  71.     frame_count++;
  72.  
  73.     // get the offset between the mmal time and linux time. We need this to correct the
  74.     // frames timestamps.
  75.  
  76.     uint64_t t_mmal = 0;
  77.     mmal_port_parameter_get_uint64(port, MMAL_PARAMETER_SYSTEM_TIME, &t_mmal);
  78.  
  79.     clock_gettime(CLOCK_MONOTONIC, &ts);
  80.     uint64_t t_linux = (uint64_t)ts.tv_sec*1000000 + ts.tv_nsec / 1000;
  81.  
  82.     int64_t t_offset = t_linux - t_mmal;
  83.  
  84.     uint64_t t_frame_mmal = buffer->pts; // this timestamp is referenced to the mmal clock.
  85.     uint64_t t_frame_linux = t_frame_mmal + t_offset; // now we have the frame timestamp referenced to the linux clock
  86.     // std::cout << "t_offset =" << t_offset << std::endl;
  87.     // to see mmal time and linux time drifting apart, uncomment above line
  88.  
  89.  
  90.     // we have a periodic event we want to sync to.
  91.     // This event is modeled like:
  92.     // t_event(k) = t_event_start + k * t_period, where k is integer.
  93.     // we wanto to keep t_frame_linux, above, locked at a fixed given distance from t_event.
  94.     // like:
  95.     // t_frame_linux = t_event_start + k * t_period + t_dist
  96.     //
  97.     // of course the locking is nor perfect, so we can define:
  98.     // t_error = t_frame_linux - (t_event_start + k * t_period + t_dist)
  99.     //
  100.     // and we should drive the software PLL to zero t_error.
  101.  
  102.     // we now should find t_error. Begin first by finding a suitable k.
  103.  
  104.     uint64_t t_event_start = pwm_start_time; // here will sync to pwm
  105.     uint64_t t_dist = pwm_offset;
  106.     double t_period = pwm_period*1e6; // t_period should be in microsec
  107.     uint32_t t_period_us = t_period;
  108.  
  109.     uint32_t k = (t_frame_linux - t_event_start - t_dist)/t_period;
  110.  
  111.     uint32_t t_error = t_frame_linux - (t_event_start + (uint64_t)(k * t_period) + t_dist);
  112.     // by contruction, t_error > 0
  113.  
  114.     // we should drive it to:
  115.     // 0, if t_error < t_period_us / 2;
  116.     // t_period_us, if t_error > t_period_us/2
  117.  
  118.     // to decrease t_error, we should decrease t_frame_linux by increasing the fps
  119.     // to increase t_error, descrease fps
  120.     // and near a dead zone, close to the targets, we could set a neutral fps
  121.  
  122.     //std::cout << "cnt= " << frame_count << " t_error=" << t_error << " t_period=" << t_period_us << " k=" << k << std::endl;
  123.  
  124.  
  125.     if (t_error > T_DEAD_ZONE && t_error < t_period_us/2)
  126.     {
  127.         MMAL_PARAMETER_FRAME_RATE_T param = {{MMAL_PARAMETER_VIDEO_FRAME_RATE, sizeof(param)}, {0, 0}};
  128.         param.frame_rate.num = FRAME_RATE_NUM_DEFAULT + FR_CORRECTION;
  129.         param.frame_rate.den = FRAME_RATE_DEN_DEFAULT;
  130.         mmal_port_parameter_set(camera_video_port, &param.hdr);
  131.     }
  132.     else if (t_error < t_period_us - T_DEAD_ZONE && t_error > t_period_us/2)
  133.     {
  134.         MMAL_PARAMETER_FRAME_RATE_T param = {{MMAL_PARAMETER_VIDEO_FRAME_RATE, sizeof(param)}, {0, 0}};
  135.         param.frame_rate.num = FRAME_RATE_NUM_DEFAULT - FR_CORRECTION;
  136.         param.frame_rate.den = FRAME_RATE_DEN_DEFAULT;
  137.         mmal_port_parameter_set(camera_video_port, &param.hdr);
  138.     }
  139.     else
  140.     {
  141.         MMAL_PARAMETER_FRAME_RATE_T param = {{MMAL_PARAMETER_VIDEO_FRAME_RATE, sizeof(param)}, {0, 0}};
  142.         param.frame_rate.num = FRAME_RATE_NUM_DEFAULT;
  143.         param.frame_rate.den = FRAME_RATE_DEN_DEFAULT;
  144.         mmal_port_parameter_set(camera_video_port, &param.hdr);
  145.     }
  146.  
  147.     MMAL_BUFFER_HEADER_T *new_buffer;
  148.     PORT_USERDATA * p_userdata = (PORT_USERDATA *) port->userdata;
  149.     MMAL_POOL_T *pool = p_userdata->camera_video_port_pool;
  150.  
  151.     mmal_buffer_header_mem_lock(buffer);
  152.  
  153.     // do something cool with data
  154.  
  155.     mmal_buffer_header_mem_unlock(buffer);
  156.     mmal_buffer_header_release(buffer);
  157.  
  158.     if (port->is_enabled)
  159.     {
  160.  
  161.         MMAL_STATUS_T status;
  162.         new_buffer = mmal_queue_get(pool->queue);
  163.  
  164.         if (new_buffer)
  165.         {
  166.             status = mmal_port_send_buffer(port, new_buffer);
  167.         }
  168.         if (!new_buffer || status != MMAL_SUCCESS)
  169.             printf("Unable to return a buffer to the video port\n");
  170.     }
  171. }
  172.  
  173.  
  174. int start_camera() {
  175.     MMAL_COMPONENT_T *camera = 0;
  176.     MMAL_COMPONENT_T *preview = 0;
  177.     MMAL_ES_FORMAT_T *format;
  178.     MMAL_STATUS_T status;
  179.     MMAL_PORT_T *camera_preview_port = NULL;
  180.     MMAL_PORT_T *camera_still_port = NULL;
  181.     MMAL_PORT_T *preview_input_port = NULL;
  182.     MMAL_POOL_T *camera_video_port_pool;
  183.     MMAL_CONNECTION_T *camera_preview_connection = 0;
  184.  
  185.     bcm_host_init();
  186.  
  187.     status = mmal_component_create(MMAL_COMPONENT_DEFAULT_CAMERA, &camera);
  188.     if (status != MMAL_SUCCESS) {
  189.         printf("Error: create camera %x\n", status);
  190.         return -1;
  191.     }
  192.  
  193.     camera_preview_port = camera->output[MMAL_CAMERA_PREVIEW_PORT];
  194.     camera_video_port = camera->output[MMAL_CAMERA_VIDEO_PORT];
  195.     camera_still_port = camera->output[MMAL_CAMERA_CAPTURE_PORT];
  196.  
  197.     {
  198.         MMAL_PARAMETER_CAMERA_CONFIG_T cam_config =
  199.         {
  200.             { MMAL_PARAMETER_CAMERA_CONFIG, sizeof (cam_config)},
  201.             .max_stills_w = WIDTH,
  202.             .max_stills_h = HEIGHT,
  203.             .stills_yuv422 = 0,
  204.             .one_shot_stills = 0,
  205.             .max_preview_video_w = WIDTH/2,
  206.             .max_preview_video_h = HEIGHT/2,
  207.             .num_preview_video_frames = 2,
  208.             .stills_capture_circular_buffer_height = 0,
  209.             .fast_preview_resume = 1,
  210.             .use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RAW_STC
  211.         };
  212.  
  213.         mmal_port_parameter_set(camera->control, &cam_config.hdr);
  214.     }
  215.  
  216.     format = camera_video_port->format;
  217.  
  218.     format->encoding = MMAL_ENCODING_I420;
  219.     format->encoding_variant = MMAL_ENCODING_I420;
  220.  
  221.     format->es->video.width = WIDTH/2;
  222.     format->es->video.height = HEIGHT/2;
  223.     format->es->video.crop.x = 0;
  224.     format->es->video.crop.y = 0;
  225.     format->es->video.crop.width = WIDTH/2;
  226.     format->es->video.crop.height = HEIGHT/2;
  227.     format->es->video.frame_rate.num = FRAME_RATE_NUM_DEFAULT;
  228.     format->es->video.frame_rate.den = FRAME_RATE_DEN_DEFAULT;
  229.  
  230.     camera_video_port->buffer_size = WIDTH * HEIGHT * 2;
  231.     camera_video_port->buffer_num = 5;
  232.     printf("  Camera video buffer_size = %d\n", camera_video_port->buffer_size);
  233.  
  234.     status = mmal_port_format_commit(camera_video_port);
  235.     printf("  Camera video recommended buffer_size = %d, %d\n", camera_video_port->buffer_size_recommended, camera_video_port->buffer_num_recommended);
  236.  
  237.     if (status != MMAL_SUCCESS) {
  238.         printf("Error: unable to commit camera video port format (%u)\n", status);
  239.         return -1;
  240.     }
  241.  
  242.     format = camera_preview_port->format;
  243.  
  244.     format->encoding = MMAL_ENCODING_OPAQUE;
  245.     format->encoding_variant = MMAL_ENCODING_I420;
  246.  
  247.     format->es->video.width = WIDTH/2;
  248.     format->es->video.height = HEIGHT/2;
  249.     format->es->video.crop.x = 0;
  250.     format->es->video.crop.y = 0;
  251.     format->es->video.crop.width = WIDTH/2;
  252.     format->es->video.crop.height = HEIGHT/2;
  253.  
  254.     status = mmal_port_format_commit(camera_preview_port);
  255.  
  256.     if (status != MMAL_SUCCESS) {
  257.         printf("Error: camera viewfinder format couldn't be set\n");
  258.         return -1;
  259.     }
  260.  
  261.     // create pool form camera video port
  262.     camera_video_port_pool = (MMAL_POOL_T *) mmal_port_pool_create(camera_video_port, camera_video_port->buffer_num, camera_video_port->buffer_size);
  263.  
  264.     std::cout << " num buffer headers: " <<  camera_video_port_pool->headers_num << std::endl;
  265.  
  266.     userdata.camera_video_port_pool = camera_video_port_pool;
  267.  
  268.     camera_video_port->userdata = (struct MMAL_PORT_USERDATA_T *) &userdata;
  269.  
  270.     status = mmal_port_enable(camera_video_port, video_buffer_callback);
  271.     if (status != MMAL_SUCCESS) {
  272.         printf("Error: unable to enable camera video port (%u)\n", status);
  273.         return -1;
  274.     }
  275.  
  276.  
  277.     status = mmal_component_enable(camera);
  278.  
  279.     status = mmal_component_create(MMAL_COMPONENT_DEFAULT_VIDEO_RENDERER, &preview);
  280.     if (status != MMAL_SUCCESS) {
  281.         printf("Error: unable to create preview (%u)\n", status);
  282.         return -1;
  283.     }
  284.     preview_input_port = preview->input[0];
  285.  
  286.     {
  287.         MMAL_DISPLAYREGION_T param;
  288.         param.hdr.id = MMAL_PARAMETER_DISPLAYREGION;
  289.         param.hdr.size = sizeof (MMAL_DISPLAYREGION_T);
  290.         param.set = MMAL_DISPLAY_SET_LAYER;
  291.         param.layer = 0;
  292.         param.set |= MMAL_DISPLAY_SET_FULLSCREEN;
  293.         param.fullscreen = 1;
  294.         status = mmal_port_parameter_set(preview_input_port, &param.hdr);
  295.         if (status != MMAL_SUCCESS && status != MMAL_ENOSYS) {
  296.             printf("Error: unable to set preview port parameters (%u)\n", status);
  297.             return -1;
  298.         }
  299.     }
  300.  
  301.     status = mmal_connection_create(&camera_preview_connection, camera_preview_port, preview_input_port, MMAL_CONNECTION_FLAG_TUNNELLING | MMAL_CONNECTION_FLAG_ALLOCATION_ON_INPUT);
  302.     if (status != MMAL_SUCCESS) {
  303.         printf("Error: unable to create connection (%u)\n", status);
  304.         return -1;
  305.     }
  306.  
  307.     status = mmal_connection_enable(camera_preview_connection);
  308.     if (status != MMAL_SUCCESS) {
  309.         printf("Error: unable to enable connection (%u)\n", status);
  310.         return -1;
  311.     }
  312.  
  313.     if (1) {
  314.         // Send all the buffers to the camera video port
  315.         int num = mmal_queue_length(camera_video_port_pool->queue);
  316.         int q;
  317.  
  318.         for (q = 0; q < num; q++) {
  319.             MMAL_BUFFER_HEADER_T *buffer = mmal_queue_get(camera_video_port_pool->queue);
  320.  
  321.             if (!buffer) {
  322.                 printf("Unable to get a required buffer %d from pool queue\n", q);
  323.             }
  324.  
  325.             if (mmal_port_send_buffer(camera_video_port, buffer) != MMAL_SUCCESS) {
  326.                 printf("Unable to send a buffer to encoder output port (%d)\n", q);
  327.             }
  328.         }
  329.     }
  330.  
  331.     if (mmal_port_parameter_set_boolean(camera_video_port, MMAL_PARAMETER_CAPTURE, 1) != MMAL_SUCCESS) {
  332.         printf("%s: Failed to start capture\n", __func__);
  333.     }
  334.  
  335.     if (mmal_port_parameter_set_int32(camera->control, MMAL_PARAMETER_EXPOSURE_COMP , -24) != MMAL_SUCCESS) {
  336.         printf("Failed to set exp comp\n");
  337.     }
  338.     if (mmal_port_parameter_set_uint32(camera->control, MMAL_PARAMETER_SHUTTER_SPEED, 23800)!= MMAL_SUCCESS) {
  339.         printf("Failed to set exp comp\n");
  340.     }
  341.  
  342.  
  343.     {
  344.        MMAL_PARAMETER_AWBMODE_T param = {{MMAL_PARAMETER_AWB_MODE,sizeof(param)}, MMAL_PARAM_AWBMODE_OFF};
  345.  
  346.        int st = mmal_port_parameter_set(camera->control, &param.hdr);
  347.        printf("awb_mode=%d\n", st);
  348.     }
  349.     {
  350.  
  351.         MMAL_PARAMETER_AWB_GAINS_T param = {{MMAL_PARAMETER_CUSTOM_AWB_GAINS, sizeof(param)}, {0, 0}, {0, 0}};
  352.  
  353.        param.r_gain.num = 65536;
  354.        param.b_gain.num = 65536;
  355.        param.r_gain.den = param.b_gain.den = 65536;
  356.        int st = mmal_port_parameter_set(camera->control, &param.hdr);
  357.        printf("awbg=%d\n", st);
  358.     }
  359.  
  360.     return 0;
  361.  
  362. }
  363.  
  364. int main()
  365. {
  366.     if (wiringPiSetup () == -1)
  367.         exit (1) ;
  368.  
  369.     //wiringPiISR (1, INT_EDGE_RISING, &pwm_int);
  370.  
  371.     pinMode (1, PWM_OUTPUT) ;
  372.     pwmSetMode  (PWM_MODE_MS);
  373.     int sclock = 19;
  374.     uint32_t range = 19.2e6/sclock/40 - 19;
  375.     pwmSetRange (range);
  376.     pwmWrite (1, 3);
  377.     struct timespec t1, t2;
  378.     uint64_t tdiff = 0;
  379.     int i = 0;
  380.     do
  381.     {
  382.         clock_gettime(CLOCK_MONOTONIC, &t1);
  383.         pwmSetClock (19);
  384.         clock_gettime(CLOCK_MONOTONIC, &t2);
  385.         tdiff = (uint64_t)t2.tv_sec*1000000 + t2.tv_nsec / 1000 - (uint64_t)t1.tv_sec*1000000 - t1.tv_nsec / 1000;
  386.         ++i;
  387.     } while (tdiff > 200);
  388.     std::cout << "tdiff=" << tdiff << "  i=" << i << "\n";
  389.  
  390.     pwm_start_time = ((uint64_t)t1.tv_sec)*1000000 + t1.tv_nsec / 1000;
  391.     pwm_period = range*sclock/19.2e6;
  392.  
  393.     std::this_thread::sleep_for(std::chrono::microseconds(10000));
  394.     std::thread mmal_thread (start_camera);
  395.  
  396.  
  397.     uint64_t offset;
  398.     while (true)
  399.     {
  400.         std::cout << "enter offset [┬Ás]:";
  401.         std::cin >> pwm_offset;
  402.         std::cout << "o=" << offset << std::endl;
  403.     }
  404.     return 0;
  405. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top