rpicamsrc: checkpoint

This commit is contained in:
Jan Schmidt 2013-10-11 19:17:05 +11:00 committed by Tim-Philipp Müller
parent e4f74290df
commit f4af399350
10 changed files with 5444 additions and 34 deletions

File diff suppressed because it is too large Load diff

View file

@ -0,0 +1,184 @@
/*
* Copyright (c) 2013 Jan Schmidt <jan@centricular.com>
Portions:
Copyright (c) 2013, Broadcom Europe Ltd
Copyright (c) 2013, James Hughes
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef RASPICAMCONTROL_H_
#define RASPICAMCONTROL_H_
/* Various parameters
*
* Exposure Mode
* MMAL_PARAM_EXPOSUREMODE_OFF,
MMAL_PARAM_EXPOSUREMODE_AUTO,
MMAL_PARAM_EXPOSUREMODE_NIGHT,
MMAL_PARAM_EXPOSUREMODE_NIGHTPREVIEW,
MMAL_PARAM_EXPOSUREMODE_BACKLIGHT,
MMAL_PARAM_EXPOSUREMODE_SPOTLIGHT,
MMAL_PARAM_EXPOSUREMODE_SPORTS,
MMAL_PARAM_EXPOSUREMODE_SNOW,
MMAL_PARAM_EXPOSUREMODE_BEACH,
MMAL_PARAM_EXPOSUREMODE_VERYLONG,
MMAL_PARAM_EXPOSUREMODE_FIXEDFPS,
MMAL_PARAM_EXPOSUREMODE_ANTISHAKE,
MMAL_PARAM_EXPOSUREMODE_FIREWORKS,
*
* AWB Mode
* MMAL_PARAM_AWBMODE_OFF,
MMAL_PARAM_AWBMODE_AUTO,
MMAL_PARAM_AWBMODE_SUNLIGHT,
MMAL_PARAM_AWBMODE_CLOUDY,
MMAL_PARAM_AWBMODE_SHADE,
MMAL_PARAM_AWBMODE_TUNGSTEN,
MMAL_PARAM_AWBMODE_FLUORESCENT,
MMAL_PARAM_AWBMODE_INCANDESCENT,
MMAL_PARAM_AWBMODE_FLASH,
MMAL_PARAM_AWBMODE_HORIZON,
*
* Image FX
MMAL_PARAM_IMAGEFX_NONE,
MMAL_PARAM_IMAGEFX_NEGATIVE,
MMAL_PARAM_IMAGEFX_SOLARIZE,
MMAL_PARAM_IMAGEFX_POSTERIZE,
MMAL_PARAM_IMAGEFX_WHITEBOARD,
MMAL_PARAM_IMAGEFX_BLACKBOARD,
MMAL_PARAM_IMAGEFX_SKETCH,
MMAL_PARAM_IMAGEFX_DENOISE,
MMAL_PARAM_IMAGEFX_EMBOSS,
MMAL_PARAM_IMAGEFX_OILPAINT,
MMAL_PARAM_IMAGEFX_HATCH,
MMAL_PARAM_IMAGEFX_GPEN,
MMAL_PARAM_IMAGEFX_PASTEL,
MMAL_PARAM_IMAGEFX_WATERCOLOUR,
MMAL_PARAM_IMAGEFX_FILM,
MMAL_PARAM_IMAGEFX_BLUR,
MMAL_PARAM_IMAGEFX_SATURATION,
MMAL_PARAM_IMAGEFX_COLOURSWAP,
MMAL_PARAM_IMAGEFX_WASHEDOUT,
MMAL_PARAM_IMAGEFX_POSTERISE,
MMAL_PARAM_IMAGEFX_COLOURPOINT,
MMAL_PARAM_IMAGEFX_COLOURBALANCE,
MMAL_PARAM_IMAGEFX_CARTOON,
*/
// There isn't actually a MMAL structure for the following, so make one
typedef struct
{
int enable; /// Turn colourFX on or off
int u,v; /// U and V to use
} MMAL_PARAM_COLOURFX_T;
typedef struct
{
int enable;
int width,height;
int quality;
} MMAL_PARAM_THUMBNAIL_CONFIG_T;
typedef struct
{
double x;
double y;
double w;
double h;
} PARAM_FLOAT_RECT_T;
/// struct contain camera settings
typedef struct
{
int sharpness; /// -100 to 100
int contrast; /// -100 to 100
int brightness; /// 0 to 100
int saturation; /// -100 to 100
int ISO; /// TODO : what range?
int videoStabilisation; /// 0 or 1 (false or true)
int exposureCompensation; /// -10 to +10 ?
MMAL_PARAM_EXPOSUREMODE_T exposureMode;
MMAL_PARAM_EXPOSUREMETERINGMODE_T exposureMeterMode;
MMAL_PARAM_AWBMODE_T awbMode;
MMAL_PARAM_IMAGEFX_T imageEffect;
MMAL_PARAMETER_IMAGEFX_PARAMETERS_T imageEffectsParameters;
MMAL_PARAM_COLOURFX_T colourEffects;
int rotation; /// 0-359
int hflip; /// 0 or 1
int vflip; /// 0 or 1
PARAM_FLOAT_RECT_T roi; /// region of interest to use on the sensor. Normalised [0,1] values in the rect
} RASPICAM_CAMERA_PARAMETERS;
void raspicamcontrol_check_configuration(int min_gpu_mem);
int raspicamcontrol_parse_cmdline(RASPICAM_CAMERA_PARAMETERS *params, const char *arg1, const char *arg2);
void raspicamcontrol_display_help();
int raspicamcontrol_cycle_test(MMAL_COMPONENT_T *camera);
int raspicamcontrol_set_all_parameters(MMAL_COMPONENT_T *camera, const RASPICAM_CAMERA_PARAMETERS *params);
int raspicamcontrol_get_all_parameters(MMAL_COMPONENT_T *camera, RASPICAM_CAMERA_PARAMETERS *params);
void raspicamcontrol_dump_parameters(const RASPICAM_CAMERA_PARAMETERS *params);
void raspicamcontrol_set_defaults(RASPICAM_CAMERA_PARAMETERS *params);
void raspicamcontrol_check_configuration(int min_gpu_mem);
// Individual setting functions
int raspicamcontrol_set_saturation(MMAL_COMPONENT_T *camera, int saturation);
int raspicamcontrol_set_sharpness(MMAL_COMPONENT_T *camera, int sharpness);
int raspicamcontrol_set_contrast(MMAL_COMPONENT_T *camera, int contrast);
int raspicamcontrol_set_brightness(MMAL_COMPONENT_T *camera, int brightness);
int raspicamcontrol_set_ISO(MMAL_COMPONENT_T *camera, int ISO);
int raspicamcontrol_set_metering_mode(MMAL_COMPONENT_T *camera, MMAL_PARAM_EXPOSUREMETERINGMODE_T mode);
int raspicamcontrol_set_video_stabilisation(MMAL_COMPONENT_T *camera, int vstabilisation);
int raspicamcontrol_set_exposure_compensation(MMAL_COMPONENT_T *camera, int exp_comp);
int raspicamcontrol_set_exposure_mode(MMAL_COMPONENT_T *camera, MMAL_PARAM_EXPOSUREMODE_T mode);
int raspicamcontrol_set_awb_mode(MMAL_COMPONENT_T *camera, MMAL_PARAM_AWBMODE_T awb_mode);
int raspicamcontrol_set_imageFX(MMAL_COMPONENT_T *camera, MMAL_PARAM_IMAGEFX_T imageFX);
int raspicamcontrol_set_colourFX(MMAL_COMPONENT_T *camera, const MMAL_PARAM_COLOURFX_T *colourFX);
int raspicamcontrol_set_rotation(MMAL_COMPONENT_T *camera, int rotation);
int raspicamcontrol_set_flips(MMAL_COMPONENT_T *camera, int hflip, int vflip);
int raspicamcontrol_set_ROI(MMAL_COMPONENT_T *camera, PARAM_FLOAT_RECT_T rect);
//Individual getting functions
int raspicamcontrol_get_saturation(MMAL_COMPONENT_T *camera);
int raspicamcontrol_get_sharpness(MMAL_COMPONENT_T *camera);
int raspicamcontrol_get_contrast(MMAL_COMPONENT_T *camera);
int raspicamcontrol_get_brightness(MMAL_COMPONENT_T *camera);
int raspicamcontrol_get_ISO(MMAL_COMPONENT_T *camera);
MMAL_PARAM_EXPOSUREMETERINGMODE_T raspicamcontrol_get_metering_mode(MMAL_COMPONENT_T *camera);
int raspicamcontrol_get_video_stabilisation(MMAL_COMPONENT_T *camera);
int raspicamcontrol_get_exposure_compensation(MMAL_COMPONENT_T *camera);
MMAL_PARAM_THUMBNAIL_CONFIG_T raspicamcontrol_get_thumbnail_parameters(MMAL_COMPONENT_T *camera);
MMAL_PARAM_EXPOSUREMODE_T raspicamcontrol_get_exposure_mode(MMAL_COMPONENT_T *camera);
MMAL_PARAM_AWBMODE_T raspicamcontrol_get_awb_mode(MMAL_COMPONENT_T *camera);
MMAL_PARAM_IMAGEFX_T raspicamcontrol_get_imageFX(MMAL_COMPONENT_T *camera);
MMAL_PARAM_COLOURFX_T raspicamcontrol_get_colourFX(MMAL_COMPONENT_T *camera);
#endif /* RASPICAMCONTROL_H_ */

1211
sys/rpicamsrc/RaspiCapture.c Normal file

File diff suppressed because it is too large Load diff

View file

@ -0,0 +1,82 @@
/*
* GStreamer
* Copyright (C) 2013 Jan Schmidt <jan@centricular.com>
*
* Permission is hereby granted, free of charge, to any person obtaining a
* copy of this software and associated documentation files (the "Software"),
* to deal in the Software without restriction, including without limitation
* the rights to use, copy, modify, merge, publish, distribute, sublicense,
* and/or sell copies of the Software, and to permit persons to whom the
* Software is furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
* DEALINGS IN THE SOFTWARE.
*
* Alternatively, the contents of this file may be used under the
* GNU Lesser General Public License Version 2.1 (the "LGPL"), in
* which case the following provisions apply instead of the ones
* mentioned above:
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Library General Public
* License as published by the Free Software Foundation; either
* version 2 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Library General Public License for more details.
*
* You should have received a copy of the GNU Library General Public
* License along with this library; if not, write to the
* Free Software Foundation, Inc., 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#ifndef __RASPICAPTURE_H__
#define __RASPICAPTURE_H__
#include <glib.h>
#include <inttypes.h>
#include "interface/mmal/mmal_common.h"
#include "interface/mmal/mmal_types.h"
#include "interface/mmal/mmal_parameters_camera.h"
#include "interface/mmal/mmal_component.h"
#include "RaspiCamControl.h"
#include "RaspiPreview.h"
G_BEGIN_DECLS
/** Structure containing all state information for the current run
*/
typedef struct
{
int timeout; /// Time taken before frame is grabbed and app then shuts down. Units are milliseconds
int width; /// Requested width of image
int height; /// requested height of image
int bitrate; /// Requested bitrate
int framerate; /// Requested frame rate (fps)
int intraperiod; /// Intra-refresh period (key frame rate)
int demoMode; /// Run app in demo mode
int demoInterval; /// Interval between camera settings changes
int immutableInput; /// Flag to specify whether encoder works in place or creates a new buffer. Result is preview can display either
/// the camera output or the encoder output (with compression artifacts)
int profile; /// H264 profile to use for encoding
RASPIPREVIEW_PARAMETERS preview_parameters; /// Preview setup parameters
RASPICAM_CAMERA_PARAMETERS camera_parameters; /// Camera setup parameters
} RASPIVID_STATE;
int raspi_capture_start();
G_END_DECLS
#endif

View file

