SHARE
TWEET

rasp

a guest Apr 20th, 2016 293 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 50
  46. #define FR_CORRECTION 2000
  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.     if (t_error > T_DEAD_ZONE && t_error < t_period_us/2)
  125.     {   uint32_t fr_correction = (uint64_t)t_error * FR_CORRECTION / t_period_us;
  126.         if (fr_correction > 256) fr_correction = 256;
  127.         //std::cout << "fr_correction=" << fr_correction << " t_error=" << t_error << std::endl;
  128.         MMAL_PARAMETER_FRAME_RATE_T param = {{MMAL_PARAMETER_VIDEO_FRAME_RATE, sizeof(param)}, {0, 0}};
  129.         param.frame_rate.num = FRAME_RATE_NUM_DEFAULT + fr_correction;
  130.         param.frame_rate.den = FRAME_RATE_DEN_DEFAULT;
  131.         mmal_port_parameter_set(camera_video_port, &param.hdr);
  132.     }
  133.     else if (t_error < t_period_us - T_DEAD_ZONE && t_error > t_period_us/2)
  134.     {   uint32_t fr_correction = (uint64_t)(t_period_us - t_error) * FR_CORRECTION / t_period_us;
  135.         if (fr_correction > 256) fr_correction = 256;
  136.         //std::cout << "fr_correction=" << fr_correction << " t_error=" << t_error << std::endl;
  137.         MMAL_PARAMETER_FRAME_RATE_T param = {{MMAL_PARAMETER_VIDEO_FRAME_RATE, sizeof(param)}, {0, 0}};
  138.         param.frame_rate.num = FRAME_RATE_NUM_DEFAULT - fr_correction;
  139.         param.frame_rate.den = FRAME_RATE_DEN_DEFAULT;
  140.         mmal_port_parameter_set(camera_video_port, &param.hdr);
  141.     }
  142.     else
  143.     {
  144.         MMAL_PARAMETER_FRAME_RATE_T param = {{MMAL_PARAMETER_VIDEO_FRAME_RATE, sizeof(param)}, {0, 0}};
  145.         param.frame_rate.num = FRAME_RATE_NUM_DEFAULT;
  146.         param.frame_rate.den = FRAME_RATE_DEN_DEFAULT;
  147.         mmal_port_parameter_set(camera_video_port, &param.hdr);
  148.     }
  149.  
  150.     MMAL_BUFFER_HEADER_T *new_buffer;
  151.     PORT_USERDATA * p_userdata = (PORT_USERDATA *) port->userdata;
  152.     MMAL_POOL_T *pool = p_userdata->camera_video_port_pool;
  153.  
  154.     mmal_buffer_header_mem_lock(buffer);
  155.  
  156.     // do something cool with data
  157.  
  158.     mmal_buffer_header_mem_unlock(buffer);
  159.     mmal_buffer_header_release(buffer);
  160.  
  161.     if (port->is_enabled)
  162.     {
  163.  
  164.         MMAL_STATUS_T status;
  165.         new_buffer = mmal_queue_get(pool->queue);
  166.  
  167.         if (new_buffer)
  168.         {
  169.             status = mmal_port_send_buffer(port, new_buffer);
  170.         }
  171.         if (!new_buffer || status != MMAL_SUCCESS)
  172.             printf("Unable to return a buffer to the video port\n");
  173.     }
  174. }
  175.  
  176.  
  177. int start_camera() {
  178.     MMAL_COMPONENT_T *camera = 0;
  179.     MMAL_COMPONENT_T *preview = 0;
  180.     MMAL_ES_FORMAT_T *format;
  181.     MMAL_STATUS_T status;
  182.     MMAL_PORT_T *camera_preview_port = NULL;
  183.     MMAL_PORT_T *camera_still_port = NULL;
  184.     MMAL_PORT_T *preview_input_port = NULL;
  185.     MMAL_POOL_T *camera_video_port_pool;
  186.     MMAL_CONNECTION_T *camera_preview_connection = 0;
  187.  
  188.     bcm_host_init();
  189.  
  190.     status = mmal_component_create(MMAL_COMPONENT_DEFAULT_CAMERA, &camera);
  191.     if (status != MMAL_SUCCESS) {
  192.         printf("Error: create camera %x\n", status);
  193.         return -1;
  194.     }
  195.  
  196.     camera_preview_port = camera->output[MMAL_CAMERA_PREVIEW_PORT];
  197.     camera_video_port = camera->output[MMAL_CAMERA_VIDEO_PORT];
  198.     camera_still_port = camera->output[MMAL_CAMERA_CAPTURE_PORT];
  199.  
  200.     {
  201.         MMAL_PARAMETER_CAMERA_CONFIG_T cam_config =
  202.         {
  203.             { MMAL_PARAMETER_CAMERA_CONFIG, sizeof (cam_config)},
  204.             .max_stills_w = WIDTH,
  205.             .max_stills_h = HEIGHT,
  206.             .stills_yuv422 = 0,
  207.             .one_shot_stills = 0,
  208.             .max_preview_video_w = WIDTH/2,
  209.             .max_preview_video_h = HEIGHT/2,
  210.             .num_preview_video_frames = 2,
  211.             .stills_capture_circular_buffer_height = 0,
  212.             .fast_preview_resume = 1,
  213.             .use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RAW_STC
  214.         };
  215.  
  216.         mmal_port_parameter_set(camera->control, &cam_config.hdr);
  217.     }
  218.  
  219.     format = camera_video_port->format;
  220.  
  221.     format->encoding = MMAL_ENCODING_I420;
  222.     format->encoding_variant = MMAL_ENCODING_I420;
  223.  
  224.     format->es->video.width = WIDTH/2;
  225.     format->es->video.height = HEIGHT/2;
  226.     format->es->video.crop.x = 0;
  227.     format->es->video.crop.y = 0;
  228.     format->es->video.crop.width = WIDTH/2;
  229.     format->es->video.crop.height = HEIGHT/2;
  230.     format->es->video.frame_rate.num = FRAME_RATE_NUM_DEFAULT;
  231.     format->es->video.frame_rate.den = FRAME_RATE_DEN_DEFAULT;
  232.  
  233.     camera_video_port->buffer_size = WIDTH * HEIGHT * 2;
  234.     camera_video_port->buffer_num = 5;
  235.     printf("  Camera video buffer_size = %d\n", camera_video_port->buffer_size);
  236.  
  237.     status = mmal_port_format_commit(camera_video_port);
  238.     printf("  Camera video recommended buffer_size = %d, %d\n", camera_video_port->buffer_size_recommended, camera_video_port->buffer_num_recommended);
  239.  
  240.     if (status != MMAL_SUCCESS) {
  241.         printf("Error: unable to commit camera video port format (%u)\n", status);
  242.         return -1;
  243.     }
  244.  
  245.     format = camera_preview_port->format;
  246.  
  247.     format->encoding = MMAL_ENCODING_OPAQUE;
  248.     format->encoding_variant = MMAL_ENCODING_I420;
  249.  
  250.     format->es->video.width = WIDTH/2;
  251.     format->es->video.height = HEIGHT/2;
  252.     format->es->video.crop.x = 0;
  253.     format->es->video.crop.y = 0;
  254.     format->es->video.crop.width = WIDTH/2;
  255.     format->es->video.crop.height = HEIGHT/2;
  256.  
  257.     status = mmal_port_format_commit(camera_preview_port);
  258.  
  259.     if (status != MMAL_SUCCESS) {
  260.         printf("Error: camera viewfinder format couldn't be set\n");
  261.         return -1;
  262.     }
  263.  
  264.     // create pool form camera video port
  265.     camera_video_port_pool = (MMAL_POOL_T *) mmal_port_pool_create(camera_video_port, camera_video_port->buffer_num, camera_video_port->buffer_size);
  266.  
  267.     std::cout << " num buffer headers: " <<  camera_video_port_pool->headers_num << std::endl;
  268.  
  269.     userdata.camera_video_port_pool = camera_video_port_pool;
  270.  
  271.     camera_video_port->userdata = (struct MMAL_PORT_USERDATA_T *) &userdata;
  272.  
  273.     status = mmal_port_enable(camera_video_port, video_buffer_callback);
  274.     if (status != MMAL_SUCCESS) {
  275.         printf("Error: unable to enable camera video port (%u)\n", status);
  276.         return -1;
  277.     }
  278.  
  279.  
  280.     status = mmal_component_enable(camera);
  281.  
  282.     status = mmal_component_create(MMAL_COMPONENT_DEFAULT_VIDEO_RENDERER, &preview);
  283.     if (status != MMAL_SUCCESS) {
  284.         printf("Error: unable to create preview (%u)\n", status);
  285.         return -1;
  286.     }
  287.     preview_input_port = preview->input[0];
  288.  
  289.     {
  290.         MMAL_DISPLAYREGION_T param;
  291.         param.hdr.id = MMAL_PARAMETER_DISPLAYREGION;
  292.         param.hdr.size = sizeof (MMAL_DISPLAYREGION_T);
  293.         param.set = MMAL_DISPLAY_SET_LAYER;
  294.         param.layer = 0;
  295.         param.set |= MMAL_DISPLAY_SET_FULLSCREEN;
  296.         param.fullscreen = 1;
  297.         status = mmal_port_parameter_set(preview_input_port, &param.hdr);
  298.         if (status != MMAL_SUCCESS && status != MMAL_ENOSYS) {
  299.             printf("Error: unable to set preview port parameters (%u)\n", status);
  300.             return -1;
  301.         }
  302.     }
  303.  
  304.     status = mmal_connection_create(&camera_preview_connection, camera_preview_port, preview_input_port, MMAL_CONNECTION_FLAG_TUNNELLING | MMAL_CONNECTION_FLAG_ALLOCATION_ON_INPUT);
  305.     if (status != MMAL_SUCCESS) {
  306.         printf("Error: unable to create connection (%u)\n", status);
  307.         return -1;
  308.     }
  309.  
  310.     status = mmal_connection_enable(camera_preview_connection);
  311.     if (status != MMAL_SUCCESS) {
  312.         printf("Error: unable to enable connection (%u)\n", status);
  313.         return -1;
  314.     }
  315.  
  316.     if (1) {
  317.         // Send all the buffers to the camera video port
  318.         int num = mmal_queue_length(camera_video_port_pool->queue);
  319.         int q;
  320.  
  321.         for (q = 0; q < num; q++) {
  322.             MMAL_BUFFER_HEADER_T *buffer = mmal_queue_get(camera_video_port_pool->queue);
  323.  
  324.             if (!buffer) {
  325.                 printf("Unable to get a required buffer %d from pool queue\n", q);
  326.             }
  327.  
  328.             if (mmal_port_send_buffer(camera_video_port, buffer) != MMAL_SUCCESS) {
  329.                 printf("Unable to send a buffer to encoder output port (%d)\n", q);
  330.             }
  331.         }
  332.     }
  333.  
  334.     if (mmal_port_parameter_set_boolean(camera_video_port, MMAL_PARAMETER_CAPTURE, 1) != MMAL_SUCCESS) {
  335.         printf("%s: Failed to start capture\n", __func__);
  336.     }
  337.  
  338.     if (mmal_port_parameter_set_int32(camera->control, MMAL_PARAMETER_EXPOSURE_COMP , -24) != MMAL_SUCCESS) {
  339.         printf("Failed to set exp comp\n");
  340.     }
  341.     if (mmal_port_parameter_set_uint32(camera->control, MMAL_PARAMETER_SHUTTER_SPEED, 23800)!= MMAL_SUCCESS) {
  342.         printf("Failed to set exp comp\n");
  343.     }
  344.  
  345.  
  346.     {
  347.        MMAL_PARAMETER_AWBMODE_T param = {{MMAL_PARAMETER_AWB_MODE,sizeof(param)}, MMAL_PARAM_AWBMODE_OFF};
  348.  
  349.        int st = mmal_port_parameter_set(camera->control, &param.hdr);
  350.        printf("awb_mode=%d\n", st);
  351.     }
  352.     {
  353.  
  354.         MMAL_PARAMETER_AWB_GAINS_T param = {{MMAL_PARAMETER_CUSTOM_AWB_GAINS, sizeof(param)}, {0, 0}, {0, 0}};
  355.  
  356.        param.r_gain.num = 65536;
  357.        param.b_gain.num = 65536;
  358.        param.r_gain.den = param.b_gain.den = 65536;
  359.        int st = mmal_port_parameter_set(camera->control, &param.hdr);
  360.        printf("awbg=%d\n", st);
  361.     }
  362.  
  363.     return 0;
  364.  
  365. }
  366.  
  367. int main()
  368. {
  369.     if (wiringPiSetup () == -1)
  370.         exit (1) ;
  371.  
  372.     //wiringPiISR (1, INT_EDGE_RISING, &pwm_int);
  373.  
  374.     pinMode (1, PWM_OUTPUT) ;
  375.     pwmSetMode  (PWM_MODE_MS);
  376.     int sclock = 19;
  377.     uint32_t range = 19.2e6/sclock/40 - 19;
  378.     pwmSetRange (range);
  379.     pwmWrite (1, 3);
  380.     struct timespec t1, t2;
  381.     uint64_t tdiff = 0;
  382.     int i = 0;
  383.     do
  384.     {
  385.         clock_gettime(CLOCK_MONOTONIC, &t1);
  386.         pwmSetClock (19);
  387.         clock_gettime(CLOCK_MONOTONIC, &t2);
  388.         tdiff = (uint64_t)t2.tv_sec*1000000 + t2.tv_nsec / 1000 - (uint64_t)t1.tv_sec*1000000 - t1.tv_nsec / 1000;
  389.         ++i;
  390.     } while (tdiff > 200);
  391.     std::cout << "tdiff=" << tdiff << "  i=" << i << "\n";
  392.  
  393.     pwm_start_time = ((uint64_t)t1.tv_sec)*1000000 + t1.tv_nsec / 1000;
  394.     pwm_period = range*sclock/19.2e6;
  395.  
  396.     std::this_thread::sleep_for(std::chrono::microseconds(10000));
  397.     std::thread mmal_thread (start_camera);
  398.  
  399.     while (true)
  400.     {
  401.         std::cout << "enter offset [┬Ás]:";
  402.         std::cin >> pwm_offset;
  403.         std::cout << "o=" << pwm_offset << std::endl;
  404.     }
  405.     return 0;
  406. }
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
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!
 
Top