"Fossies" - the Fresh Open Source Software Archive

Member "motion-Release-4.3.0/src/mmalcam.c" (14 Jan 2020, 14176 Bytes) of package /linux/misc/motion-Release-4.3.0.tar.gz:


As a special service "Fossies" has tried to format the requested source page into HTML format using (guessed) C and C++ source code syntax highlighting (style: standard) with prefixed line numbers and code folding option. Alternatively you can here view or download the uninterpreted source code file. For more information about "mmalcam.c" see the Fossies "Dox" file reference documentation.

    1 /*
    2  * mmalcam.c
    3  *
    4  *    Raspberry Pi camera module using MMAL API.
    5  *
    6  *    Built upon functionality from the Raspberry Pi userland utility raspivid.
    7  *
    8  *    Copyright 2013 by Nicholas Tuckett
    9  *    This software is distributed under the GNU public license version 2
   10  *    See also the file 'COPYING'.
   11  *
   12  */
   13 
   14 #include "translate.h"
   15 #include "motion.h"
   16 #include "rotate.h"
   17 
   18 #ifdef HAVE_MMAL
   19 
   20 #include "interface/vcos/vcos.h"
   21 #include "interface/mmal/mmal.h"
   22 #include "interface/mmal/mmal_buffer.h"
   23 #include "interface/mmal/mmal_port.h"
   24 #include "interface/mmal/util/mmal_util.h"
   25 #include "interface/mmal/util/mmal_util_params.h"
   26 #include "interface/mmal/util/mmal_default_components.h"
   27 #include "interface/mmal/util/mmal_connection.h"
   28 #include "raspicam/RaspiCamControl.h"
   29 
   30 #define MMALCAM_OK        0
   31 #define MMALCAM_ERROR    -1
   32 
   33 #define MMAL_CAMERA_PREVIEW_PORT 0
   34 #define MMAL_CAMERA_VIDEO_PORT 1
   35 #define MMAL_CAMERA_CAPTURE_PORT 2
   36 #define VIDEO_FRAME_RATE_NUM 30
   37 #define VIDEO_FRAME_RATE_DEN 1
   38 #define VIDEO_OUTPUT_BUFFERS_NUM 3
   39 
   40 const int MAX_BITRATE = 30000000; // 30Mbits/s
   41 
   42 static void parse_camera_control_params(const char *control_params_str, RASPICAM_CAMERA_PARAMETERS *camera_params)
   43 {
   44     char *control_params_tok = alloca(strlen(control_params_str) + 1);
   45     strcpy(control_params_tok, control_params_str);
   46 
   47     char *next_param = strtok(control_params_tok, " ");
   48 
   49     while (next_param != NULL) {
   50         char *param_val = strtok(NULL, " ");
   51         if (raspicamcontrol_parse_cmdline(camera_params, next_param + 1, param_val) < 2) {
   52             next_param = param_val;
   53         } else {
   54             next_param = strtok(NULL, " ");
   55         }
   56     }
   57 }
   58 
   59 static void check_disable_port(MMAL_PORT_T *port)
   60 {
   61     if (port && port->is_enabled) {
   62         mmal_port_disable(port);
   63     }
   64 }
   65 
   66 static void camera_control_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
   67 {
   68     if (buffer->cmd != MMAL_EVENT_PARAMETER_CHANGED) {
   69         MOTION_LOG(ERR, TYPE_VIDEO, NO_ERRNO
   70             ,_("Received unexpected camera control callback event, 0x%08x"), buffer->cmd);
   71     }
   72 
   73     mmal_buffer_header_release(buffer);
   74 }
   75 
   76 static void camera_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
   77 {
   78     mmalcam_context_ptr mmalcam = (mmalcam_context_ptr) port->userdata;
   79     mmal_queue_put(mmalcam->camera_buffer_queue, buffer);
   80 }
   81 
   82 static void set_port_format(mmalcam_context_ptr mmalcam, MMAL_ES_FORMAT_T *format)
   83 {
   84     format->encoding = MMAL_ENCODING_OPAQUE;
   85     format->encoding_variant = MMAL_ENCODING_I420;
   86     format->es->video.width = mmalcam->width;
   87     format->es->video.height = mmalcam->height;
   88     format->es->video.crop.x = 0;
   89     format->es->video.crop.y = 0;
   90     format->es->video.crop.width = mmalcam->width;
   91     format->es->video.crop.height = mmalcam->height;
   92 }
   93 
   94 static void set_video_port_format(mmalcam_context_ptr mmalcam, MMAL_ES_FORMAT_T *format)
   95 {
   96     set_port_format(mmalcam, format);
   97     format->es->video.frame_rate.num = mmalcam->framerate;
   98     format->es->video.frame_rate.den = VIDEO_FRAME_RATE_DEN;
   99     if (mmalcam->framerate > 30){
  100         /* The pi noir camera could not determine autoexpose at high frame rates */
  101         MOTION_LOG(WRN, TYPE_VIDEO, NO_ERRNO, _("A high frame rate can cause problems with exposure of images"));
  102         MOTION_LOG(WRN, TYPE_VIDEO, NO_ERRNO, _("If autoexposure is not working, try a lower frame rate."));
  103     }
  104 }
  105 
  106 static int create_camera_component(mmalcam_context_ptr mmalcam, const char *mmalcam_name)
  107 {
  108     MMAL_STATUS_T status;
  109     MMAL_COMPONENT_T *camera_component;
  110     MMAL_PORT_T *video_port = NULL;
  111 
  112     status = mmal_component_create(mmalcam_name, &camera_component);
  113 
  114     if (status != MMAL_SUCCESS) {
  115         MOTION_LOG(ERR, TYPE_VIDEO, NO_ERRNO
  116             ,_("Failed to create MMAL camera component %s"), mmalcam_name);
  117         goto error;
  118     }
  119 
  120     if (camera_component->output_num == 0) {
  121         MOTION_LOG(ERR, TYPE_VIDEO, NO_ERRNO
  122             ,_("MMAL camera %s doesn't have output ports"), mmalcam_name);
  123         goto error;
  124     }
  125 
  126     video_port = camera_component->output[MMAL_CAMERA_VIDEO_PORT];
  127 
  128     status = mmal_port_enable(camera_component->control, camera_control_callback);
  129 
  130     if (status) {
  131         MOTION_LOG(ERR, TYPE_VIDEO, NO_ERRNO
  132             ,_("Unable to enable control port : error %d"), status);
  133         goto error;
  134     }
  135 
  136     //  set up the camera configuration
  137     {
  138         MMAL_PARAMETER_CAMERA_CONFIG_T cam_config = {
  139                 { MMAL_PARAMETER_CAMERA_CONFIG, sizeof(cam_config) },
  140                 .max_stills_w = mmalcam->width,
  141                 .max_stills_h = mmalcam->height,
  142                 .stills_yuv422 = 0,
  143                 .one_shot_stills = 0,
  144                 .max_preview_video_w = mmalcam->width,
  145                 .max_preview_video_h = mmalcam->height,
  146                 .num_preview_video_frames = 3,
  147                 .stills_capture_circular_buffer_height = 0,
  148                 .fast_preview_resume = 0,
  149                 .use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RESET_STC };
  150         mmal_port_parameter_set(camera_component->control, &cam_config.hdr);
  151     }
  152 
  153     set_video_port_format(mmalcam, video_port->format);
  154     video_port->format->encoding = MMAL_ENCODING_I420;
  155     // set buffer size for an aligned/padded frame
  156     video_port->buffer_size = VCOS_ALIGN_UP(mmalcam->width, 32) *
  157         VCOS_ALIGN_UP(mmalcam->height, 16) * 3 / 2;
  158 
  159     if (mmal_port_parameter_set_boolean(video_port, MMAL_PARAMETER_NO_IMAGE_PADDING, 1)
  160             != MMAL_SUCCESS) {
  161         MOTION_LOG(WRN, TYPE_VIDEO, NO_ERRNO, _("MMAL no-padding setup failed"));
  162     }
  163 
  164     status = mmal_port_format_commit(video_port);
  165 
  166     if (status) {
  167         MOTION_LOG(ERR, TYPE_VIDEO, NO_ERRNO, _("camera video format couldn't be set"));
  168         goto error;
  169     }
  170 
  171     // Ensure there are enough buffers to avoid dropping frames
  172     if (video_port->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM) {
  173         video_port->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM;
  174     }
  175 
  176     status = mmal_component_enable(camera_component);
  177 
  178     if (status) {
  179         MOTION_LOG(ERR, TYPE_VIDEO, NO_ERRNO, _("camera component couldn't be enabled"));
  180         goto error;
  181     }
  182 
  183     raspicamcontrol_set_all_parameters(camera_component, mmalcam->camera_parameters);
  184     mmalcam->camera_component = camera_component;
  185     mmalcam->camera_capture_port = video_port;
  186     mmalcam->camera_capture_port->userdata = (struct MMAL_PORT_USERDATA_T*) mmalcam;
  187     MOTION_LOG(NTC, TYPE_VIDEO, NO_ERRNO, _("MMAL camera component created"));
  188     return MMALCAM_OK;
  189 
  190     error: if (mmalcam->camera_component != NULL ) {
  191         mmal_component_destroy(camera_component);
  192         mmalcam->camera_component = NULL;
  193     }
  194 
  195     return MMALCAM_ERROR;
  196 }
  197 
  198 static void destroy_camera_component(mmalcam_context_ptr mmalcam)
  199 {
  200     if (mmalcam->camera_component) {
  201         mmal_component_destroy(mmalcam->camera_component);
  202         mmalcam->camera_component = NULL;
  203     }
  204 }
  205 
  206 static int create_camera_buffer_structures(mmalcam_context_ptr mmalcam)
  207 {
  208     mmalcam->camera_buffer_pool = mmal_pool_create(mmalcam->camera_capture_port->buffer_num,
  209             mmalcam->camera_capture_port->buffer_size);
  210     if (mmalcam->camera_buffer_pool == NULL ) {
  211         MOTION_LOG(ERR, TYPE_VIDEO, NO_ERRNO, _("MMAL camera buffer pool creation failed"));
  212         return MMALCAM_ERROR;
  213     }
  214 
  215     mmalcam->camera_buffer_queue = mmal_queue_create();
  216     if (mmalcam->camera_buffer_queue == NULL ) {
  217         MOTION_LOG(ERR, TYPE_VIDEO, NO_ERRNO, _("MMAL camera buffer queue creation failed"));
  218         return MMALCAM_ERROR;
  219     }
  220 
  221     return MMALCAM_OK;
  222 }
  223 
  224 static int send_pooled_buffers_to_port(MMAL_POOL_T *pool, MMAL_PORT_T *port)
  225 {
  226     int num = mmal_queue_length(pool->queue);
  227 
  228     int i;
  229     for (i = 0; i < num; i++) {
  230         MMAL_BUFFER_HEADER_T *buffer = mmal_queue_get(pool->queue);
  231 
  232         if (!buffer) {
  233             MOTION_LOG(ERR, TYPE_VIDEO, NO_ERRNO
  234                 ,_("Unable to get a required buffer %d from pool queue"), i);
  235             return MMALCAM_ERROR;
  236         }
  237 
  238         if (mmal_port_send_buffer(port, buffer) != MMAL_SUCCESS) {
  239             MOTION_LOG(ERR, TYPE_VIDEO, NO_ERRNO, _("Unable to send a buffer to port (%d)"), i);
  240             return MMALCAM_ERROR;
  241         }
  242     }
  243 
  244     return MMALCAM_OK;
  245 }
  246 
  247 static void destroy_camera_buffer_structures(mmalcam_context_ptr mmalcam)
  248 {
  249     if (mmalcam->camera_buffer_queue != NULL ) {
  250         mmal_queue_destroy(mmalcam->camera_buffer_queue);
  251         mmalcam->camera_buffer_queue = NULL;
  252     }
  253 
  254     if (mmalcam->camera_buffer_pool != NULL ) {
  255         mmal_pool_destroy(mmalcam->camera_buffer_pool);
  256         mmalcam->camera_buffer_pool = NULL;
  257     }
  258 }
  259 
  260 /**
  261  * mmalcam_start
  262  *
  263  *      This routine is called from the main motion thread.  It's job is
  264  *      to open up the requested camera device via MMAL and do any required
  265  *      initialization.
  266  *
  267  * Parameters:
  268  *
  269  *      cnt     Pointer to the motion context structure for this device.
  270  *
  271  * Returns:     0 on success
  272  *              -1 on any failure
  273  */
  274 
  275 int mmalcam_start(struct context *cnt)
  276 {
  277     mmalcam_context_ptr mmalcam;
  278 
  279     cnt->mmalcam = (mmalcam_context*) mymalloc(sizeof(struct mmalcam_context));
  280     memset(cnt->mmalcam, 0, sizeof(mmalcam_context));
  281     mmalcam = cnt->mmalcam;
  282     mmalcam->cnt = cnt;
  283 
  284     MOTION_LOG(NTC, TYPE_VIDEO, NO_ERRNO
  285         ,_("MMAL Camera thread starting... for camera (%s) of %d x %d at %d fps")
  286         ,cnt->conf.mmalcam_name, cnt->conf.width, cnt->conf.height, cnt->conf.framerate);
  287 
  288     mmalcam->camera_parameters = (RASPICAM_CAMERA_PARAMETERS*)malloc(sizeof(RASPICAM_CAMERA_PARAMETERS));
  289     if (mmalcam->camera_parameters == NULL) {
  290         MOTION_LOG(ERR, TYPE_VIDEO, NO_ERRNO, _("camera params couldn't be allocated"));
  291         return MMALCAM_ERROR;
  292     }
  293 
  294     raspicamcontrol_set_defaults(mmalcam->camera_parameters);
  295     mmalcam->width = cnt->conf.width;
  296     mmalcam->height = cnt->conf.height;
  297     mmalcam->framerate = cnt->conf.framerate;
  298 
  299     if (cnt->conf.mmalcam_control_params) {
  300         parse_camera_control_params(cnt->conf.mmalcam_control_params, mmalcam->camera_parameters);
  301     }
  302 
  303     cnt->imgs.width = mmalcam->width;
  304     cnt->imgs.height = mmalcam->height;
  305     cnt->imgs.size_norm = (mmalcam->width * mmalcam->height * 3) / 2;
  306     cnt->imgs.motionsize = mmalcam->width * mmalcam->height;
  307 
  308     int retval = create_camera_component(mmalcam, cnt->conf.mmalcam_name);
  309 
  310     if (retval == 0) {
  311         retval = create_camera_buffer_structures(mmalcam);
  312     }
  313 
  314     if (retval == 0) {
  315         if (mmal_port_enable(mmalcam->camera_capture_port, camera_buffer_callback)) {
  316             MOTION_LOG(ERR, TYPE_VIDEO, NO_ERRNO, _("MMAL camera capture port enabling failed"));
  317             retval = MMALCAM_ERROR;
  318         }
  319     }
  320 
  321     if (retval == 0) {
  322         if (mmal_port_parameter_set_boolean(mmalcam->camera_capture_port, MMAL_PARAMETER_CAPTURE, 1)
  323                 != MMAL_SUCCESS) {
  324             MOTION_LOG(ERR, TYPE_VIDEO, NO_ERRNO, _("MMAL camera capture start failed"));
  325             retval = MMALCAM_ERROR;
  326         }
  327     }
  328 
  329     if (retval == 0) {
  330         retval = send_pooled_buffers_to_port(mmalcam->camera_buffer_pool, mmalcam->camera_capture_port);
  331     }
  332 
  333     return retval;
  334 }
  335 
  336 /**
  337  * mmalcam_cleanup
  338  *
  339  *      This routine shuts down any MMAL resources, then releases any allocated data
  340  *      within the mmalcam context and frees the context itself.
  341  *      This function is also called from motion_init if first time connection
  342  *      fails and we start retrying until we get a valid first frame from the
  343  *      camera.
  344  *
  345  * Parameters:
  346  *
  347  *      mmalcam          Pointer to a mmalcam context
  348  *
  349  * Returns:              Nothing.
  350  *
  351  */
  352 void mmalcam_cleanup(struct mmalcam_context *mmalcam)
  353 {
  354     MOTION_LOG(NTC, TYPE_VIDEO, NO_ERRNO, _("MMAL Camera cleanup"));
  355 
  356     if (mmalcam != NULL ) {
  357         if (mmalcam->camera_component) {
  358             check_disable_port(mmalcam->camera_capture_port);
  359             mmal_component_disable(mmalcam->camera_component);
  360             destroy_camera_buffer_structures(mmalcam);
  361             destroy_camera_component(mmalcam);
  362         }
  363 
  364         if (mmalcam->camera_parameters) {
  365             free(mmalcam->camera_parameters);
  366         }
  367 
  368         free(mmalcam);
  369     }
  370 }
  371 
  372 /**
  373  * mmalcam_next
  374  *
  375  *      This routine is called when the main 'motion' thread wants a new
  376  *      frame of video.  It fetches the most recent frame available from
  377  *      the Pi camera already in YUV420P, and returns it to motion.
  378  *
  379  * Parameters:
  380  *      cnt             Pointer to the context for this thread
  381  *      image           Pointer to a buffer for the returned image
  382  *
  383  * Returns:             Error code
  384  */
  385 int mmalcam_next(struct context *cnt,  struct image_data *img_data)
  386 {
  387     mmalcam_context_ptr mmalcam;
  388 
  389     if ((!cnt) || (!cnt->mmalcam))
  390         return NETCAM_FATAL_ERROR;
  391 
  392     mmalcam = cnt->mmalcam;
  393 
  394     MMAL_BUFFER_HEADER_T *camera_buffer = mmal_queue_wait(mmalcam->camera_buffer_queue);
  395 
  396     if (camera_buffer->cmd == 0 && (camera_buffer->flags & MMAL_BUFFER_HEADER_FLAG_FRAME_END)
  397             && camera_buffer->length >= cnt->imgs.size_norm) {
  398         mmal_buffer_header_mem_lock(camera_buffer);
  399         memcpy(img_data->image_norm, camera_buffer->data, cnt->imgs.size_norm);
  400         mmal_buffer_header_mem_unlock(camera_buffer);
  401     } else {
  402         MOTION_LOG(ERR, TYPE_VIDEO, NO_ERRNO
  403             ,_("cmd %d flags %08x size %d/%d at %08x, img_size=%d")
  404             ,camera_buffer->cmd, camera_buffer->flags, camera_buffer->length
  405             ,camera_buffer->alloc_size, camera_buffer->data, cnt->imgs.size_norm);
  406     }
  407 
  408     mmal_buffer_header_release(camera_buffer);
  409 
  410     if (mmalcam->camera_capture_port->is_enabled) {
  411         MMAL_STATUS_T status;
  412         MMAL_BUFFER_HEADER_T *new_buffer = mmal_queue_get(mmalcam->camera_buffer_pool->queue);
  413 
  414         if (new_buffer) {
  415             status = mmal_port_send_buffer(mmalcam->camera_capture_port, new_buffer);
  416         }
  417 
  418         if (!new_buffer || status != MMAL_SUCCESS)
  419             MOTION_LOG(ERR, TYPE_VIDEO, NO_ERRNO
  420                 ,_("Unable to return a buffer to the camera video port"));
  421     }
  422 
  423     rotate_map(cnt,img_data);
  424 
  425     return 0;
  426 }
  427 
  428 #endif