@ -0,0 +1,286 @@
/*
* Copyright (c) 2013 Jan Schmidt <jan@centricular.com>
Portions:
Copyright (c) 2013, Broadcom Europe Ltd
Copyright (c) 2013, James Hughes
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <memory.h>
#include "interface/vcos/vcos.h"
#include "interface/mmal/mmal.h"
#include "interface/mmal/mmal_logging.h"
#include "interface/mmal/mmal_buffer.h"
#include "interface/mmal/util/mmal_util.h"
#include "interface/mmal/util/mmal_util_params.h"
#include "interface/mmal/util/mmal_default_components.h"
#include "interface/mmal/util/mmal_connection.h"
#include "RaspiPreview.h"
#if 0
#define CommandPreview 1
#define CommandFullScreen 2
#define CommandOpacity 3
#define CommandDisablePreview 4
static COMMAND_LIST cmdline_commands[] =
{
{ CommandPreview, "-preview", "p", "Preview window settings <'x,y,w,h'>", 1 },
{ CommandFullScreen, "-fullscreen", "f", "Fullscreen preview mode", 0 },
{ CommandOpacity, "-opacity", "op", "Preview window opacity (0-255)", 1},
{ CommandDisablePreview,"-nopreview", "n", "Do not display a preview window", 0},
};
static int cmdline_commands_size = sizeof(cmdline_commands) / sizeof(cmdline_commands[0]);
#endif
/**
* Create the preview component, set up its ports
*
* @param state Pointer to state control struct
*
* @return MMAL_SUCCESS if all OK, something else otherwise
*
*/
MMAL_STATUS_T raspipreview_create(RASPIPREVIEW_PARAMETERS *state)
{
MMAL_COMPONENT_T *preview = 0;
MMAL_PORT_T *preview_port = NULL;
MMAL_STATUS_T status;
if (!state->wantPreview)
{
// No preview required, so create a null sink component to take its place
status = mmal_component_create("vc.null_sink", &preview);
if (status != MMAL_SUCCESS)
{
vcos_log_error("Unable to create null sink component");
goto error;
}
}
else
{
status = mmal_component_create(MMAL_COMPONENT_DEFAULT_VIDEO_RENDERER,
&preview);
if (status != MMAL_SUCCESS)
{
vcos_log_error("Unable to create preview component");
goto error;
}
if (!preview->input_num)
{
status = MMAL_ENOSYS;
vcos_log_error("No input ports found on component");
goto error;
}
preview_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 = PREVIEW_LAYER;
param.set |= MMAL_DISPLAY_SET_ALPHA;
param.alpha = state->opacity;
if (state->wantFullScreenPreview)
{
param.set |= MMAL_DISPLAY_SET_FULLSCREEN;
param.fullscreen = 1;
}
else
{
param.set |= (MMAL_DISPLAY_SET_DEST_RECT | MMAL_DISPLAY_SET_FULLSCREEN);
param.fullscreen = 0;
param.dest_rect = state->previewWindow;
}
status = mmal_port_parameter_set(preview_port, &param.hdr);
if (status != MMAL_SUCCESS && status != MMAL_ENOSYS)
{
vcos_log_error("unable to set preview port parameters (%u)", status);
goto error;
}
}
/* Enable component */
status = mmal_component_enable(preview);
if (status != MMAL_SUCCESS)
{
vcos_log_error("Unable to enable preview/null sink component (%u)", status);
goto error;
}
state->preview_component = preview;
return status;
error:
if (preview)
mmal_component_destroy(preview);
return status;
}
/**
* Destroy the preview component
*
* @param state Pointer to state control struct
*
*/
void raspipreview_destroy(RASPIPREVIEW_PARAMETERS *state)
{
if (state->preview_component)
{
mmal_component_destroy(state->preview_component);
state->preview_component = NULL;
}
}
/**
* Assign set of default parameters to the passed in parameter block
*
* @param state Pointer to parameter block
*
*/
void raspipreview_set_defaults(RASPIPREVIEW_PARAMETERS *state)
{
state->wantPreview = 1;
state->wantFullScreenPreview = 1;
state->opacity = 255;
state->previewWindow.x = 0;
state->previewWindow.y = 0;
state->previewWindow.width = 1024;
state->previewWindow.height = 768;
state->preview_component = NULL;
}
/**
* Dump parameters as human readable to stdout
*
* @param state Pointer to parameter block
*
*/
void raspipreview_dump_parameters(RASPIPREVIEW_PARAMETERS *state)
{
fprintf(stderr, "Preview %s, Full screen %s\n", state->wantPreview ? "Yes" : "No",
state->wantFullScreenPreview ? "Yes" : "No");
fprintf(stderr, "Preview window %d,%d,%d,%d\nOpacity %d\n", state->previewWindow.x,
state->previewWindow.y, state->previewWindow.width,
state->previewWindow.height, state->opacity);
};
#if 0
/**
* Parse a possible command pair - command and parameter
* @param arg1 Command
* @param arg2 Parameter (could be NULL)
* @return How many parameters were used, 0,1,2
*/
int raspipreview_parse_cmdline(RASPIPREVIEW_PARAMETERS *params, const char *arg1, const char *arg2)
{
int command_id, used = 0, num_parameters;
if (!arg1)
return 0;
command_id = raspicli_get_command_id(cmdline_commands, cmdline_commands_size, arg1, &num_parameters);
// If invalid command, or we are missing a parameter, drop out
if (command_id==-1 || (command_id != -1 && num_parameters > 0 && arg2 == NULL))
return 0;
switch (command_id)
{
case CommandPreview: // Preview window
{
int tmp;
params->wantPreview = 1;
tmp = sscanf(arg2, "%d,%d,%d,%d",
&params->previewWindow.x, &params->previewWindow.y,
&params->previewWindow.width, &params->previewWindow.height);
// Failed to get any window parameters, so revert to full screen
if (tmp == 0)
params->wantFullScreenPreview = 1;
else
params->wantFullScreenPreview = 0;
used = 2;
break;
}
case CommandFullScreen: // Want full screen preview mode (overrides display rect)
params->wantPreview = 1;
params->wantFullScreenPreview = 1;
used = 1;
break;
case CommandOpacity: // Define preview window opacity
if (sscanf(arg2, "%u", &params->opacity) != 1)
params->opacity = 255;
else
used = 2;
break;
case CommandDisablePreview: // Turn off preview output
params->wantPreview = 0;
used = 1;
break;
}
return used;
}
/**
* Display help for command line options
*/
void raspipreview_display_help()
{
fprintf(stderr, "\nPreview parameter commands\n\n");
raspicli_display_help(cmdline_commands, cmdline_commands_size);
}
#endif

View file

@ -0,0 +1,57 @@
/*
Copyright (c) 2013, Broadcom Europe Ltd
Copyright (c) 2013, James Hughes
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef RASPIPREVIEW_H_
#define RASPIPREVIEW_H_
/// Layer that preview window should be displayed on
#define PREVIEW_LAYER 2
#define PREVIEW_FRAME_RATE_NUM 30
#define PREVIEW_FRAME_RATE_DEN 1
#define FULL_RES_PREVIEW_FRAME_RATE_NUM 15
#define FULL_RES_PREVIEW_FRAME_RATE_DEN 1
typedef struct
{
int wantPreview; /// Display a preview
int wantFullScreenPreview; /// 0 is use previewRect, non-zero to use full screen
int opacity; /// Opacity of window - 0 = transparent, 255 = opaque
MMAL_RECT_T previewWindow; /// Destination rectangle for the preview window.
MMAL_COMPONENT_T *preview_component; /// Pointer to the created preview display component
} RASPIPREVIEW_PARAMETERS;
MMAL_STATUS_T raspipreview_create(RASPIPREVIEW_PARAMETERS *state);
void raspipreview_destroy(RASPIPREVIEW_PARAMETERS *state);
void raspipreview_set_defaults(RASPIPREVIEW_PARAMETERS *state);
void raspipreview_dump_parameters(RASPIPREVIEW_PARAMETERS *state);
int raspipreview_parse_cmdline(RASPIPREVIEW_PARAMETERS *params, const char *arg1, const char *arg2);
void raspipreview_display_help();
#endif /* RASPIPREVIEW_H_ */

1516
sys/rpicamsrc/RaspiStill.c Normal file

File diff suppressed because it is too large Load diff

View file

@ -0,0 +1,957 @@
/*
* Copyright (c) 2013 Jan Schmidt <jan@centricular.com>
Portions:
Copyright (c) 2013, Broadcom Europe Ltd
Copyright (c) 2013, James Hughes
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
/*
* \file RaspiStillYUV.c
* Command line program to capture a still frame and dump uncompressed it to file.
* Also optionally display a preview/viewfinder of current camera input.
*
* \date 4th March 2013
* \Author: James Hughes
*
* Description
*
* 2 components are created; camera and preview.
* Camera component has three ports, preview, video and stills.
* Preview is connected using standard mmal connections, the stills output
* is written straight to the file in YUV 420 format via the requisite buffer
* callback. video port is not used
*
* We use the RaspiCamControl code to handle the specific camera settings.
* We use the RaspiPreview code to handle the generic preview
*/
// We use some GNU extensions (basename)
#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <memory.h>
#include <sysexits.h>
#define VERSION_STRING "v1.3.2"
#include "bcm_host.h"
#include "interface/vcos/vcos.h"
#include "interface/mmal/mmal.h"
#include "interface/mmal/mmal_logging.h"
#include "interface/mmal/mmal_buffer.h"
#include "interface/mmal/util/mmal_util.h"
#include "interface/mmal/util/mmal_util_params.h"
#include "interface/mmal/util/mmal_default_components.h"
#include "interface/mmal/util/mmal_connection.h"
#include "RaspiCamControl.h"
#include "RaspiPreview.h"
#include "RaspiCLI.h"
#include <semaphore.h>
/// Camera number to use - we only have one camera, indexed from 0.
#define CAMERA_NUMBER 0
// Standard port setting for the camera component
#define MMAL_CAMERA_PREVIEW_PORT 0
#define MMAL_CAMERA_VIDEO_PORT 1
#define MMAL_CAMERA_CAPTURE_PORT 2
// Stills format information
#define STILLS_FRAME_RATE_NUM 3
#define STILLS_FRAME_RATE_DEN 1
/// Video render needs at least 2 buffers.
#define VIDEO_OUTPUT_BUFFERS_NUM 3
int mmal_status_to_int(MMAL_STATUS_T status);
/** Structure containing all state information for the current run
*/
typedef struct
{
int timeout; /// Time taken before frame is grabbed and app then shuts down. Units are milliseconds
int width; /// Requested width of image
int height; /// requested height of image
char *filename; /// filename of output file
int verbose; /// !0 if want detailed run information
int timelapse; /// Delay between each picture in timelapse mode. If 0, disable timelapse
int useRGB; /// Output RGB data rather than YUV
RASPIPREVIEW_PARAMETERS preview_parameters; /// Preview setup parameters
RASPICAM_CAMERA_PARAMETERS camera_parameters; /// Camera setup parameters
MMAL_COMPONENT_T *camera_component; /// Pointer to the camera component
MMAL_COMPONENT_T *null_sink_component; /// Pointer to the camera component
MMAL_CONNECTION_T *preview_connection; /// Pointer to the connection from camera to preview
MMAL_POOL_T *camera_pool; /// Pointer to the pool of buffers used by camera stills port
} RASPISTILLYUV_STATE;
/** Struct used to pass information in camera still port userdata to callback
*/
typedef struct
{
FILE *file_handle; /// File handle to write buffer data to.
VCOS_SEMAPHORE_T complete_semaphore; /// semaphore which is posted when we reach end of frame (indicates end of capture or fault)
RASPISTILLYUV_STATE *pstate; /// pointer to our state in case required in callback
} PORT_USERDATA;
static void display_valid_parameters(char *app_name);
/// Comamnd ID's and Structure defining our command line options
#define CommandHelp 0
#define CommandWidth 1
#define CommandHeight 2
#define CommandOutput 3
#define CommandVerbose 4
#define CommandTimeout 5
#define CommandTimelapse 6
#define CommandUseRGB 7
static COMMAND_LIST cmdline_commands[] =
{
{ CommandHelp, "-help", "?", "This help information", 0 },
{ CommandWidth, "-width", "w", "Set image width <size>", 1 },
{ CommandHeight, "-height", "h", "Set image height <size>", 1 },
{ CommandOutput, "-output", "o", "Output filename <filename>. If not specifed, no image is saved", 1 },
{ CommandVerbose, "-verbose", "v", "Output verbose information during run", 0 },
{ CommandTimeout, "-timeout", "t", "Time (in ms) before takes picture and shuts down. If not specified set to 5s", 1 },
{ CommandTimelapse,"-timelapse", "tl", "Timelapse mode. Takes a picture every <t>ms", 1},
{ CommandUseRGB, "-rgb", "rgb","Save as RGB data rather than YUV", 0},
};
static int cmdline_commands_size = sizeof(cmdline_commands) / sizeof(cmdline_commands[0]);
/**
* Assign a default set of parameters to the state passed in
*
* @param state Pointer to state structure to assign defaults to
*/
static void default_status(RASPISTILLYUV_STATE *state)
{
if (!state)
{
vcos_assert(0);
return;
}
// Default everything to zero
memset(state, 0, sizeof(RASPISTILLYUV_STATE));
// Now set anything non-zero
state->timeout = 5000; // 5s delay before take image
state->width = 2592;
state->height = 1944;
state->timelapse = 0;
// Setup preview window defaults
raspipreview_set_defaults(&state->preview_parameters);
// Set up the camera_parameters to default
raspicamcontrol_set_defaults(&state->camera_parameters);
}
/**
* Dump image state parameters to stderr. Used for debugging
*
* @param state Pointer to state structure to assign defaults to
*/
static void dump_status(RASPISTILLYUV_STATE *state)
{
if (!state)
{
vcos_assert(0);
return;
}
fprintf(stderr, "Width %d, Height %d, filename %s\n", state->width, state->height, state->filename);
fprintf(stderr, "Time delay %d, Timelapse %d\n", state->timeout, state->timelapse);
raspipreview_dump_parameters(&state->preview_parameters);
raspicamcontrol_dump_parameters(&state->camera_parameters);
}
/**
* Parse the incoming command line and put resulting parameters in to the state
*
* @param argc Number of arguments in command line
* @param argv Array of pointers to strings from command line
* @param state Pointer to state structure to assign any discovered parameters to
* @return non-0 if failed for some reason, 0 otherwise
*/
static int parse_cmdline(int argc, const char **argv, RASPISTILLYUV_STATE *state)
{
// Parse the command line arguments.
// We are looking for --<something> or -<abreviation of something>
int valid = 1; // set 0 if we have a bad parameter
int i;
for (i = 1; i < argc && valid; i++)
{
int command_id, num_parameters;
if (!argv[i])
continue;
if (argv[i][0] != '-')
{
valid = 0;
continue;
}
// Assume parameter is valid until proven otherwise
valid = 1;
command_id = raspicli_get_command_id(cmdline_commands, cmdline_commands_size, &argv[i][1], &num_parameters);
// If we found a command but are missing a parameter, continue (and we will drop out of the loop)
if (command_id != -1 && num_parameters > 0 && (i + 1 >= argc) )
continue;
// We are now dealing with a command line option
switch (command_id)
{
case CommandHelp:
display_valid_parameters(basename(argv[0]));
return -1;
case CommandWidth: // Width > 0
if (sscanf(argv[i + 1], "%u", &state->width) != 1)
valid = 0;
else
i++;
break;
case CommandHeight: // Height > 0
if (sscanf(argv[i + 1], "%u", &state->height) != 1)
valid = 0;
else
i++;
break;
case CommandOutput: // output filename
{
int len = strlen(argv[i + 1]);
if (len)
{
state->filename = malloc(len + 1);
vcos_assert(state->filename);
if (state->filename)
strncpy(state->filename, argv[i + 1], len);
i++;
}
else
valid = 0;
break;
}
case CommandVerbose: // display lots of data during run
state->verbose = 1;
break;
case CommandTimeout: // Time to run viewfinder for before taking picture, in seconds
{
if (sscanf(argv[i + 1], "%u", &state->timeout) == 1)
{
// TODO : What limits do we need for timeout?
i++;
}
else
valid = 0;
break;
}
case CommandTimelapse:
if (sscanf(argv[i + 1], "%u", &state->timelapse) != 1)
valid = 0;
else
i++;
break;
case CommandUseRGB: // display lots of data during run
state->useRGB = 1;
break;
default:
{
// Try parsing for any image specific parameters
// result indicates how many parameters were used up, 0,1,2
// but we adjust by -1 as we have used one already
const char *second_arg = (i + 1 < argc) ? argv[i + 1] : NULL;
int parms_used = (raspicamcontrol_parse_cmdline(&state->camera_parameters, &argv[i][1], second_arg));
// Still unused, try preview options
if (!parms_used)
parms_used = raspipreview_parse_cmdline(&state->preview_parameters, &argv[i][1], second_arg);
// If no parms were used, this must be a bad parameters
if (!parms_used)
valid = 0;
else
i += parms_used - 1;
break;
}
}
}
if (!valid)
{
fprintf(stderr, "Invalid command line option (%s)\n", argv[i]);
return 1;
}
return 0;
}
/**
* Display usage information for the application to stdout
*
* @param app_name String to display as the application name
*
*/
static void display_valid_parameters(char *app_name)
{
fprintf(stderr, "Runs camera for specific time, and take uncompressed YUV capture at end if requested\n\n");
fprintf(stderr, "usage: %s [options]\n\n", app_name);
fprintf(stderr, "Image parameter commands\n\n");
raspicli_display_help(cmdline_commands, cmdline_commands_size);
// Help for preview options
raspipreview_display_help();
// Now display any help information from the camcontrol code
raspicamcontrol_display_help();
fprintf(stderr, "\n");
return;
}
/**
* buffer header callback function for camera control
*
* @param port Pointer to port from which callback originated
* @param buffer mmal buffer header pointer
*/
static void camera_control_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
{
fprintf(stderr, "Camera control callback cmd=0x%08x", buffer->cmd);
if (buffer->cmd == MMAL_EVENT_PARAMETER_CHANGED)
{
}
else
{
vcos_log_error("Received unexpected camera control callback event, 0x%08x", buffer->cmd);
}
mmal_buffer_header_release(buffer);
}
/**
* buffer header callback function for camera output port
*
* Callback will dump buffer data to the specific file
*
* @param port Pointer to port from which callback originated
* @param buffer mmal buffer header pointer
*/
static void camera_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
{
int complete = 0;
// We pass our file handle and other stuff in via the userdata field.
PORT_USERDATA *pData = (PORT_USERDATA *)port->userdata;
if (pData)
{
int bytes_written = buffer->length;
if (buffer->length)
{
mmal_buffer_header_mem_lock(buffer);
bytes_written = fwrite(buffer->data, 1, buffer->length, pData->file_handle);
mmal_buffer_header_mem_unlock(buffer);
}
// We need to check we wrote what we wanted - it's possible we have run out of storage.
if (bytes_written != buffer->length)
{
vcos_log_error("Unable to write buffer to file - aborting");
complete = 1;
}
// Check end of frame or error
if (buffer->flags & (MMAL_BUFFER_HEADER_FLAG_FRAME_END | MMAL_BUFFER_HEADER_FLAG_TRANSMISSION_FAILED))
complete = 1;
}
else
{
vcos_log_error("Received a camera still buffer callback with no state");
}
// release buffer back to the pool
mmal_buffer_header_release(buffer);
// and send one back to the port (if still open)
if (port->is_enabled)
{
MMAL_STATUS_T status;
MMAL_BUFFER_HEADER_T *new_buffer = mmal_queue_get(pData->pstate->camera_pool->queue);
// and back to the port from there.
if (new_buffer)
{
status = mmal_port_send_buffer(port, new_buffer);
}
if (!new_buffer || status != MMAL_SUCCESS)
vcos_log_error("Unable to return the buffer to the camera still port");
}
if (complete)
{
vcos_semaphore_post(&(pData->complete_semaphore));
}
}
/**
* Create the camera component, set up its ports
*
* @param state Pointer to state control struct
*
* @return 0 if failed, pointer to component if successful
*
*/
static MMAL_STATUS_T create_camera_component(RASPISTILLYUV_STATE *state)
{
MMAL_COMPONENT_T *camera = 0;
MMAL_ES_FORMAT_T *format;
MMAL_PORT_T *preview_port = NULL, *video_port = NULL, *still_port = NULL;
MMAL_STATUS_T status;
MMAL_POOL_T *pool;
/* Create the component */
status = mmal_component_create(MMAL_COMPONENT_DEFAULT_CAMERA, &camera);
if (status != MMAL_SUCCESS)
{
vcos_log_error("Failed to create camera component");
goto error;
}
if (!camera->output_num)
{
vcos_log_error("Camera doesn't have output ports");
goto error;
}
preview_port = camera->output[MMAL_CAMERA_PREVIEW_PORT];
video_port = camera->output[MMAL_CAMERA_VIDEO_PORT];
still_port = camera->output[MMAL_CAMERA_CAPTURE_PORT];
// Enable the camera, and tell it its control callback function
status = mmal_port_enable(camera->control, camera_control_callback);
if (status)
{
vcos_log_error("Unable to enable control port : error %d", status);
goto error;
}
// set up the camera configuration
{
MMAL_PARAMETER_CAMERA_CONFIG_T cam_config =
{
{ MMAL_PARAMETER_CAMERA_CONFIG, sizeof(cam_config) },
.max_stills_w = state->width,
.max_stills_h = state->height,
.stills_yuv422 = 0,
.one_shot_stills = 1,
.max_preview_video_w = state->preview_parameters.previewWindow.width,
.max_preview_video_h = state->preview_parameters.previewWindow.height,
.num_preview_video_frames = 3,
.stills_capture_circular_buffer_height = 0,
.fast_preview_resume = 0,
.use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RESET_STC
};
mmal_port_parameter_set(camera->control, &cam_config.hdr);
}
raspicamcontrol_set_all_parameters(camera, &state->camera_parameters);
// Now set up the port formats
format = preview_port->format;
format->encoding = MMAL_ENCODING_OPAQUE;
format->encoding_variant = MMAL_ENCODING_I420;
format->es->video.width = state->preview_parameters.previewWindow.width;
format->es->video.height = state->preview_parameters.previewWindow.height;
format->es->video.crop.x = 0;
format->es->video.crop.y = 0;
format->es->video.crop.width = state->preview_parameters.previewWindow.width;
format->es->video.crop.height = state->preview_parameters.previewWindow.height;
format->es->video.frame_rate.num = PREVIEW_FRAME_RATE_NUM;
format->es->video.frame_rate.den = PREVIEW_FRAME_RATE_DEN;
status = mmal_port_format_commit(preview_port);
if (status)
{
vcos_log_error("camera viewfinder format couldn't be set");
goto error;
}
// Set the same format on the video port (which we dont use here)
mmal_format_full_copy(video_port->format, format);
status = mmal_port_format_commit(video_port);
if (status)
{
vcos_log_error("camera video format couldn't be set");
goto error;
}
// Ensure there are enough buffers to avoid dropping frames
if (video_port->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM)
video_port->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM;
format = still_port->format;
// Set our stills format on the stills port
if (state->useRGB)
{
format->encoding = MMAL_ENCODING_BGR24;
format->encoding_variant = MMAL_ENCODING_BGR24;
}
else
{
format->encoding = MMAL_ENCODING_I420;
format->encoding_variant = MMAL_ENCODING_I420;
}
format->es->video.width = state->width;
format->es->video.height = state->height;
format->es->video.crop.x = 0;
format->es->video.crop.y = 0;
format->es->video.crop.width = state->width;
format->es->video.crop.height = state->height;
format->es->video.frame_rate.num = STILLS_FRAME_RATE_NUM;
format->es->video.frame_rate.den = STILLS_FRAME_RATE_DEN;
if (still_port->buffer_size < still_port->buffer_size_min)
still_port->buffer_size = still_port->buffer_size_min;
still_port->buffer_num = still_port->buffer_num_recommended;
status = mmal_port_format_commit(still_port);
if (status)
{
vcos_log_error("camera still format couldn't be set");
goto error;
}
/* Enable component */
status = mmal_component_enable(camera);
if (status)
{
vcos_log_error("camera component couldn't be enabled");
goto error;
}
/* Create pool of buffer headers for the output port to consume */
pool = mmal_port_pool_create(still_port, still_port->buffer_num, still_port->buffer_size);
if (!pool)
{
vcos_log_error("Failed to create buffer header pool for camera still port %s", still_port->name);
}
state->camera_pool = pool;
state->camera_component = camera;
if (state->verbose)
fprintf(stderr, "Camera component done\n");
return status;
error:
if (camera)
mmal_component_destroy(camera);
return status;
}
/**
* Destroy the camera component
*
* @param state Pointer to state control struct
*
*/
static void destroy_camera_component(RASPISTILLYUV_STATE *state)
{
if (state->camera_component)
{
mmal_component_destroy(state->camera_component);
state->camera_component = NULL;
}
}
/**
* Connect two specific ports together
*
* @param output_port Pointer the output port
* @param input_port Pointer the input port
* @param Pointer to a mmal connection pointer, reassigned if function successful
* @return Returns a MMAL_STATUS_T giving result of operation
*
*/
static MMAL_STATUS_T connect_ports(MMAL_PORT_T *output_port, MMAL_PORT_T *input_port, MMAL_CONNECTION_T **connection)
{
MMAL_STATUS_T status;
status = mmal_connection_create(connection, output_port, input_port, MMAL_CONNECTION_FLAG_TUNNELLING | MMAL_CONNECTION_FLAG_ALLOCATION_ON_INPUT);
if (status == MMAL_SUCCESS)
{
status = mmal_connection_enable(*connection);
if (status != MMAL_SUCCESS)
mmal_connection_destroy(*connection);
}
return status;
}
/**
* Checks if specified port is valid and enabled, then disables it
*
* @param port Pointer the port
*
*/
static void check_disable_port(MMAL_PORT_T *port)
{
if (port && port->is_enabled)
mmal_port_disable(port);
}
/**
* Handler for sigint signals
*
* @param signal_number ID of incoming signal.
*
*/
static void signal_handler(int signal_number)
{
// Going to abort on all signals
vcos_log_error("Aborting program\n");
// Need to close any open stuff...
exit(255);
}
/**
* main
*/
int main(int argc, const char **argv)
{
// Our main data storage vessel..
RASPISTILLYUV_STATE state;
int exit_code = EX_OK;
MMAL_STATUS_T status = MMAL_SUCCESS;
MMAL_PORT_T *camera_preview_port = NULL;
MMAL_PORT_T *camera_video_port = NULL;
MMAL_PORT_T *camera_still_port = NULL;
MMAL_PORT_T *preview_input_port = NULL;
FILE *output_file = NULL;
bcm_host_init();
// Register our application with the logging system
vcos_log_register("RaspiStill", VCOS_LOG_CATEGORY);
signal(SIGINT, signal_handler);
// Do we have any parameters
if (argc == 1)
{
fprintf(stderr, "\n%s Camera App %s\n\n", basename(argv[0]), VERSION_STRING);
display_valid_parameters(basename(argv[0]));
exit(EX_USAGE);
}
default_status(&state);
// Parse the command line and put options in to our status structure
if (parse_cmdline(argc, argv, &state))
{
status = -1;
exit(EX_USAGE);
}
if (state.verbose)
{
fprintf(stderr, "\n%s Camera App %s\n\n", basename(argv[0]), VERSION_STRING);
dump_status(&state);
}
// OK, we have a nice set of parameters. Now set up our components
// We have two components. Camera and Preview
// Camera is different in stills/video, but preview
// is the same so handed off to a separate module
if ((status = create_camera_component(&state)) != MMAL_SUCCESS)
{
vcos_log_error("%s: Failed to create camera component", __func__);
exit_code = EX_SOFTWARE;
}
else if ((status = raspipreview_create(&state.preview_parameters)) != MMAL_SUCCESS)
{
vcos_log_error("%s: Failed to create preview component", __func__);
destroy_camera_component(&state);
exit_code = EX_SOFTWARE;
}
else
{
PORT_USERDATA callback_data;
if (state.verbose)
fprintf(stderr, "Starting component connection stage\n");
camera_preview_port = state.camera_component->output[MMAL_CAMERA_PREVIEW_PORT];
camera_video_port = state.camera_component->output[MMAL_CAMERA_VIDEO_PORT];
camera_still_port = state.camera_component->output[MMAL_CAMERA_CAPTURE_PORT];
// Note we are lucky that the preview and null sink components use the same input port
// so we can simple do this without conditionals
preview_input_port = state.preview_parameters.preview_component->input[0];
// Connect camera to preview (which might be a null_sink if no preview required)
status = connect_ports(camera_preview_port, preview_input_port, &state.preview_connection);
if (status == MMAL_SUCCESS)
{
VCOS_STATUS_T vcos_status;
if (state.filename)
{
if (state.verbose)
fprintf(stderr, "Opening output file %s\n", state.filename);
output_file = fopen(state.filename, "wb");
if (!output_file)
{
// Notify user, carry on but discarding output buffers
vcos_log_error("%s: Error opening output file: %s\nNo output file will be generated\n", __func__, state.filename);
}
}
// Set up our userdata - this is passed though to the callback where we need the information.
callback_data.file_handle = output_file;
callback_data.pstate = &state;
vcos_status = vcos_semaphore_create(&callback_data.complete_semaphore, "RaspiStill-sem", 0);
vcos_assert(vcos_status == VCOS_SUCCESS);
camera_still_port->userdata = (struct MMAL_PORT_USERDATA_T *)&callback_data;
if (state.verbose)
fprintf(stderr, "Enabling camera still output port\n");
// Enable the camera still output port and tell it its callback function
status = mmal_port_enable(camera_still_port, camera_buffer_callback);
if (status != MMAL_SUCCESS)
{
vcos_log_error("Failed to setup camera output");
goto error;
}
if (state.verbose)
fprintf(stderr, "Starting video preview\n");
int num_iterations = state.timelapse ? state.timeout / state.timelapse : 1;
int frame;
FILE *output_file = NULL;
for (frame = 1;frame<=num_iterations; frame++)
{
if (state.timelapse)
vcos_sleep(state.timelapse);
else
vcos_sleep(state.timeout);
// Open the file
if (state.filename)
{
if (state.filename[0] == '-')
{
output_file = stdout;
// Ensure we don't upset the output stream with diagnostics/info
state.verbose = 0;
}
else
{
char *use_filename = state.filename;
if (state.timelapse)
asprintf(&use_filename, state.filename, frame);
if (state.verbose)
fprintf(stderr, "Opening output file %s\n", use_filename);
output_file = fopen(use_filename, "wb");
if (!output_file)
{
// Notify user, carry on but discarding encoded output buffers
vcos_log_error("%s: Error opening output file: %s\nNo output file will be generated\n", __func__, use_filename);
}
// asprintf used in timelapse mode allocates its own memory which we need to free
if (state.timelapse)
free(use_filename);
}
callback_data.file_handle = output_file;
}
// And only do the capture if we have specified a filename and its opened OK
if (output_file)
{
// Send all the buffers to the camera output port
{
int num = mmal_queue_length(state.camera_pool->queue);
int q;
for (q=0;q<num;q++)
{
MMAL_BUFFER_HEADER_T *buffer = mmal_queue_get(state.camera_pool->queue);
if (!buffer)
vcos_log_error("Unable to get a required buffer %d from pool queue", q);
if (mmal_port_send_buffer(camera_still_port, buffer)!= MMAL_SUCCESS)
vcos_log_error("Unable to send a buffer to camera output port (%d)", q);
}
}
if (state.verbose)
fprintf(stderr, "Starting capture %d\n", frame);
// Fire the capture
if (mmal_port_parameter_set_boolean(camera_still_port, MMAL_PARAMETER_CAPTURE, 1) != MMAL_SUCCESS)
{
vcos_log_error("%s: Failed to start capture", __func__);
}
else
{
// Wait for capture to complete
// For some reason using vcos_semaphore_wait_timeout sometimes returns immediately with bad parameter error
// even though it appears to be all correct, so reverting to untimed one until figure out why its erratic
vcos_semaphore_wait(&callback_data.complete_semaphore);
if (state.verbose)
fprintf(stderr, "Finished capture %d\n", frame);
}
// Ensure we don't die if get callback with no open file
callback_data.file_handle = NULL;
if (output_file != stdout)
fclose(output_file);
}
}
vcos_semaphore_delete(&callback_data.complete_semaphore);
}
else
{
mmal_status_to_int(status);
vcos_log_error("%s: Failed to connect camera to preview", __func__);
}
error:
mmal_status_to_int(status);
if (state.verbose)
fprintf(stderr, "Closing down\n");
if (output_file)
fclose(output_file);
// Disable all our ports that are not handled by connections
check_disable_port(camera_video_port);
mmal_connection_destroy(state.preview_connection);
/* Disable components */
if (state.preview_parameters.preview_component)
mmal_component_disable(state.preview_parameters.preview_component);
if (state.camera_component)
mmal_component_disable(state.camera_component);
raspipreview_destroy(&state.preview_parameters);
destroy_camera_component(&state);
if (state.verbose)
fprintf(stderr, "Close down completed, all components disconnected, disabled and destroyed\n\n");
}
if (status != MMAL_SUCCESS)
raspicamcontrol_check_configuration(128);
return exit_code;
}

View file

@ -62,6 +62,7 @@
#include <gst/video/video.h>
#include "gstrpicamsrc.h"
#include "RaspiCapture.h"
#include "bcm_host.h"
#include "interface/vcos/vcos.h"
@ -95,7 +96,6 @@ enum
"width = " GST_VIDEO_SIZE_RANGE "," \
"height = " GST_VIDEO_SIZE_RANGE "," \
"framerate = " GST_VIDEO_FPS_RANGE
#define H264_CAPS \
"video/x-h264, " \
"width = " GST_VIDEO_SIZE_RANGE ", " \
@ -106,49 +106,34 @@ enum
"profile = (string) { baseline, main, high }"
static GstStaticPadTemplate video_src_template =
GST_STATIC_PAD_TEMPLATE ("vidsrc",
GST_STATIC_PAD_TEMPLATE ("src",
GST_PAD_SRC,
GST_PAD_ALWAYS,
GST_STATIC_CAPS (RAW_AND_JPEG_CAPS "; " H264_CAPS)
);
static GstStaticPadTemplate viewfind_src_template =
GST_STATIC_PAD_TEMPLATE ("vfsrc",
GST_PAD_SRC,
GST_PAD_ALWAYS,
GST_STATIC_CAPS (RAW_AND_JPEG_CAPS "; " H264_CAPS)
);
static GstStaticPadTemplate image_src_template =
GST_STATIC_PAD_TEMPLATE ("imgsrc",
GST_PAD_SRC,
GST_PAD_ALWAYS,
GST_STATIC_CAPS (RAW_AND_JPEG_CAPS)
);
#define gst_rpi_cam_src_parent_class parent_class
G_DEFINE_TYPE (GstRpiCamSrc, gst_rpi_cam_src, GST_TYPE_BASE_CAMERA_SRC);
G_DEFINE_TYPE (GstRpiCamSrc, gst_rpi_cam_src, GST_TYPE_PUSH_SRC);
static void gst_rpi_cam_src_set_property (GObject * object, guint prop_id,
const GValue * value, GParamSpec * pspec);
static void gst_rpi_cam_src_get_property (GObject * object, guint prop_id,
GValue * value, GParamSpec * pspec);
static gboolean
gst_rpi_cam_src_setup_pipeline (GstBaseCameraSrc *parent)
{
GstRpiCamSrc *self = GST_RPICAMSRC(parent);
g_print ("In setup_pipeline\n");
}
static gboolean gst_rpi_cam_src_start (GstBaseSrc *parent);
static GstFlowReturn gst_rpi_cam_src_fill_buffer (GstPushSrc *parent, GstBuffer *buf);
static void
gst_rpi_cam_src_class_init (GstRpiCamSrcClass * klass)
{
GObjectClass *gobject_class;
GstElementClass *gstelement_class;
GstBaseCameraSrcClass *basecamsrc_class;
GstBaseSrcClass *basesrc_class;
GstPushSrcClass *pushsrc_class;
gobject_class = (GObjectClass *) klass;
gstelement_class = (GstElementClass *) klass;
basecamsrc_class = (GstBaseCameraSrcClass *) klass;
basesrc_class = (GstBaseSrcClass *) klass;
pushsrc_class = (GstPushSrcClass *) klass;
gobject_class->set_property = gst_rpi_cam_src_set_property;
gobject_class->get_property = gst_rpi_cam_src_get_property;
@ -159,19 +144,18 @@ gst_rpi_cam_src_class_init (GstRpiCamSrcClass * klass)
"Raspberry Pi camera module source",
"Jan Schmidt <jan@centricular.com>");
gst_element_class_add_pad_template (gstelement_class,
gst_static_pad_template_get (&viewfind_src_template));
gst_element_class_add_pad_template (gstelement_class,
gst_static_pad_template_get (&video_src_template));
gst_element_class_add_pad_template (gstelement_class,
gst_static_pad_template_get (&image_src_template));
basecamsrc_class->setup_pipeline = gst_rpi_cam_src_setup_pipeline;
basesrc_class->start = GST_DEBUG_FUNCPTR(gst_rpi_cam_src_start);
pushsrc_class->fill = GST_DEBUG_FUNCPTR(gst_rpi_cam_src_fill_buffer);
}
static void
gst_rpi_cam_src_init (GstRpiCamSrc *src)
{
gst_base_src_set_format (GST_BASE_SRC (src), GST_FORMAT_TIME);
gst_base_src_set_live (GST_BASE_SRC (src), TRUE);
}
static void
@ -210,6 +194,21 @@ rpicamsrc_init (GstPlugin * rpicamsrc)
GST_TYPE_RPICAMSRC);
}
static gboolean
gst_rpi_cam_src_start (GstBaseSrc *parent)
{
GstRpiCamSrc *src = GST_RPICAMSRC(parent);
g_print ("In start()\n");
raspi_capture_start();
return TRUE;
}
static GstFlowReturn
gst_rpi_cam_src_fill_buffer (GstPushSrc *parent, GstBuffer *buf)
{
return GST_FLOW_ERROR;
}
#ifndef PACKAGE
#define PACKAGE "gstrpicamsrc"
#endif

View file

@ -45,7 +45,7 @@
#define __GST_RPICAMSRC_H__
#include <gst/gst.h>
#include <gst/basecamerabinsrc/gstbasecamerasrc.h>
#include <gst/base/gstpushsrc.h>
G_BEGIN_DECLS
@ -64,16 +64,14 @@ typedef struct _GstRpiCamSrcClass GstRpiCamSrcClass;
struct _GstRpiCamSrc
{
GstBaseCameraSrc parent;
GstPushSrc parent;
GstPad *viewfind_srcpad;
GstPad *video_srcpad;
GstPad *image_srcpad;
};
struct _GstRpiCamSrcClass
{
GstBaseCameraSrcClass parent_class;
GstPushSrcClass parent_class;
};
GType gst_rpi_cam_src_get_type (void);