MmalVideoCamera::MmalVideoCamera(int imageWidth, int imageHeight, int frameRate) :
m_imageWidth(imageWidth),
m_imageHeight(imageHeight),
m_frameRate(frameRate),
m_imagePreviewWidth(VCOS_ALIGN_UP(imageWidth, 32)),
m_imagePreviewHeight(VCOS_ALIGN_UP(imageHeight, 16)),
m_callbackData(NULL),
m_cs(),
m_camera(NULL),
m_pool(NULL),
m_videoPort(NULL),
m_stillPort(NULL),
m_previewPort(NULL),
m_videoPortEnabled(false)
{
std::cout << "Creating callback data..." << std::endl;
m_callbackData = new MmalVideoCallbackData(m_imageWidth, m_imageHeight, 3);
std::cout << "Creating camera..." << std::endl;
createCameraComponent();
std::cout << "Created camera" << std::endl;
std::cout << "Target Image Width: " << m_imageWidth << std::endl;
std::cout << "Target Image Height: " << m_imageHeight << std::endl;
createBufferPool();
std::cout << "Initialized camera" << std::endl;
}
void set_port_default_format(MMAL_ES_FORMAT_T *port_fmt){
//port_fmt->encoding = MMAL_ENCODING_OPAQUE;
port_fmt->encoding = MMAL_ENCODING_I420;
port_fmt->encoding_variant = MMAL_ENCODING_I420;
port_fmt->es->video.width = VCOS_ALIGN_UP(WIDTH, 32);
port_fmt->es->video.height = VCOS_ALIGN_UP(HEIGHT, 16);
port_fmt->es->video.crop.x = 0;
port_fmt->es->video.crop.y = 0;
port_fmt->es->video.crop.width = WIDTH;
port_fmt->es->video.crop.height = HEIGHT;
port_fmt->es->video.frame_rate.den = 1;
port_fmt->es->video.frame_rate.num = 0;
}
uint32_t mmal_encoding_width_to_stride(uint32_t encoding, uint32_t width)
{
unsigned int i;
for(i = 0; pixel_pitch[i].encoding != MMAL_ENCODING_UNKNOWN; i++)
if(pixel_pitch[i].encoding == encoding) break;
if(pixel_pitch[i].encoding == MMAL_ENCODING_UNKNOWN)
return 0;
return VCOS_ALIGN_UP(pixel_pitch[i].pitch_num * width / pixel_pitch[i].pitch_den, pixel_pitch[i].alignment);
}
/**
* 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;
}
MMAL_PARAMETER_INT32_T camera_num =
{{MMAL_PARAMETER_CAMERA_NUM, sizeof(camera_num)}, state->cameraNum};
status = mmal_port_parameter_set(camera->control, &camera_num.hdr);
if (status != MMAL_SUCCESS)
{
vcos_log_error("Could not select camera : error %d", status);
goto error;
}
if (!camera->output_num)
{
status = MMAL_ENOSYS;
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];
if (state->settings)
{
MMAL_PARAMETER_CHANGE_EVENT_REQUEST_T change_event_request =
{{MMAL_PARAMETER_CHANGE_EVENT_REQUEST, sizeof(MMAL_PARAMETER_CHANGE_EVENT_REQUEST_T)},
MMAL_PARAMETER_CAMERA_SETTINGS, 1};
status = mmal_port_parameter_set(camera->control, &change_event_request.hdr);
if ( status != MMAL_SUCCESS )
{
vcos_log_error("No camera settings events");
}
}
// Enable the camera, and tell it its control callback function
status = mmal_port_enable(camera->control, camera_control_callback);
if (status != MMAL_SUCCESS )
{
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
};
if (state->fullResPreview)
{
cam_config.max_preview_video_w = state->width;
cam_config.max_preview_video_h = state->height;
}
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;
if(state->camera_parameters.shutter_speed > 6000000)
{
MMAL_PARAMETER_FPS_RANGE_T fps_range = {{MMAL_PARAMETER_FPS_RANGE, sizeof(fps_range)},
{ 50, 1000 }, {166, 1000}};
mmal_port_parameter_set(preview_port, &fps_range.hdr);
}
else if(state->camera_parameters.shutter_speed > 1000000)
{
MMAL_PARAMETER_FPS_RANGE_T fps_range = {{MMAL_PARAMETER_FPS_RANGE, sizeof(fps_range)},
{ 166, 1000 }, {999, 1000}};
mmal_port_parameter_set(preview_port, &fps_range.hdr);
}
if (state->fullResPreview)
{
// In this mode we are forcing the preview to be generated from the full capture resolution.
// This runs at a max of 15fps with the OV5647 sensor.
format->es->video.width = VCOS_ALIGN_UP(state->width, 32);
format->es->video.height = VCOS_ALIGN_UP(state->height, 16);
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 = FULL_RES_PREVIEW_FRAME_RATE_NUM;
format->es->video.frame_rate.den = FULL_RES_PREVIEW_FRAME_RATE_DEN;
}
else
{
// Use a full FOV 4:3 mode
format->es->video.width = VCOS_ALIGN_UP(state->preview_parameters.previewWindow.width, 32);
format->es->video.height = VCOS_ALIGN_UP(state->preview_parameters.previewWindow.height, 16);
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 != MMAL_SUCCESS )
{
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 != MMAL_SUCCESS )
{
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;
if(state->camera_parameters.shutter_speed > 6000000)
{
MMAL_PARAMETER_FPS_RANGE_T fps_range = {{MMAL_PARAMETER_FPS_RANGE, sizeof(fps_range)},
{ 50, 1000 }, {166, 1000}};
mmal_port_parameter_set(still_port, &fps_range.hdr);
}
else if(state->camera_parameters.shutter_speed > 1000000)
{
MMAL_PARAMETER_FPS_RANGE_T fps_range = {{MMAL_PARAMETER_FPS_RANGE, sizeof(fps_range)},
{ 167, 1000 }, {999, 1000}};
mmal_port_parameter_set(still_port, &fps_range.hdr);
}
// 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 = VCOS_ALIGN_UP(state->width, 32);
format->es->video.height = VCOS_ALIGN_UP(state->height, 16);
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 != MMAL_SUCCESS )
{
vcos_log_error("camera still format couldn't be set");
goto error;
}
/* Enable component */
status = mmal_component_enable(camera);
if (status != MMAL_SUCCESS )
{
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;
}
void MmalVideoCamera::createCameraComponent()
{
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;
try
{
status = mmal_component_create(MMAL_COMPONENT_DEFAULT_CAMERA, &camera);
if (status != MMAL_SUCCESS)
{
throw Exception("Failed to create camera component");
}
if (camera->output_num == 0)
{
status = MMAL_ENOSYS;
throw Exception("Camera doesn't have output ports");
}
MMAL_CHECK(mmal_port_parameter_set_uint32(camera->control, MMAL_PARAMETER_CAMERA_CUSTOM_SENSOR_CONFIG, MMAL_VIDEO_SENSOR_MODE));
preview_port = camera->output[MMAL_CAMERA_PREVIEW_PORT];
video_port = camera->output[MMAL_CAMERA_VIDEO_PORT];
still_port = camera->output[MMAL_CAMERA_CAPTURE_PORT];
// Populate the camera configuration struct
MMAL_PARAMETER_CAMERA_CONFIG_T cameraConfig;
cameraConfig.hdr.id = MMAL_PARAMETER_CAMERA_CONFIG;
cameraConfig.hdr.size = sizeof(cameraConfig);
cameraConfig.max_stills_w = m_imageWidth;
cameraConfig.max_stills_h = m_imageHeight;
cameraConfig.stills_yuv422 = 0;
cameraConfig.one_shot_stills = 0;
cameraConfig.max_preview_video_w = m_imagePreviewWidth;
cameraConfig.max_preview_video_h = m_imagePreviewHeight;
cameraConfig.num_preview_video_frames = 3;
cameraConfig.stills_capture_circular_buffer_height = 0;
cameraConfig.fast_preview_resume = 0;
cameraConfig.use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RESET_STC;
if (mmal_port_parameter_set(camera->control, &cameraConfig.hdr) != MMAL_SUCCESS)
{
throw Exception("Error calling mmal_port_parameter_set()");
}
//
// Setup the video port
//
format = video_port->format;
format->encoding = MMAL_ENCODING_BGR24;
format->encoding_variant = MMAL_ENCODING_BGR24;
format->es->video.width = VCOS_ALIGN_UP(m_imageWidth, 32);
format->es->video.height = VCOS_ALIGN_UP(m_imageHeight, 16);
format->es->video.crop.x = 0;
format->es->video.crop.y = 0;
format->es->video.crop.width = m_imageWidth;
format->es->video.crop.height = m_imageHeight;
format->es->video.frame_rate.num = m_frameRate;
format->es->video.frame_rate.den = FULL_RES_VIDEO_FRAME_RATE_DEN;
status = mmal_port_format_commit(video_port);
if (status != MMAL_SUCCESS)
{
throw Exception("Failure to setup camera video port");
}
// Make sure we have enough video buffers
if (video_port->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM)
{
video_port->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM;
}
// Flip the image
MMAL_PARAMETER_MIRROR_T mirror = {{MMAL_PARAMETER_MIRROR, sizeof(MMAL_PARAMETER_MIRROR_T)}, MMAL_PARAM_MIRROR_NONE};
mirror.value = MMAL_PARAM_MIRROR_BOTH;
mmal_port_parameter_set(preview_port, &mirror.hdr);
mmal_port_parameter_set(video_port, &mirror.hdr);
mmal_port_parameter_set(still_port, &mirror.hdr);
// Enable component
status = mmal_component_enable(camera);
if (status != MMAL_SUCCESS)
{
throw Exception("camera component couldn't be enabled");
}
//int iso = 100;
//MMAL_CHECK(mmal_port_parameter_set_uint32(camera->control, MMAL_PARAMETER_ISO, iso));
// MMAL_PARAM_EXPOSUREMODE_OFF
//MMAL_PARAMETER_EXPOSUREMODE_T exp_mode = {{MMAL_PARAMETER_EXPOSURE_MODE, sizeof(exp_mode)}, MMAL_PARAM_EXPOSUREMODE_AUTO};
//MMAL_CHECK(mmal_port_parameter_set(camera->control, &exp_mode.hdr));
/*
// AWB MODE
{
std::cout << "Disabling auto white balancing" << std::endl;
MMAL_PARAMETER_AWBMODE_T param = {{MMAL_PARAMETER_AWB_MODE,sizeof(param)}, MMAL_PARAM_AWBMODE_OFF};
mmal_port_parameter_set(camera->control, ¶m.hdr);
}
// AWB GAIN
{
real redGain = 0.2;
real blueGain = 1.0;
std::cout << "Setting color gain, r=" << redGain << ", b=" << blueGain << std::endl;
MMAL_PARAMETER_AWB_GAINS_T param = {{MMAL_PARAMETER_CUSTOM_AWB_GAINS,sizeof(param)}, {0,0}, {0,0}};
param.r_gain.num = (unsigned int)(redGain * 65536);
param.b_gain.num = (unsigned int)(blueGain * 65536);
param.r_gain.den = param.b_gain.den = 65536;
mmal_port_parameter_set(camera->control, ¶m.hdr);
}
*/
std::cout << "Camera Enabled..." << std::endl;
}
catch (...)
{
if (camera != NULL)
{
mmal_component_destroy(camera);
}
throw;
}
m_camera = camera;
m_videoPort = video_port;
m_stillPort = still_port;
m_previewPort = preview_port;
}
MMAL_COMPONENT_T *Private_Impl::create_camera_component ( RASPIVID_STATE *state ) {
MMAL_COMPONENT_T *camera = 0;
MMAL_ES_FORMAT_T *format;
MMAL_PORT_T *video_port = NULL;
// MMAL_PORT_T *preview_port = NULL;
// MMAL_PORT_T *still_port = NULL;
MMAL_STATUS_T status;
/* Create the component */
status = mmal_component_create ( MMAL_COMPONENT_DEFAULT_CAMERA, &camera );
if ( status != MMAL_SUCCESS ) {
cerr << ( "Failed to create camera component" );
return 0;
}
if ( !camera->output_num ) {
cerr << ( "Camera doesn't have output ports" );
mmal_component_destroy ( camera );
return 0;
}
video_port = camera->output[MMAL_CAMERA_VIDEO_PORT];
// preview_port=camera->output[MMAL_CAMERA_PREVIEW_PORT];
// still_port=camera->output[MMAL_CAMERA_CAPTURE_PORT];
// set up the camera configuration
MMAL_PARAMETER_CAMERA_CONFIG_T cam_config;
cam_config.hdr.id = MMAL_PARAMETER_CAMERA_CONFIG;
cam_config.hdr.size = sizeof ( cam_config );
cam_config.max_stills_w = state->width;
cam_config.max_stills_h = state->height;
cam_config.stills_yuv422 = 0;
cam_config.one_shot_stills = 0;
cam_config.max_preview_video_w = state->width;
cam_config.max_preview_video_h = state->height;
cam_config.num_preview_video_frames = 3;
cam_config.stills_capture_circular_buffer_height = 0;
cam_config.fast_preview_resume = 0;
cam_config.use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RAW_STC;
mmal_port_parameter_set ( camera->control, &cam_config.hdr );
/**
* Set the ROI of the sensor to use for captures/preview
* @param camera Pointer to camera component
* @param rect Normalised coordinates of ROI rectangle
*
*
*/
MMAL_PARAMETER_INPUT_CROP_T crop = {{MMAL_PARAMETER_INPUT_CROP, sizeof(MMAL_PARAMETER_INPUT_CROP_T)}};
double x = state->roi.x;
double y = state->roi.y;
double w = state->roi.w;
double h = state->roi.h;
if (x + w > 1.0)
w = 1 - x;
if (y + h > 1.0)
h = 1 - y;
crop.rect.x = (65536 * x);
crop.rect.y = (65536 * y);
crop.rect.width = (65536 * w);
crop.rect.height = (65536 * h);
mmal_port_parameter_set(camera->control, &crop.hdr);
// Set the encode format on the video port
format = video_port->format;
format->encoding_variant = convertFormat ( State.captureFtm );
format->encoding = convertFormat ( State.captureFtm );
format->es->video.width = VCOS_ALIGN_UP(state->width, 32);
format->es->video.height = VCOS_ALIGN_UP(state->height, 16);
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 = state->framerate;
format->es->video.frame_rate.den = VIDEO_FRAME_RATE_DEN;
status = mmal_port_format_commit ( video_port );
if ( status ) {
cerr << ( "camera video format couldn't be set" );
mmal_component_destroy ( camera );
return 0;
}
// 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;
video_port->buffer_size = video_port->buffer_size_recommended;
//video_port->buffer_num = video_port->buffer_num_recommended;
//PR : create pool of message on video port
MMAL_POOL_T *pool;
pool = mmal_port_pool_create ( video_port, video_port->buffer_num, video_port->buffer_size );
if ( !pool ) {
cerr << ( "Failed to create buffer header pool for video output port" );
}
state->video_pool = pool;
// PR : plug the callback to the video port
status = mmal_port_enable ( video_port, video_buffer_callback );
if ( status ) {
cerr << ( "camera video callback2 error" );
mmal_component_destroy ( camera );
return 0;
}
/* Enable component */
status = mmal_component_enable ( camera );
if ( status ) {
cerr << ( "camera component couldn't be enabled" );
mmal_component_destroy ( camera );
return 0;
}
state->camera_component = camera;//this needs to be before set_all_parameters
return camera;
}
void MmalStillCamera::createCameraComponent()
{
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;
try
{
status = mmal_component_create(MMAL_COMPONENT_DEFAULT_CAMERA, &camera);
if (status != MMAL_SUCCESS)
{
throw Exception("Failed to create camera component");
}
if (camera->output_num == 0)
{
status = MMAL_ENOSYS;
throw Exception("Camera doesn't have output ports");
}
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, CameraControlCallback);
if (status != MMAL_SUCCESS)
{
throw Exception("Unable to enable control port");
}
// Populate the camera configuration struct
MMAL_PARAMETER_CAMERA_CONFIG_T cameraConfig;
cameraConfig.hdr.id = MMAL_PARAMETER_CAMERA_CONFIG;
cameraConfig.hdr.size = sizeof(cameraConfig);
cameraConfig.max_stills_w = m_imageWidth;
cameraConfig.max_stills_h = m_imageHeight;
cameraConfig.stills_yuv422 = 0;
cameraConfig.one_shot_stills = 1;
cameraConfig.max_preview_video_w = VCOS_ALIGN_UP(FULL_FOV_PREVIEW_4x3_X, 32);
cameraConfig.max_preview_video_h = VCOS_ALIGN_UP(FULL_FOV_PREVIEW_4x3_Y, 16);
cameraConfig.num_preview_video_frames = 3;
cameraConfig.stills_capture_circular_buffer_height = 0;
cameraConfig.fast_preview_resume = 0;
cameraConfig.use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RESET_STC;
if (mmal_port_parameter_set(camera->control, &cameraConfig.hdr) != MMAL_SUCCESS)
{
throw Exception("Error calling mmal_port_parameter_set()");
}
format = preview_port->format;
format->encoding = MMAL_ENCODING_OPAQUE;
format->encoding_variant = MMAL_ENCODING_I420;
format->es->video.width = VCOS_ALIGN_UP(FULL_FOV_PREVIEW_4x3_X, 32);
format->es->video.height = VCOS_ALIGN_UP(FULL_FOV_PREVIEW_4x3_Y, 16);
format->es->video.crop.x = 0;
format->es->video.crop.y = 0;
format->es->video.crop.width = FULL_FOV_PREVIEW_4x3_X;
format->es->video.crop.height = FULL_FOV_PREVIEW_4x3_Y;
format->es->video.frame_rate.num = FULL_RES_PREVIEW_FRAME_RATE_NUM;
format->es->video.frame_rate.den = FULL_RES_PREVIEW_FRAME_RATE_DEN;
status = mmal_port_format_commit(preview_port);
if (status != MMAL_SUCCESS)
{
throw Exception("Failure to setup camera preview port");
}
// We must also setup the video port even though this is an image acquisition
mmal_format_full_copy(video_port->format, format);
status = mmal_port_format_commit(video_port);
if (status != MMAL_SUCCESS)
{
throw Exception("Failure to setup camera video port");
}
// Make sure we have enough video buffers
if (video_port->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM)
{
video_port->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM;
}
// Use the video format for images as well
format = still_port->format;
// Set our stills format on the stills (for encoder) port
format->encoding = MMAL_ENCODING_BGR24;
format->encoding_variant = MMAL_ENCODING_BGR24;
format->es->video.width = VCOS_ALIGN_UP(m_imageWidth, 32);
format->es->video.height = VCOS_ALIGN_UP(m_imageHeight, 16);
format->es->video.crop.x = 0;
format->es->video.crop.y = 0;
format->es->video.crop.width = m_imageWidth;
format->es->video.crop.height = m_imageHeight;
format->es->video.frame_rate.num = STILLS_FRAME_RATE_NUM;
format->es->video.frame_rate.den = STILLS_FRAME_RATE_DEN;
status = mmal_port_format_commit(still_port);
if (status != MMAL_SUCCESS)
{
throw Exception("Failure to setup camera image port");
}
// Verify number of buffers
if (still_port->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM)
{
still_port->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM;
}
// Flip the image
MMAL_PARAMETER_MIRROR_T mirror = {{MMAL_PARAMETER_MIRROR, sizeof(MMAL_PARAMETER_MIRROR_T)}, MMAL_PARAM_MIRROR_NONE};
mirror.value = MMAL_PARAM_MIRROR_BOTH;
mmal_port_parameter_set(preview_port, &mirror.hdr);
mmal_port_parameter_set(video_port, &mirror.hdr);
mmal_port_parameter_set(still_port, &mirror.hdr);
// Enable component
status = mmal_component_enable(camera);
if (status != MMAL_SUCCESS)
{
throw Exception("camera component couldn't be enabled");
}
}
catch (...)
{
if (camera != NULL)
{
mmal_component_destroy(camera);
}
throw;
}
m_camera = camera;
m_previewPort = preview_port;
m_videoPort = video_port;
m_stillPort = still_port;
}
MmalStillCamera::MmalStillCamera(int imageWidth, int imageHeight, bool enableBurstMode) :
m_imageWidth(imageWidth),
m_imageHeight(imageHeight),
m_callbackData(NULL),
m_cs(),
m_camera(NULL),
m_preview(NULL),
m_encoder(NULL),
m_pool(NULL),
m_previewPort(NULL),
m_videoPort(NULL),
m_stillPort(NULL),
m_targetPort(NULL)
{
m_callbackData = new MmalStillCallbackData(m_imageWidth, m_imageHeight, 3);
createCameraComponent();
std::cout << "Created camera" << std::endl;
createPreview();
std::cout << "Created preview" << std::endl;
std::cout << "Target Image Width: " << VCOS_ALIGN_UP(m_imageWidth, 32) << std::endl;
std::cout << "Target Image Height: " << VCOS_ALIGN_UP(m_imageHeight, 16) << std::endl;
MMAL_STATUS_T status;
m_targetPort = m_stillPort;
// Create pool of buffer headers for the output port to consume
m_pool = mmal_port_pool_create(m_targetPort, m_targetPort->buffer_num, m_targetPort->buffer_size);
if (m_pool == NULL)
{
throw Exception("Failed to create buffer header pool for encoder output port");
}
MMAL_CONNECTION_T *preview_connection = NULL;
status = connect_ports(m_previewPort, m_preview->input[0], &preview_connection);
if (status != MMAL_SUCCESS)
{
throw Exception("Error connecting preview port");
}
m_targetPort->userdata = (struct MMAL_PORT_USERDATA_T *) m_callbackData;
m_callbackData->image = NULL;
m_callbackData->pool = m_pool;
// Enable the encoder output port and tell it its callback function
if (mmal_port_enable(m_targetPort, EncoderBufferCallback) != MMAL_SUCCESS)
{
std::cerr << "Error enabling the encoder buffer callback" << std::endl;
}
// Send all the buffers to the encoder output port
int num = mmal_queue_length(m_pool->queue);
for (int q = 0; q < num; q++)
{
MMAL_BUFFER_HEADER_T *buffer = mmal_queue_get(m_pool->queue);
if (buffer == NULL)
{
std::cerr << "Unable to get buffer " << q << " from the pool" << std::endl;
}
if (mmal_port_send_buffer(m_targetPort, buffer)!= MMAL_SUCCESS)
{
std::cerr << "Unable to send a buffer " << q << " to encoder output port " << std::endl;
}
}
// Enable burst mode
if (enableBurstMode)
{
if (mmal_port_parameter_set_boolean(m_camera->control, MMAL_PARAMETER_CAMERA_BURST_CAPTURE, 1) != MMAL_SUCCESS)
{
std::cerr << "Error enabling burst mode" << std::endl;
}
std::cout << "Enabled burst mode for still images" << std::endl;
}
std::cout << "Initialized camera" << std::endl;
}
/**
* Reset the camera component, set up its ports
*
* @param state Pointer to state control struct
*
* @return MMAL_SUCCESS if all OK, something else otherwise
*
*/
MMAL_STATUS_T reset_camera_component(RASPIVID_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;
/* Get the camera component */
if( state->camera_component == NULL ) {
vcos_log_error("camera component does not exist");
}
else {
camera = state->camera_component;
}
#ifdef __NOT_REQURED__
status = raspicamcontrol_set_stereo_mode(camera->output[0], &state->camera_parameters.stereo_mode);
status += raspicamcontrol_set_stereo_mode(camera->output[1], &state->camera_parameters.stereo_mode);
status += raspicamcontrol_set_stereo_mode(camera->output[2], &state->camera_parameters.stereo_mode);
if (status != MMAL_SUCCESS)
{
vcos_log_error("Could not set stereo mode : error %d", status);
goto error;
}
MMAL_PARAMETER_INT32_T camera_num =
{{MMAL_PARAMETER_CAMERA_NUM, sizeof(camera_num)}, state->cameraNum};
status = mmal_port_parameter_set(camera->control, &camera_num.hdr);
if (status != MMAL_SUCCESS)
{
vcos_log_error("Could not select camera : error %d", status);
goto error;
}
if (!camera->output_num)
{
status = MMAL_ENOSYS;
vcos_log_error("Camera doesn't have output ports");
goto error;
}
status = mmal_port_parameter_set_uint32(camera->control, MMAL_PARAMETER_CAMERA_CUSTOM_SENSOR_CONFIG, state->sensor_mode);
if (status != MMAL_SUCCESS)
{
vcos_log_error("Could not set sensor mode : error %d", status);
goto error;
}
#endif /* NOT_REQUIRED */
preview_port = camera->output[MMAL_CAMERA_PREVIEW_PORT];
video_port = camera->output[MMAL_CAMERA_VIDEO_PORT];
still_port = camera->output[MMAL_CAMERA_CAPTURE_PORT];
#ifdef __NOT_REQURED__
if (state->settings)
{
MMAL_PARAMETER_CHANGE_EVENT_REQUEST_T change_event_request =
{ {MMAL_PARAMETER_CHANGE_EVENT_REQUEST, sizeof(MMAL_PARAMETER_CHANGE_EVENT_REQUEST_T)},
MMAL_PARAMETER_CAMERA_SETTINGS, 1
};
status = mmal_port_parameter_set(camera->control, &change_event_request.hdr);
if ( status != MMAL_SUCCESS )
{
vcos_log_error("No camera settings events");
}
}
#endif /* NOT_REQUIRED */
//
mmal_encoder_set_camera_settings(camera);
// Enable the camera, and tell it its control callback function
status = mmal_port_enable(camera->control, camera_control_callback);
if (status != MMAL_SUCCESS)
{
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 = 0,
.max_preview_video_w = state->width,
.max_preview_video_h = state->height,
.num_preview_video_frames = 3,
.stills_capture_circular_buffer_height = 0,
.fast_preview_resume = 0,
.use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RAW_STC
};
mmal_port_parameter_set(camera->control, &cam_config.hdr);
}
// Now set up the port formats
// Set the encode format on the Preview port
// HW limitations mean we need the preview to be the same size as the required recorded output
format = preview_port->format;
format->encoding = MMAL_ENCODING_OPAQUE;
format->encoding_variant = MMAL_ENCODING_I420;
if(state->camera_parameters.shutter_speed > 6000000)
{
MMAL_PARAMETER_FPS_RANGE_T fps_range = {{MMAL_PARAMETER_FPS_RANGE, sizeof(fps_range)},
{ 50, 1000 }, {166, 1000}
};
mmal_port_parameter_set(preview_port, &fps_range.hdr);
}
else if(state->camera_parameters.shutter_speed > 1000000)
{
MMAL_PARAMETER_FPS_RANGE_T fps_range = {{MMAL_PARAMETER_FPS_RANGE, sizeof(fps_range)},
{ 166, 1000 }, {999, 1000}
};
mmal_port_parameter_set(preview_port, &fps_range.hdr);
}
//enable dynamic framerate if necessary
if (state->camera_parameters.shutter_speed)
{
if (state->framerate > 1000000./state->camera_parameters.shutter_speed)
{
state->framerate=0;
if (state->verbose)
DLOG("Enable dynamic frame rate to fulfil shutter speed requirement");
}
}
format->encoding = MMAL_ENCODING_OPAQUE;
format->es->video.width = VCOS_ALIGN_UP(state->width, 32);
format->es->video.height = VCOS_ALIGN_UP(state->height, 16);
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 = PREVIEW_FRAME_RATE_NUM;
format->es->video.frame_rate.den = PREVIEW_FRAME_RATE_DEN;
status = mmal_port_format_commit(preview_port);
if (status != MMAL_SUCCESS)
{
vcos_log_error("camera viewfinder format couldn't be set");
goto error;
}
// Set the encode format on the video port
format = video_port->format;
format->encoding_variant = MMAL_ENCODING_I420;
if(state->camera_parameters.shutter_speed > 6000000)
{
MMAL_PARAMETER_FPS_RANGE_T fps_range = {{MMAL_PARAMETER_FPS_RANGE, sizeof(fps_range)},
{ 50, 1000 }, {166, 1000}
};
mmal_port_parameter_set(video_port, &fps_range.hdr);
}
else if(state->camera_parameters.shutter_speed > 1000000)
{
MMAL_PARAMETER_FPS_RANGE_T fps_range = {{MMAL_PARAMETER_FPS_RANGE, sizeof(fps_range)},
{ 167, 1000 }, {999, 1000}
};
mmal_port_parameter_set(video_port, &fps_range.hdr);
}
format->encoding = MMAL_ENCODING_OPAQUE;
format->es->video.width = VCOS_ALIGN_UP(state->width, 32);
format->es->video.height = VCOS_ALIGN_UP(state->height, 16);
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 = state->framerate;
format->es->video.frame_rate.den = VIDEO_FRAME_RATE_DEN;
status = mmal_port_format_commit(video_port);
if (status != MMAL_SUCCESS)
{
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;
// Set the encode format on the still port
format = still_port->format;
format->encoding = MMAL_ENCODING_OPAQUE;
format->encoding_variant = MMAL_ENCODING_I420;
format->es->video.width = VCOS_ALIGN_UP(state->width, 32);
format->es->video.height = VCOS_ALIGN_UP(state->height, 16);
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 = 0;
format->es->video.frame_rate.den = 1;
status = mmal_port_format_commit(still_port);
if (status != MMAL_SUCCESS)
{
vcos_log_error("camera still format couldn't be set");
goto error;
}
/* Ensure there are enough buffers to avoid dropping frames */
if (still_port->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM)
still_port->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM;
/* Enable component */
status = mmal_component_enable(camera);
if (status != MMAL_SUCCESS)
{
vcos_log_error("camera component couldn't be enabled");
goto error;
}
raspicamcontrol_set_all_parameters(camera, &state->camera_parameters);
state->camera_component = camera;
update_annotation_data(state);
return status;
error:
if (camera)
mmal_component_destroy(camera);
return status;
}
void MmalStillCamera::initialize(CameraMode cameraMode)
{
m_resolution = MmalUtil::get()->getClosestResolution(m_name, cameraMode);
if (m_imageWidth != -1)
{
throw Exception("Camera is already initialized");
}
m_imageWidth = m_resolution.width;
m_imageHeight = m_resolution.height;
m_callbackData = new MmalStillCallbackData(m_imageWidth, m_imageHeight, 3);
// Handle the sensor properties
real sensorWidth, sensorHeight, focalLength;
MmalUtil::get()->getSensorProperties(m_name, sensorWidth, sensorHeight, focalLength);
setSensorProperties(sensorWidth, sensorHeight, focalLength);
createCameraComponent();
InfoLog << "Created camera" << Logger::ENDL;
createPreview();
InfoLog << "Created preview" << Logger::ENDL;
InfoLog << "Target Image Width: " << VCOS_ALIGN_UP(m_imageWidth, 32) << Logger::ENDL;
InfoLog << "Target Image Height: " << VCOS_ALIGN_UP(m_imageHeight, 16) << Logger::ENDL;
MMAL_STATUS_T status;
m_targetPort = m_stillPort;
// Create pool of buffer headers for the output port to consume
m_pool = mmal_port_pool_create(m_targetPort, m_targetPort->buffer_num, m_targetPort->buffer_size);
if (m_pool == NULL)
{
throw Exception("Failed to create buffer header pool for encoder output port");
}
MMAL_CONNECTION_T *preview_connection = NULL;
status = connect_ports(m_previewPort, m_preview->input[0], &preview_connection);
if (status != MMAL_SUCCESS)
{
throw Exception("Error connecting preview port");
}
m_targetPort->userdata = (struct MMAL_PORT_USERDATA_T *) m_callbackData;
m_callbackData->image = NULL;
m_callbackData->pool = m_pool;
// Enable the encoder output port and tell it its callback function
if (mmal_port_enable(m_targetPort, EncoderBufferCallback) != MMAL_SUCCESS)
{
ErrorLog << "Error enabling the encoder buffer callback" << Logger::ENDL;
}
// Send all the buffers to the encoder output port
int num = mmal_queue_length(m_pool->queue);
for (int q = 0; q < num; q++)
{
MMAL_BUFFER_HEADER_T *buffer = mmal_queue_get(m_pool->queue);
if (buffer == NULL)
{
ErrorLog << "Unable to get buffer " << q << " from the pool" << Logger::ENDL;
}
if (mmal_port_send_buffer(m_targetPort, buffer) != MMAL_SUCCESS)
{
ErrorLog << "Unable to send a buffer " << q << " to encoder output port " << Logger::ENDL;
}
}
// Enable burst mode
if (m_burstModeEnabled)
{
if (mmal_port_parameter_set_boolean(m_camera->control, MMAL_PARAMETER_CAMERA_BURST_CAPTURE, 1) != MMAL_SUCCESS)
{
ErrorLog << "Error enabling burst mode" << Logger::ENDL;
}
InfoLog << "Enabled burst mode for still images" << Logger::ENDL;
}
InfoLog << "Initialized camera" << Logger::ENDL;
}
