Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /***
- *
- * This example is just a proof of a concept 6by9 gave in the raspi forum:
- * that one could synchronize the raspi cam to an external periodical event just
- * fiddling with the framerates.
- *
- * So if you have an external periodical event like:
- * t_event = start_time + k * period, for k being an integer
- *
- * what we want here is to have the frame taken always at the same controlled t_dist from t_event.
- *
- * One could sync many raspis together using PTP, for instance, and get stereo vision synced.
- * Or sync the raspi to the internal (or external) PWM, like done here.
- *
- * This is not a solution for imaging at the exact external trigger event.
- *
- * and is just a hack. it is not like I am caring for mutexes, return status of mmal calls, polluting globals.
- * It it just to show that the concept works.
- *
- *
- ***/
- #include <iostream>
- #include <thread>
- #include <time.h>
- #include <wiringPi.h>
- // if one is compiling this, will need to add a few include directories manually in the gcc comamnd.
- // at least this is needed when compiling for a buildroot generated sysroot.
- // since inside the include files there are many other includes without paths.
- #include "mmal/mmal.h"
- #include "mmal/util/mmal_util.h"
- #include "mmal/util/mmal_default_components.h"
- #include "mmal/util/mmal_connection.h"
- #include "mmal/util/mmal_util_params.h"
- #include "bcm_host.h"
- #define WIDTH 1296
- #define HEIGHT 972
- #define FRAME_RATE_NUM_DEFAULT (40*256)
- #define FRAME_RATE_DEN_DEFAULT 256
- #define T_DEAD_ZONE 10
- #define FR_CORRECTION 32
- #define MMAL_CAMERA_PREVIEW_PORT 0
- #define MMAL_CAMERA_VIDEO_PORT 1
- #define MMAL_CAMERA_CAPTURE_PORT 2
- typedef struct {
- MMAL_POOL_T *camera_video_port_pool;
- } PORT_USERDATA;
- static PORT_USERDATA userdata;
- static MMAL_PORT_T * camera_video_port = nullptr;
- uint64_t pwm_counter;
- uint64_t pwm_start_time;
- double pwm_period;
- uint64_t pwm_offset;
- static void video_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
- {
- static uint64_t frame_count = 0;
- static struct timespec ts;
- frame_count++;
- // get the offset between the mmal time and linux time. We need this to correct the
- // frames timestamps.
- uint64_t t_mmal = 0;
- mmal_port_parameter_get_uint64(port, MMAL_PARAMETER_SYSTEM_TIME, &t_mmal);
- clock_gettime(CLOCK_MONOTONIC, &ts);
- uint64_t t_linux = (uint64_t)ts.tv_sec*1000000 + ts.tv_nsec / 1000;
- int64_t t_offset = t_linux - t_mmal;
- uint64_t t_frame_mmal = buffer->pts; // this timestamp is referenced to the mmal clock.
- uint64_t t_frame_linux = t_frame_mmal + t_offset; // now we have the frame timestamp referenced to the linux clock
- // std::cout << "t_offset =" << t_offset << std::endl;
- // to see mmal time and linux time drifting apart, uncomment above line
- // we have a periodic event we want to sync to.
- // This event is modeled like:
- // t_event(k) = t_event_start + k * t_period, where k is integer.
- // we wanto to keep t_frame_linux, above, locked at a fixed given distance from t_event.
- // like:
- // t_frame_linux = t_event_start + k * t_period + t_dist
- //
- // of course the locking is nor perfect, so we can define:
- // t_error = t_frame_linux - (t_event_start + k * t_period + t_dist)
- //
- // and we should drive the software PLL to zero t_error.
- // we now should find t_error. Begin first by finding a suitable k.
- uint64_t t_event_start = pwm_start_time; // here will sync to pwm
- uint64_t t_dist = pwm_offset;
- double t_period = pwm_period*1e6; // t_period should be in microsec
- uint32_t t_period_us = t_period;
- uint32_t k = (t_frame_linux - t_event_start - t_dist)/t_period;
- uint32_t t_error = t_frame_linux - (t_event_start + (uint64_t)(k * t_period) + t_dist);
- // by contruction, t_error > 0
- // we should drive it to:
- // 0, if t_error < t_period_us / 2;
- // t_period_us, if t_error > t_period_us/2
- // to decrease t_error, we should decrease t_frame_linux by increasing the fps
- // to increase t_error, descrease fps
- // and near a dead zone, close to the targets, we could set a neutral fps
- //std::cout << "cnt= " << frame_count << " t_error=" << t_error << " t_period=" << t_period_us << " k=" << k << std::endl;
- if (t_error > T_DEAD_ZONE && t_error < t_period_us/2)
- {
- MMAL_PARAMETER_FRAME_RATE_T param = {{MMAL_PARAMETER_VIDEO_FRAME_RATE, sizeof(param)}, {0, 0}};
- param.frame_rate.num = FRAME_RATE_NUM_DEFAULT + FR_CORRECTION;
- param.frame_rate.den = FRAME_RATE_DEN_DEFAULT;
- mmal_port_parameter_set(camera_video_port, ¶m.hdr);
- }
- else if (t_error < t_period_us - T_DEAD_ZONE && t_error > t_period_us/2)
- {
- MMAL_PARAMETER_FRAME_RATE_T param = {{MMAL_PARAMETER_VIDEO_FRAME_RATE, sizeof(param)}, {0, 0}};
- param.frame_rate.num = FRAME_RATE_NUM_DEFAULT - FR_CORRECTION;
- param.frame_rate.den = FRAME_RATE_DEN_DEFAULT;
- mmal_port_parameter_set(camera_video_port, ¶m.hdr);
- }
- else
- {
- MMAL_PARAMETER_FRAME_RATE_T param = {{MMAL_PARAMETER_VIDEO_FRAME_RATE, sizeof(param)}, {0, 0}};
- param.frame_rate.num = FRAME_RATE_NUM_DEFAULT;
- param.frame_rate.den = FRAME_RATE_DEN_DEFAULT;
- mmal_port_parameter_set(camera_video_port, ¶m.hdr);
- }
- MMAL_BUFFER_HEADER_T *new_buffer;
- PORT_USERDATA * p_userdata = (PORT_USERDATA *) port->userdata;
- MMAL_POOL_T *pool = p_userdata->camera_video_port_pool;
- mmal_buffer_header_mem_lock(buffer);
- // do something cool with data
- mmal_buffer_header_mem_unlock(buffer);
- mmal_buffer_header_release(buffer);
- if (port->is_enabled)
- {
- MMAL_STATUS_T status;
- new_buffer = mmal_queue_get(pool->queue);
- if (new_buffer)
- {
- status = mmal_port_send_buffer(port, new_buffer);
- }
- if (!new_buffer || status != MMAL_SUCCESS)
- printf("Unable to return a buffer to the video port\n");
- }
- }
- int start_camera() {
- MMAL_COMPONENT_T *camera = 0;
- MMAL_COMPONENT_T *preview = 0;
- MMAL_ES_FORMAT_T *format;
- MMAL_STATUS_T status;
- MMAL_PORT_T *camera_preview_port = NULL;
- MMAL_PORT_T *camera_still_port = NULL;
- MMAL_PORT_T *preview_input_port = NULL;
- MMAL_POOL_T *camera_video_port_pool;
- MMAL_CONNECTION_T *camera_preview_connection = 0;
- bcm_host_init();
- status = mmal_component_create(MMAL_COMPONENT_DEFAULT_CAMERA, &camera);
- if (status != MMAL_SUCCESS) {
- printf("Error: create camera %x\n", status);
- return -1;
- }
- camera_preview_port = camera->output[MMAL_CAMERA_PREVIEW_PORT];
- camera_video_port = camera->output[MMAL_CAMERA_VIDEO_PORT];
- camera_still_port = camera->output[MMAL_CAMERA_CAPTURE_PORT];
- {
- MMAL_PARAMETER_CAMERA_CONFIG_T cam_config =
- {
- { MMAL_PARAMETER_CAMERA_CONFIG, sizeof (cam_config)},
- .max_stills_w = WIDTH,
- .max_stills_h = HEIGHT,
- .stills_yuv422 = 0,
- .one_shot_stills = 0,
- .max_preview_video_w = WIDTH/2,
- .max_preview_video_h = HEIGHT/2,
- .num_preview_video_frames = 2,
- .stills_capture_circular_buffer_height = 0,
- .fast_preview_resume = 1,
- .use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RAW_STC
- };
- mmal_port_parameter_set(camera->control, &cam_config.hdr);
- }
- format = camera_video_port->format;
- format->encoding = MMAL_ENCODING_I420;
- format->encoding_variant = MMAL_ENCODING_I420;
- format->es->video.width = WIDTH/2;
- format->es->video.height = HEIGHT/2;
- format->es->video.crop.x = 0;
- format->es->video.crop.y = 0;
- format->es->video.crop.width = WIDTH/2;
- format->es->video.crop.height = HEIGHT/2;
- format->es->video.frame_rate.num = FRAME_RATE_NUM_DEFAULT;
- format->es->video.frame_rate.den = FRAME_RATE_DEN_DEFAULT;
- camera_video_port->buffer_size = WIDTH * HEIGHT * 2;
- camera_video_port->buffer_num = 5;
- printf(" Camera video buffer_size = %d\n", camera_video_port->buffer_size);
- status = mmal_port_format_commit(camera_video_port);
- printf(" Camera video recommended buffer_size = %d, %d\n", camera_video_port->buffer_size_recommended, camera_video_port->buffer_num_recommended);
- if (status != MMAL_SUCCESS) {
- printf("Error: unable to commit camera video port format (%u)\n", status);
- return -1;
- }
- format = camera_preview_port->format;
- format->encoding = MMAL_ENCODING_OPAQUE;
- format->encoding_variant = MMAL_ENCODING_I420;
- format->es->video.width = WIDTH/2;
- format->es->video.height = HEIGHT/2;
- format->es->video.crop.x = 0;
- format->es->video.crop.y = 0;
- format->es->video.crop.width = WIDTH/2;
- format->es->video.crop.height = HEIGHT/2;
- status = mmal_port_format_commit(camera_preview_port);
- if (status != MMAL_SUCCESS) {
- printf("Error: camera viewfinder format couldn't be set\n");
- return -1;
- }
- // create pool form camera video port
- camera_video_port_pool = (MMAL_POOL_T *) mmal_port_pool_create(camera_video_port, camera_video_port->buffer_num, camera_video_port->buffer_size);
- std::cout << " num buffer headers: " << camera_video_port_pool->headers_num << std::endl;
- userdata.camera_video_port_pool = camera_video_port_pool;
- camera_video_port->userdata = (struct MMAL_PORT_USERDATA_T *) &userdata;
- status = mmal_port_enable(camera_video_port, video_buffer_callback);
- if (status != MMAL_SUCCESS) {
- printf("Error: unable to enable camera video port (%u)\n", status);
- return -1;
- }
- status = mmal_component_enable(camera);
- status = mmal_component_create(MMAL_COMPONENT_DEFAULT_VIDEO_RENDERER, &preview);
- if (status != MMAL_SUCCESS) {
- printf("Error: unable to create preview (%u)\n", status);
- return -1;
- }
- preview_input_port = preview->input[0];
- {
- MMAL_DISPLAYREGION_T param;
- param.hdr.id = MMAL_PARAMETER_DISPLAYREGION;
- param.hdr.size = sizeof (MMAL_DISPLAYREGION_T);
- param.set = MMAL_DISPLAY_SET_LAYER;
- param.layer = 0;
- param.set |= MMAL_DISPLAY_SET_FULLSCREEN;
- param.fullscreen = 1;
- status = mmal_port_parameter_set(preview_input_port, ¶m.hdr);
- if (status != MMAL_SUCCESS && status != MMAL_ENOSYS) {
- printf("Error: unable to set preview port parameters (%u)\n", status);
- return -1;
- }
- }
- status = mmal_connection_create(&camera_preview_connection, camera_preview_port, preview_input_port, MMAL_CONNECTION_FLAG_TUNNELLING | MMAL_CONNECTION_FLAG_ALLOCATION_ON_INPUT);
- if (status != MMAL_SUCCESS) {
- printf("Error: unable to create connection (%u)\n", status);
- return -1;
- }
- status = mmal_connection_enable(camera_preview_connection);
- if (status != MMAL_SUCCESS) {
- printf("Error: unable to enable connection (%u)\n", status);
- return -1;
- }
- if (1) {
- // Send all the buffers to the camera video port
- int num = mmal_queue_length(camera_video_port_pool->queue);
- int q;
- for (q = 0; q < num; q++) {
- MMAL_BUFFER_HEADER_T *buffer = mmal_queue_get(camera_video_port_pool->queue);
- if (!buffer) {
- printf("Unable to get a required buffer %d from pool queue\n", q);
- }
- if (mmal_port_send_buffer(camera_video_port, buffer) != MMAL_SUCCESS) {
- printf("Unable to send a buffer to encoder output port (%d)\n", q);
- }
- }
- }
- if (mmal_port_parameter_set_boolean(camera_video_port, MMAL_PARAMETER_CAPTURE, 1) != MMAL_SUCCESS) {
- printf("%s: Failed to start capture\n", __func__);
- }
- if (mmal_port_parameter_set_int32(camera->control, MMAL_PARAMETER_EXPOSURE_COMP , -24) != MMAL_SUCCESS) {
- printf("Failed to set exp comp\n");
- }
- if (mmal_port_parameter_set_uint32(camera->control, MMAL_PARAMETER_SHUTTER_SPEED, 23800)!= MMAL_SUCCESS) {
- printf("Failed to set exp comp\n");
- }
- {
- MMAL_PARAMETER_AWBMODE_T param = {{MMAL_PARAMETER_AWB_MODE,sizeof(param)}, MMAL_PARAM_AWBMODE_OFF};
- int st = mmal_port_parameter_set(camera->control, ¶m.hdr);
- printf("awb_mode=%d\n", st);
- }
- {
- MMAL_PARAMETER_AWB_GAINS_T param = {{MMAL_PARAMETER_CUSTOM_AWB_GAINS, sizeof(param)}, {0, 0}, {0, 0}};
- param.r_gain.num = 65536;
- param.b_gain.num = 65536;
- param.r_gain.den = param.b_gain.den = 65536;
- int st = mmal_port_parameter_set(camera->control, ¶m.hdr);
- printf("awbg=%d\n", st);
- }
- return 0;
- }
- int main()
- {
- if (wiringPiSetup () == -1)
- exit (1) ;
- //wiringPiISR (1, INT_EDGE_RISING, &pwm_int);
- pinMode (1, PWM_OUTPUT) ;
- pwmSetMode (PWM_MODE_MS);
- int sclock = 19;
- uint32_t range = 19.2e6/sclock/40 - 19;
- pwmSetRange (range);
- pwmWrite (1, 3);
- struct timespec t1, t2;
- uint64_t tdiff = 0;
- int i = 0;
- do
- {
- clock_gettime(CLOCK_MONOTONIC, &t1);
- pwmSetClock (19);
- clock_gettime(CLOCK_MONOTONIC, &t2);
- tdiff = (uint64_t)t2.tv_sec*1000000 + t2.tv_nsec / 1000 - (uint64_t)t1.tv_sec*1000000 - t1.tv_nsec / 1000;
- ++i;
- } while (tdiff > 200);
- std::cout << "tdiff=" << tdiff << " i=" << i << "\n";
- pwm_start_time = ((uint64_t)t1.tv_sec)*1000000 + t1.tv_nsec / 1000;
- pwm_period = range*sclock/19.2e6;
- std::this_thread::sleep_for(std::chrono::microseconds(10000));
- std::thread mmal_thread (start_camera);
- uint64_t offset;
- while (true)
- {
- std::cout << "enter offset [µs]:";
- std::cin >> pwm_offset;
- std::cout << "o=" << offset << std::endl;
- }
- return 0;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement