void PiCam::video_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer) {
PiCam* cam = (PiCam*) port->userdata;
if (buffer->length) {
mmal_buffer_header_mem_lock(buffer);
memcpy(cam->frame.data, buffer->data, 3*cam->width*cam->height);
//memcpy(cam->frame.data, buffer->data, cam->width*cam->height);
mmal_buffer_header_mem_unlock(buffer);
cam->callback(cam->frame);
//cv::waitKey(1);
}
// release buffer back to the pool
mmal_buffer_header_release(buffer);
/*
if (vcos_semaphore_trywait(&(cam->frame_semaphore)) != VCOS_SUCCESS) {
vcos_semaphore_post(&(cam->frame_semaphore));
}
*/
// 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(cam->videoPool->queue);
if (new_buffer)
status = mmal_port_send_buffer(port, new_buffer);
if (!new_buffer || status != MMAL_SUCCESS)
vcos_log_error("Unable to return a buffer to the encoder port");
}
}
static void encoder_output_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer) {
MMAL_BUFFER_HEADER_T *new_buffer;
PORT_USERDATA *userdata = (PORT_USERDATA *) port->userdata;
MMAL_POOL_T *pool = userdata->encoder_output_pool;
//fprintf(stderr, "INFO:%s\n", __func__);
mmal_buffer_header_mem_lock(buffer);
fwrite(buffer->data, 1, buffer->length, stdout);
mmal_buffer_header_mem_unlock(buffer);
mmal_buffer_header_release(buffer);
if (port->is_enabled) {
MMAL_STATUS_T status;
new_buffer = mmal_queue_get(pool->queue);
if (new_buffer) {
status = mmal_port_send_buffer(port, new_buffer);
}
if (!new_buffer || status != MMAL_SUCCESS) {
fprintf(stderr, "Unable to return a buffer to the video port\n");
}
}
}
static void encoder_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
{
USERDATA_T *data;
vcos_assert((data = (USERDATA_T *)port->userdata) != NULL
&& "No data associated with this buffer");
vcos_assert((buffer && buffer->length > 0)
&& "buffer == NULL or buffer has size 0");
mmal_buffer_header_mem_lock(buffer);
vcos_assert((fwrite(buffer->data, 1, buffer->length, data->file_handle) == buffer->length)
&& "Failed writing buffer to file (different size returned)");
mmal_buffer_header_mem_unlock(buffer);
mmal_buffer_header_release(buffer);
// send one buffer back
if (port->is_enabled)
{
MMAL_BUFFER_HEADER_T *new_buffer;
if ((new_buffer = mmal_queue_get(data->pool->queue)) != NULL )
vcos_assert((mmal_port_send_buffer(port, new_buffer) == MMAL_SUCCESS)
&& "Unable to return buffer to encoder port");
}
}
/**
* buffer header callback function for encoder
*
* 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 encoder_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 && pData->file_handle)
{
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;
}
// Now flag if we have completed
if (buffer->flags & (MMAL_BUFFER_HEADER_FLAG_FRAME_END | MMAL_BUFFER_HEADER_FLAG_TRANSMISSION_FAILED))
complete = 1;
}
else
{
vcos_log_error("Received a encoder 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_SUCCESS;
MMAL_BUFFER_HEADER_T *new_buffer;
new_buffer = mmal_queue_get(pData->pstate->encoder_pool->queue);
if (new_buffer)
{
status = mmal_port_send_buffer(port, new_buffer);
}
if (!new_buffer || status != MMAL_SUCCESS)
vcos_log_error("Unable to return a buffer to the encoder port");
}
if (complete)
vcos_semaphore_post(&(pData->complete_semaphore));
}
/* Send a payload buffer to a connected port/client */
static MMAL_STATUS_T mmal_port_clock_forward_payload(MMAL_PORT_T *port, const MMAL_CLOCK_PAYLOAD_T *payload)
{
MMAL_STATUS_T status;
MMAL_BUFFER_HEADER_T *buffer;
buffer = mmal_queue_get(port->priv->module->queue);
if (!buffer)
{
LOG_ERROR("no free buffers available");
return MMAL_ENOSPC;
}
status = mmal_buffer_header_mem_lock(buffer);
if (status != MMAL_SUCCESS)
{
LOG_ERROR("failed to lock buffer %s", mmal_status_to_string(status));
mmal_queue_put_back(port->priv->module->queue, buffer);
goto end;
}
buffer->length = sizeof(MMAL_CLOCK_PAYLOAD_T);
memcpy(buffer->data, payload, buffer->length);
mmal_buffer_header_mem_unlock(buffer);
mmal_port_buffer_header_callback(port, buffer);
end:
return status;
}
/* This will output a frame that can be used for OpenCV */
IplImage* cvQueryRPiFrame(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
{
MMAL_BUFFER_HEADER_T *new_buffer;
PORT_USERDATA *pData = (PORT_USERDATA *)port->userdata;
if (pData)
{
if (buffer->length)
{
/*Convert buffer to RGB IplImage*/
mmal_buffer_header_mem_lock(buffer);
int w=pData->pstate->width; // get image size
int h=pData->pstate->height;
int h4=h/4;
memcpy(yCSI_CAM->imageData,buffer->data,w*h);
memcpy(uCSI_CAM->imageData,buffer->data+w*h,w*h4);
memcpy(vCSI_CAM->imageData,buffer->data+w*h+w*h4,w*h4);
cvResize(uCSI_CAM, uCSI_CAM_BIG, CV_INTER_NN);
cvResize(vCSI_CAM, vCSI_CAM_BIG, CV_INTER_NN); //CV_INTER_LINEAR looks better but it's slower
cvMerge(yCSI_CAM, uCSI_CAM_BIG, vCSI_CAM_BIG, NULL, CSI_CAM_IMAGE);
cvCvtColor(CSI_CAM_IMAGE,CSI_CAM_DSTIMAGE,CV_YCrCb2RGB); // convert in RGB color space (slow)
mmal_buffer_header_mem_unlock(buffer);
}
else
{
vcos_log_error("buffer null");
}
}
else
{
vcos_log_error("Received a encoder 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;
new_buffer = mmal_queue_get(pData->pstate->video_pool->queue);
if (new_buffer)
status = mmal_port_send_buffer(port, new_buffer);
if (!new_buffer || status != MMAL_SUCCESS)
vcos_log_error("Unable to return a buffer to the encoder port");
}
return CSI_CAM_DSTIMAGE;
}
/**
* buffer header callback function for video
*
* @param port Pointer to port from which callback originated
* @param buffer mmal buffer header pointer
*/
static void video_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
{
MMAL_BUFFER_HEADER_T *new_buffer;
RASPIVID_STATE * state = (RASPIVID_STATE *)port->userdata;
if (state)
{
if (state->finished) {
vcos_semaphore_post(&state->capture_done_sem);
return;
}
if (buffer->length)
{
mmal_buffer_header_mem_lock(buffer);
//
// *** PR : OPEN CV Stuff here !
//
int w=state->width; // get image size
int h=state->height;
int pixelSize = state->monochrome ? 1 : 3;
memcpy(state->dstImage->imageData,buffer->data,w*h*pixelSize);
vcos_semaphore_post(&state->capture_done_sem);
vcos_semaphore_wait(&state->capture_sem);
mmal_buffer_header_mem_unlock(buffer);
}
else
{
vcos_log_error("buffer null");
}
}
else
{
vcos_log_error("Received a encoder 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;
new_buffer = mmal_queue_get(state->video_pool->queue);
if (new_buffer)
status = mmal_port_send_buffer(port, new_buffer);
if (!new_buffer || status != MMAL_SUCCESS)
vcos_log_error("Unable to return a buffer to the encoder port");
}
}
/**
* 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)
{
if (buffer->length)
{
mmal_buffer_header_mem_lock(buffer);
int i;
for( i = 0; i < buffer->length; i++ )
pData->data_dump[i] = buffer->data[i];
mmal_buffer_header_mem_unlock(buffer);
}
// 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));
}
}
/**
* buffer header callback function for encoder
*
* 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 encoder_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
{
MMAL_BUFFER_HEADER_T *new_buffer;
// 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;
vcos_assert(pData->file_handle);
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);
}
if (bytes_written != buffer->length)
{
vcos_log_error("Failed to write buffer data (%d from %d)- aborting", bytes_written, buffer->length);
pData->abort = 1;
}
}
else
{
vcos_log_error("Received a encoder 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;
new_buffer = mmal_queue_get(pData->pstate->encoder_pool->queue);
if (new_buffer)
status = mmal_port_send_buffer(port, new_buffer);
if (!new_buffer || status != MMAL_SUCCESS)
vcos_log_error("Unable to return a buffer to the encoder port");
}
}
static void jpegencoder_buffer_callback (MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer) {
int bytes_written = buffer->length;
char *filename_temp, *filename_temp2;
if(mjpeg_cnt == 0) {
if(!jpegoutput_file) {
asprintf(&filename_temp, jpeg_filename, image_cnt);
asprintf(&filename_temp2, "%s.part", filename_temp);
jpegoutput_file = fopen(filename_temp2, "wb");
free(filename_temp);
free(filename_temp2);
if(!jpegoutput_file) error("Could not open mjpeg-destination");
}
if(buffer->length) {
mmal_buffer_header_mem_lock(buffer);
bytes_written = fwrite(buffer->data, 1, buffer->length, jpegoutput_file);
mmal_buffer_header_mem_unlock(buffer);
}
if(bytes_written != buffer->length) error("Could not write all bytes");
}
if(buffer->flags & MMAL_BUFFER_HEADER_FLAG_FRAME_END) {
mjpeg_cnt++;
if(mjpeg_cnt == divider) {
fclose(jpegoutput_file);
jpegoutput_file = NULL;
asprintf(&filename_temp, jpeg_filename, image_cnt);
asprintf(&filename_temp2, "%s.part", filename_temp);
rename(filename_temp2, filename_temp);
free(filename_temp);
free(filename_temp2);
image_cnt++;
mjpeg_cnt = 0;
}
}
mmal_buffer_header_release(buffer);
if (port->is_enabled) {
MMAL_STATUS_T status = MMAL_SUCCESS;
MMAL_BUFFER_HEADER_T *new_buffer;
new_buffer = mmal_queue_get(pool_jpegencoder->queue);
if (new_buffer) status = mmal_port_send_buffer(port, new_buffer);
if (!new_buffer || status != MMAL_SUCCESS) error("Could not send buffers to port");
}
}
void Private_Impl::video_buffer_callback ( MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer ) {
MMAL_BUFFER_HEADER_T *new_buffer;
PORT_USERDATA *pData = ( PORT_USERDATA * ) port->userdata;
bool hasGrabbed = false;
//pData->_mutex.lock();
std::unique_lock<std::mutex> lck ( pData->_mutex );
if ( pData ) {
if ( pData->wantToGrab && buffer->length ) {
mmal_buffer_header_mem_lock ( buffer );
//printf("pdata buff length%d\n",pData->_buffData.size);
pData->_buffData.resize ( buffer->length );
memcpy ( pData->_buffData.data, buffer->data, buffer->length );
pData->npts = buffer->pts;
//tst=buffer->pts;
pData->wantToGrab = false;
hasGrabbed = true;
mmal_buffer_header_mem_unlock ( buffer );
}
}
//pData->_mutex.unlock();
// if ( hasGrabbed ) pData->Thcond.BroadCast(); //wake up waiting client
// 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;
new_buffer = mmal_queue_get ( pData->pstate->video_pool->queue );
if ( new_buffer ) {}
status = mmal_port_send_buffer ( port, new_buffer );
if ( !new_buffer || status != MMAL_SUCCESS )
printf ( "Unable to return a buffer to the encoder port\n" );
}
if ( pData->pstate->shutterSpeed != 0 )
mmal_port_parameter_set_uint32 ( pData->pstate->camera_component->control, MMAL_PARAMETER_SHUTTER_SPEED, pData->pstate->shutterSpeed ) ;
if ( hasGrabbed ) pData->Thcond.BroadCast(); //wake up waiting client
}
/**
* 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)
{
MMAL_STATUS_T status;
// We pass our file handle and other stuff in via the userdata field.
PORT_USERDATA *pData = (PORT_USERDATA *)port->userdata;
if (pData)
{
if (buffer->length)
{
mmal_buffer_header_mem_lock(buffer);
fwrite(buffer->data, 1, buffer->length, pData->file_handle);
mmal_buffer_header_mem_unlock(buffer);
}
// What about error conditions?
if (buffer->flags & (MMAL_BUFFER_HEADER_FLAG_FRAME_END | MMAL_BUFFER_HEADER_FLAG_EOS | MMAL_BUFFER_HEADER_FLAG_TRANSMISSION_FAILED))
{
vcos_semaphore_post(&(pData->complete_semaphore));
}
}
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_BUFFER_HEADER_T *new_buffer = mmal_queue_get(pData->pstate->camera_pool->queue);
// and back to the port from there.
status = mmal_port_send_buffer(port, new_buffer);
if (status != MMAL_SUCCESS)
vcos_log_error("Unable to return the buffer to the camera still port");
}
}
static void
h264_header_save(MMAL_BUFFER_HEADER_T *mmalbuf)
{
VideoCircularBuffer *vcb = &video_circular_buffer;
if (vcb->h264_header_position + mmalbuf->length > H264_MAX_HEADER_SIZE)
log_printf("h264 header bytes error.\n");
else
{
/* Save header bytes to write to .mp4 video files
*/
mmal_buffer_header_mem_lock(mmalbuf);
memcpy(vcb->h264_header + vcb->h264_header_position,
mmalbuf->data, mmalbuf->length);
mmal_buffer_header_mem_unlock(mmalbuf);
vcb->h264_header_position += mmalbuf->length;
}
}
void
still_jpeg_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
{
CameraObject *data = (CameraObject *) port->userdata;
int n;
static int bytes_written;
if (buffer->length && still_jpeg_encoder.file)
{
mmal_buffer_header_mem_lock(buffer);
n = fwrite(buffer->data, 1, buffer->length, still_jpeg_encoder.file);
bytes_written += n;
mmal_buffer_header_mem_unlock(buffer);
if (n != buffer->length)
{
log_printf("still_jpeg_callback: %s file write error. %m\n", data->name);
exit(1);
}
}
if (buffer->flags & MMAL_BUFFER_HEADER_FLAG_FRAME_END)
{
fclose(still_jpeg_encoder.file);
if (pikrellcam.still_capture_event)
event_add("still capture command", pikrellcam.t_now, 0,
event_still_capture_cmd,
pikrellcam.on_still_capture_cmd);
else if (pikrellcam.timelapse_capture_event)
{
if (bytes_written > 0)
time_lapse.sequence += 1;
else if (pikrellcam.timelapse_jpeg_last)
{
unlink(pikrellcam.timelapse_jpeg_last);
dup_string(&pikrellcam.timelapse_jpeg_last, "failed");
}
}
pikrellcam.still_capture_event = FALSE;
pikrellcam.timelapse_capture_event = FALSE;
bytes_written = 0;
pikrellcam.state_modified = TRUE;
still_jpeg_encoder.file = NULL;
}
return_buffer_to_port(port, buffer);
}
static void buffer_callback ( MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer ) {
RASPICAM_USERDATA *userdata = ( RASPICAM_USERDATA* ) port->userdata;
if ( userdata == NULL || userdata->cameraBoard == NULL ) {
} else {
unsigned int flags = buffer->flags;
mmal_buffer_header_mem_lock ( buffer );
for ( unsigned int i = 0; i < buffer->length; i++, userdata->bufferPosition++ ) {
if ( userdata->offset >= userdata->length ) {
cout << userdata->cameraBoard->API_NAME << ": Buffer provided was too small! Failed to copy data into buffer.\n";
userdata->cameraBoard = NULL;
break;
} else {
if ( userdata->cameraBoard->getEncoding() == RASPICAM_ENCODING_RGB ) {
// Determines if the byte is an RGB value
if ( userdata->bufferPosition >= 54 ) {
userdata->data[userdata->offset] = buffer->data[i];
userdata->offset++;
}
} else {
userdata->data[userdata->offset] = buffer->data[i];
userdata->offset++;
}
}
}
mmal_buffer_header_mem_unlock ( buffer );
unsigned int END_FLAG = 0;
END_FLAG |= MMAL_BUFFER_HEADER_FLAG_FRAME_END;
END_FLAG |= MMAL_BUFFER_HEADER_FLAG_TRANSMISSION_FAILED;
END_FLAG &= flags;
if ( END_FLAG != 0 ) {
if ( userdata->mutex == NULL ) {
userdata->imageCallback ( userdata->data, userdata->startingOffset, userdata->length - userdata->startingOffset );
} else {
sem_post ( userdata->mutex );
}
}
}
mmal_buffer_header_release ( buffer );
if ( port->is_enabled ) {
MMAL_BUFFER_HEADER_T *new_buffer = mmal_queue_get ( userdata->encoderPool->queue );
if ( new_buffer ) mmal_port_send_buffer ( port, new_buffer );
}
}
/* Process buffers received from other clock ports */
static MMAL_STATUS_T mmal_port_clock_process_buffer(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
{
MMAL_STATUS_T status = MMAL_SUCCESS;
MMAL_CLOCK_PAYLOAD_T payload;
if (buffer->length != sizeof(MMAL_CLOCK_PAYLOAD_T))
{
LOG_ERROR("invalid buffer length %d", buffer->length);
return MMAL_EINVAL;
}
mmal_buffer_header_mem_lock(buffer);
memcpy(&payload, buffer->data, sizeof(MMAL_CLOCK_PAYLOAD_T));
mmal_buffer_header_mem_unlock(buffer);
if (payload.magic != MMAL_CLOCK_PAYLOAD_MAGIC)
{
LOG_ERROR("buffer corrupt (magic %4.4s)", (char*)&payload.magic);
return MMAL_EINVAL;
}
LOG_TRACE("port %s length %d id %4.4s time %"PRIi64,
port->name, buffer->length, (char*)&payload.id, payload.time);
switch (payload.id)
{
case MMAL_CLOCK_PAYLOAD_TIME:
mmal_clock_media_time_set(port->priv->module->clock, payload.time);
break;
case MMAL_CLOCK_PAYLOAD_SCALE:
mmal_clock_scale_set(port->priv->module->clock, payload.data.scale);
break;
default:
LOG_ERROR("invalid id %4.4s", (char*)&payload.id);
status = MMAL_EINVAL;
break;
}
/* Finished with the buffer, so return it */
buffer->length = 0;
mmal_port_buffer_header_callback(port, buffer);
return status;
}
void CCameraOutput::EndReadFrame()
{
if(LockedBuffer)
{
// unlock and then release buffer back to the pool from whence it came
mmal_buffer_header_mem_unlock(LockedBuffer);
mmal_buffer_header_release(LockedBuffer);
LockedBuffer = NULL;
// and send it back to the port (if still open)
if (BufferPort->is_enabled)
{
MMAL_STATUS_T status;
MMAL_BUFFER_HEADER_T *new_buffer;
new_buffer = mmal_queue_get(BufferPool->queue);
if (new_buffer)
status = mmal_port_send_buffer(BufferPort, new_buffer);
if (!new_buffer || status != MMAL_SUCCESS)
printf("Unable to return a buffer to the video port\n");
}
}
}
static void jpegencoder2_buffer_callback (MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer) {
int bytes_written = buffer->length;
if(buffer->length) {
mmal_buffer_header_mem_lock(buffer);
bytes_written = fwrite(buffer->data, 1, buffer->length, jpegoutput2_file);
mmal_buffer_header_mem_unlock(buffer);
}
if(bytes_written != buffer->length) error("Could not write all bytes");
if(buffer->flags & MMAL_BUFFER_HEADER_FLAG_FRAME_END) {
fclose(jpegoutput2_file);
if(status_filename != 0) {
if(!timelapse) {
status_file = fopen(status_filename, "w");
fprintf(status_file, "ready");
fclose(status_file);
}
}
image2_cnt++;
capturing = 0;
}
mmal_buffer_header_release(buffer);
if (port->is_enabled) {
MMAL_STATUS_T status = MMAL_SUCCESS;
MMAL_BUFFER_HEADER_T *new_buffer;
new_buffer = mmal_queue_get(pool_jpegencoder2->queue);
if (new_buffer) status = mmal_port_send_buffer(port, new_buffer);
if (!new_buffer || status != MMAL_SUCCESS) error("Could not send buffers to port");
}
}
static void h264encoder_buffer_callback (MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer) {
int bytes_written = buffer->length;
if(buffer->length) {
mmal_buffer_header_mem_lock(buffer);
bytes_written = fwrite(buffer->data, 1, buffer->length, h264output_file);
mmal_buffer_header_mem_unlock(buffer);
if(bytes_written != buffer->length) error("Could not write all bytes");
}
mmal_buffer_header_release(buffer);
if (port->is_enabled) {
MMAL_STATUS_T status = MMAL_SUCCESS;
MMAL_BUFFER_HEADER_T *new_buffer;
new_buffer = mmal_queue_get(pool_h264encoder->queue);
if (new_buffer) status = mmal_port_send_buffer(port, new_buffer);
if (!new_buffer || status != MMAL_SUCCESS) error("Could not send buffers to port");
}
}
/* In pikrellcam, this callback receives resized I420 frames before
| sending them on to a jpeg encoder component which generates the
| mjpeg.jpg stream image. Here we send the frame data to the motion display
| routine for possible drawing of region outlines, motion vectors
| and/or various status text. Motion detection was done in the h264
| callback and a flag is set there so these two paths can be synchronized
| so motion vectors can be drawn on the right frame.
*/
void
I420_video_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
{
CameraObject *obj = (CameraObject *) port->userdata;
MMAL_BUFFER_HEADER_T *buffer_in;
static struct timeval timer;
int utime;
static int encoder_busy_count;
if ( buffer->length > 0
&& motion_frame_event
)
{
motion_frame_event = FALSE;
/* Do not send buffer to encoder if it has not received the previous
| one we sent unless this is the frame we want for a preview save.
| In that case, we may be sending a buffer to preview save before
| the previous buffer is handled. This is accounted for below.
*/
if ( mjpeg_encoder_send_count == mjpeg_encoder_recv_count
|| motion_frame.do_preview_save
)
{
if (obj->callback_port_in && obj->callback_pool_in)
{
buffer_in = mmal_queue_get(obj->callback_pool_in->queue);
if ( buffer_in
&& obj->callback_port_in->buffer_size >= buffer->length
)
{
mmal_buffer_header_mem_lock(buffer);
memcpy(buffer_in->data, buffer->data, buffer->length);
buffer_in->length = buffer->length;
mmal_buffer_header_mem_unlock(buffer);
display_draw(buffer_in->data);
if (motion_frame.do_preview_save)
{
/* If mjpeg encoder has not received previous buffer,
| then the buffer to save will be the second buffer
| it gets from now. Otherwise it's the next buffer.
*/
pthread_mutex_lock(&mjpeg_encoder_count_lock);
if (mjpeg_encoder_send_count == mjpeg_encoder_recv_count)
mjpeg_do_preview_save = 1;
else
mjpeg_do_preview_save = 2;
pthread_mutex_unlock(&mjpeg_encoder_count_lock);
if (mjpeg_do_preview_save == 2 && pikrellcam.debug)
printf("%s: encoder not clear -> preview save delayed\n",
fname_base(pikrellcam.video_pathname));
}
motion_frame.do_preview_save = FALSE;
++mjpeg_encoder_send_count;
mmal_port_send_buffer(obj->callback_port_in, buffer_in);
}
}
}
else
{
++encoder_busy_count;
if (pikrellcam.debug)
printf("encoder not clear (%d) -> skipping mjpeg frame.\n",
encoder_busy_count);
if (encoder_busy_count > 2) /* Frame maybe dropped ??, move on */
{
if (pikrellcam.debug)
printf(" Syncing recv/send counts.\n");
encoder_busy_count = 0;
mjpeg_encoder_recv_count = mjpeg_encoder_send_count;
}
}
if (buffer->flags & MMAL_BUFFER_HEADER_FLAG_FRAME_END)
{
if (pikrellcam.debug_fps && (utime = micro_elapsed_time(&timer)) > 0)
printf("%s fps %d\n", obj->name, 1000000 / utime);
}
}
return_buffer_to_port(port, buffer);
}
void
video_h264_encoder_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *mmalbuf)
{
VideoCircularBuffer *vcb = &video_circular_buffer;
MotionFrame *mf = &motion_frame;
KeyFrame *kf;
int i, end_space, t_elapsed, event = 0;
int t_usec, dt_frame;
boolean force_stop;
time_t t_cur = pikrellcam.t_now;
static int fps_count, pause_frame_count_adjust;
static time_t t_sec, t_prev;
uint64_t t64_now;
static int t_annotate, t_key_frame;
static struct timeval tv;
static uint64_t t0_stc, pts_prev;
static boolean prev_pause;
if (vcb->state == VCB_STATE_RESTARTING)
{
if (mmalbuf->flags & MMAL_BUFFER_HEADER_FLAG_CONFIG)
h264_header_save(mmalbuf);
fps_count = 0;
return_buffer_to_port(port, mmalbuf);
return;
}
if (mmalbuf->pts > 0)
{
if (pikrellcam.t_now > tv.tv_sec + 10)
{ /* Rarely, but time skew can change if ntp updates. */
gettimeofday(&tv, NULL);
mmal_port_parameter_get_uint64(port, MMAL_PARAMETER_SYSTEM_TIME, &t0_stc);
t0_stc = (uint64_t) tv.tv_sec * 1000000LL + (uint64_t) tv.tv_usec - t0_stc;
}
/* Skew adjust to the system clock to get second transitions.
| Annotate times need to be set early to get displayed time synced
| with system time. Needs >2 frames + offset to get it to work over
| range of fps values.
| Key frames can be delivered one frame after a request, so request
| within 1 1/2 frames before second time transitions.
*/
t64_now = t0_stc + mmalbuf->pts;
t_sec = (int) (t64_now / 1000000LL);
t_usec = (int) (t64_now % 1000000LL);
dt_frame = 1000000 / pikrellcam.camera_adjust.video_fps;
if ( t_annotate < t_sec
&& t_usec > 900000 - 5 * dt_frame / 2
)
{
t_annotate = t_sec;
annotate_text_update(t_annotate + 1);
}
if ( (vcb->state == VCB_STATE_NONE || vcb->pause)
&& t_key_frame < t_sec
&& t_usec > 1000000 - 3 * dt_frame / 2
)
{
t_key_frame = t_sec;
mmal_port_parameter_set_boolean(port,
MMAL_PARAMETER_VIDEO_REQUEST_I_FRAME, 1);
}
if ( (mmalbuf->flags & MMAL_BUFFER_HEADER_FLAG_KEYFRAME)
&& t_cur < t_sec
)
t_cur = t_sec;
}
pthread_mutex_lock(&vcb->mutex);
if (mmalbuf->flags & MMAL_BUFFER_HEADER_FLAG_CONFIG)
h264_header_save(mmalbuf);
else if (mmalbuf->flags & MMAL_BUFFER_HEADER_FLAG_CODECSIDEINFO)
{
if (++fps_count >= pikrellcam.mjpeg_divider)
{
motion_frame_event = TRUE; /* synchronize with i420 callback */
fps_count = 0;
mmal_buffer_header_mem_lock(mmalbuf);
memcpy(motion_frame.vectors, mmalbuf->data, motion_frame.vectors_size);
mmal_buffer_header_mem_unlock(mmalbuf);
motion_frame_process(vcb, &motion_frame);
}
}
else
{
if ( (mmalbuf->flags & MMAL_BUFFER_HEADER_FLAG_KEYFRAME)
&& !vcb->in_keyframe
)
{
/* Keep key_frame[cur_frame_index] always pointing to the latest
| keyframe (this one) in the video buffer. Then adjust the
| key_frame[pre_frame_index] to point to a keyframe
| in the video buffer that is pre_capture time behind.
| If paused, always keep tail pointing to the latest keyframe.
*/
vcb->in_keyframe = TRUE;
vcb->cur_frame_index = (vcb->cur_frame_index + 1) % KEYFRAME_SIZE;
kf = &vcb->key_frame[vcb->cur_frame_index];
kf->position = vcb->head;
kf->frame_count = 0;
pause_frame_count_adjust = 0;
if (vcb->pause && vcb->state == VCB_STATE_MANUAL_RECORD)
{
vcb->tail = vcb->head;
pts_prev = mmalbuf->pts;
pause_frame_count_adjust = 0;
}
kf->t_frame = t_cur;
kf->frame_pts = mmalbuf->pts;
while (t_cur - vcb->key_frame[vcb->pre_frame_index].t_frame
> pikrellcam.motion_times.pre_capture)
{
vcb->pre_frame_index = (vcb->pre_frame_index + 1) % KEYFRAME_SIZE;
if (vcb->pre_frame_index == vcb->cur_frame_index)
break;
}
}
if (mmalbuf->flags & MMAL_BUFFER_HEADER_FLAG_FRAME_END)
{
vcb->in_keyframe = FALSE;
i = vcb->pre_frame_index;
while (1)
{
vcb->key_frame[i].frame_count += 1;
if (i++ == vcb->cur_frame_index)
break;
i %= KEYFRAME_SIZE;
}
if ( vcb->state == VCB_STATE_MOTION_RECORD
|| vcb->state == VCB_STATE_MANUAL_RECORD
)
{
if (!vcb->pause)
vcb->frame_count += 1;
else
pause_frame_count_adjust += 1;
}
}
if (t_cur > t_prev)
{
if (!vcb->pause)
++vcb->record_elapsed_time;
t_prev = t_cur;
}
if (vcb->state == VCB_STATE_MOTION_RECORD_START)
{
/* Write mp4 header and set tail to beginning of pre_capture
| video data, then write the entire pre_capture time data.
| The keyframe data we collected above keeps a pointer to
| video data close to the pre_capture time we want.
*/
fwrite(vcb->h264_header, 1, vcb->h264_header_position, vcb->file);
pikrellcam.video_header_size = vcb->h264_header_position;
pikrellcam.video_size = vcb->h264_header_position;
vcb->tail = vcb->key_frame[vcb->record_start_frame_index].position;
vcb_video_write(vcb);
vcb->frame_count = vcb->key_frame[vcb->record_start_frame_index].frame_count;
vcb->video_frame_count = vcb->frame_count;
motion_event_write(vcb, mf);
vcb->state = VCB_STATE_MOTION_RECORD;
if (mf->external_trigger_time_limit > 0)
{
vcb->motion_sync_time = t_cur + mf->external_trigger_time_limit;
vcb->max_record_time = mf->external_trigger_time_limit;
}
else
{
vcb->motion_sync_time = t_cur + pikrellcam.motion_times.post_capture;
vcb->max_record_time = pikrellcam.motion_record_time_limit;
}
/* Schedule any motion begin command.
*/
event |= EVENT_MOTION_BEGIN;
}
if (vcb->state == VCB_STATE_MANUAL_RECORD_START)
{
/* Write mp4 header and set tail to most recent keyframe.
| So manual records may have up to about a sec pre_capture.
*/
fwrite(vcb->h264_header, 1, vcb->h264_header_position, vcb->file);
pikrellcam.video_header_size = vcb->h264_header_position;
pikrellcam.video_size = vcb->h264_header_position;
vcb->tail = vcb->key_frame[vcb->record_start_frame_index].position;
vcb_video_write(vcb);
vcb->frame_count = vcb->key_frame[vcb->record_start_frame_index].frame_count;
pts_prev = 0;
vcb->state = VCB_STATE_MANUAL_RECORD;
event |= EVENT_PREVIEW_SAVE;
}
if (h264_conn_status == H264_TCP_SEND_HEADER)
tcp_send_h264_header(vcb->h264_header, vcb->h264_header_position);
/* Save video data into the circular buffer.
*/
mmal_buffer_header_mem_lock(mmalbuf);
end_space = vcb->size - vcb->head;
if (mmalbuf->length <= end_space)
{
memcpy(vcb->data + vcb->head, mmalbuf->data, mmalbuf->length);
if(h264_conn_status == H264_TCP_SEND_DATA)
tcp_send_h264_data("data 1",vcb->data + vcb->head, mmalbuf->length);
}
else
{
memcpy(vcb->data + vcb->head, mmalbuf->data, end_space);
memcpy(vcb->data, mmalbuf->data + end_space, mmalbuf->length - end_space);
if (h264_conn_status == H264_TCP_SEND_DATA)
{
tcp_send_h264_data("data 2",vcb->data + vcb->head, end_space);
tcp_send_h264_data("data 3",vcb->data, mmalbuf->length - end_space);
}
}
vcb->head = (vcb->head + mmalbuf->length) % vcb->size;
mmal_buffer_header_mem_unlock(mmalbuf);
/* And write video data to a video file according to record state.
| Record time limit (if any) does not include pre capture times or
| manual paused time which is accounted for in record_elapsed_time.
*/
force_stop = FALSE;
if (vcb->max_record_time > 0)
{
t_elapsed = vcb->record_elapsed_time;
if (vcb->state == VCB_STATE_MOTION_RECORD)
t_elapsed -= (mf->external_trigger_pre_capture > 0) ?
mf->external_trigger_pre_capture
: pikrellcam.motion_times.pre_capture;
else
t_elapsed -= vcb->manual_pre_capture;
if (t_elapsed >= vcb->max_record_time)
force_stop = TRUE;
}
if (vcb->state == VCB_STATE_MANUAL_RECORD)
{
if (!vcb->pause)
{
if (mmalbuf->pts > 0)
{
if (pts_prev > 0)
vcb->last_pts += mmalbuf->pts - pts_prev;
else
vcb->last_pts = mmalbuf->pts;
pts_prev = mmalbuf->pts;
}
if (prev_pause)
{
vcb->frame_count += pause_frame_count_adjust;
pause_frame_count_adjust = 0;
}
vcb->video_frame_count = vcb->frame_count;
vcb_video_write(vcb); /* Continuously write video data */
}
prev_pause = vcb->pause;
if (force_stop)
video_record_stop(vcb);
}
else if (vcb->state == VCB_STATE_MOTION_RECORD)
{
/* Always write until we reach motion_sync time (which is last
| motion detect time + post_capture time), then hold during
| event_gap time. Motion events during event_gap time will bump
| motion_sync_time and event_gap expiration time higher thus
| triggering more writes up to the new sync_time.
| If there is not another motion event, event_gap time will be
| reached and we stop recording with the post_capture time
| already written.
*/
if (t_cur <= vcb->motion_sync_time)
{
if (mmalbuf->pts > 0)
vcb->last_pts = mmalbuf->pts;
vcb->video_frame_count = vcb->frame_count;
vcb_video_write(vcb);
}
if ( force_stop
|| ( mf->external_trigger_time_limit == 0
&& t_cur >= vcb->motion_last_detect_time + pikrellcam.motion_times.event_gap
)
)
{
/* For motion recording, preview_save_mode "first" has been
| handled in motion_frame_process(). But if not "first",
| there is a preview save waiting to be handled.
*/
video_record_stop(vcb);
event |= EVENT_MOTION_END;
if (strcmp(pikrellcam.motion_preview_save_mode, "first") != 0)
event |= EVENT_MOTION_PREVIEW_SAVE_CMD;
}
}
}
pthread_mutex_unlock(&vcb->mutex);
return_buffer_to_port(port, mmalbuf);
/* This handles preview saves for manual records for possible future use.
| preview_save_cmd does not apply for manual records.
| All preview saves for motion records are scheduled in motion_frame_process().
| preview_save_cmd for save mode "best" is handled in video_record_stop().
*/
if (event & EVENT_PREVIEW_SAVE)
event_add("manual preview save", pikrellcam.t_now, 0,
event_preview_save, NULL);
if ( (event & EVENT_MOTION_BEGIN)
&& *pikrellcam.on_motion_begin_cmd != '\0'
)
event_add("motion begin", pikrellcam.t_now, 0,
exec_no_wait, pikrellcam.on_motion_begin_cmd);
/* motion end event and preview dispose are handled in video_record_stop()
*/
}
/**
* buffer header callback function for encoder
*
* 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 encoder_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
{
MMAL_BUFFER_HEADER_T *new_buffer;
static int64_t base_time = -1;
//fprintf(stderr,"*");fflush(stderr);
// All our segment times based on the receipt of the first encoder callback
if (base_time == -1)
base_time = vcos_getmicrosecs64()/1000;
// 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;
int64_t current_time = vcos_getmicrosecs64()/1000;
vcos_assert(pData->file_handle);
if(pData->pstate->inlineMotionVectors) vcos_assert(pData->imv_file_handle);
//else
//{
// For segmented record mode, we need to see if we have exceeded our time/size,
// but also since we have inline headers turned on we need to break when we get one to
// ensure that the new stream has the header in it. If we break on an I-frame, the
// SPS/PPS header is actually in the previous chunk.
/*if ((buffer->flags & MMAL_BUFFER_HEADER_FLAG_CONFIG) &&
((pData->pstate->segmentSize && current_time > base_time + pData->pstate->segmentSize) ||
(pData->pstate->splitWait && pData->pstate->splitNow)))
{
FILE *new_handle;
base_time = current_time;
pData->pstate->splitNow = 0;
pData->pstate->segmentNumber++;
// Only wrap if we have a wrap point set
if (pData->pstate->segmentWrap && pData->pstate->segmentNumber > pData->pstate->segmentWrap)
pData->pstate->segmentNumber = 1;
new_handle = open_filename(pData->pstate);
if (new_handle)
{
fclose(pData->file_handle);
pData->file_handle = new_handle;
}
new_handle = open_imv_filename(pData->pstate);
if (new_handle)
{
fclose(pData->imv_file_handle);
pData->imv_file_handle = new_handle;
}
}*/
if (buffer->length)
{
//printf("writing...");
mmal_buffer_header_mem_lock(buffer);
if(buffer->flags & MMAL_BUFFER_HEADER_FLAG_CODECSIDEINFO)
{
if(pData->pstate->inlineMotionVectors)
{
bytes_written = fwrite(buffer->data, 1, buffer->length, pData->imv_file_handle);
}
else
{
//We do not want to save inlineMotionVectors...
bytes_written = buffer->length;
}
}
else
{
bytes_written = fwrite(buffer->data, 1, buffer->length, pData->file_handle);
}
mmal_buffer_header_mem_unlock(buffer);
if (bytes_written != buffer->length)
{
vcos_log_error("Failed to write buffer data (%d from %d)- aborting", bytes_written, buffer->length);
pData->abort = 1;
}
}
//}
}
else
{
vcos_log_error("Received a encoder 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;
new_buffer = mmal_queue_get(pData->pstate->encoder_pool->queue);
if (new_buffer)
status = mmal_port_send_buffer(port, new_buffer);
if (!new_buffer || status != MMAL_SUCCESS)
vcos_log_error("Unable to return a buffer to the encoder port");
}
//printf("done.\n");
}
static void video_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
{
//fprintf(stderr,"#");fflush(stderr);
MMAL_BUFFER_HEADER_T *new_buffer;
RASPIVID_STATE * state = (RASPIVID_STATE *)port->userdata;
counter++;
if (state)
{
if (state->finished) {
vcos_semaphore_post(&state->capture_done_sem);
return;
}
if (buffer->length)
{
mmal_buffer_header_mem_lock(buffer);
//
// *** PR : OPEN CV Stuff here !
//
int w=state->width; // get image size
int h=state->height;
int h4=h/4;
memcpy(state->py->imageData,buffer->data,w*h*3); // read BGR
/*if (state->graymode==0)
{
memcpy(state->pu->imageData,buffer->data+w*h,w*h4); // read U
memcpy(state->pv->imageData,buffer->data+w*h+w*h4,w*h4); // read v
}*/
if (state->graymode==0)
{
memcpy(state->pu->imageData,buffer->data+w*h,w*h4); // read U
memcpy(state->pv->imageData,buffer->data+w*h+w*h4,w*h4); // read v
}
//fprintf(stderr,"a");fflush(stderr);
vcos_semaphore_post(&state->capture_done_sem);
//fprintf(stderr,"b");fflush(stderr);
vcos_semaphore_wait(&state->capture_sem);
//fprintf(stderr,"c");fflush(stderr);
mmal_buffer_header_mem_unlock(buffer);
}
else
{
vcos_log_error("buffer null");
}
}
else
{
vcos_log_error("Received a encoder 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;
new_buffer = mmal_queue_get(state->video_pool->queue);
if (new_buffer)
status = mmal_port_send_buffer(port, new_buffer);
if (!new_buffer || status != MMAL_SUCCESS)
vcos_log_error("Unable to return a buffer to the encoder port");
}
}
/**
* buffer header callback function for encoder
*
* 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 encoder_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)
{
if (buffer->length)
{
mmal_buffer_header_mem_lock(buffer);
//
// *** PR : OPEN CV Stuff here !
//
// create a CvMat empty structure, with size of the buffer.
CvMat* buf = cvCreateMat(1,buffer->length,CV_8UC1);
// copy buffer from cam to CvMat
buf->data.ptr = buffer->data;
// decode image (interpret jpg)
IplImage *img = cvDecodeImage(buf, CV_LOAD_IMAGE_COLOR);
// we can save it !
cvSaveImage("foobar.bmp", img,0);
// or display it
cvNamedWindow("camcvWin", CV_WINDOW_AUTOSIZE);
cvShowImage("camcvWin", img );
cvWaitKey(0);
//cvReleaseImage(&img );
mmal_buffer_header_mem_unlock(buffer);
}
// Now flag if we have completed
if (buffer->flags & (MMAL_BUFFER_HEADER_FLAG_FRAME_END | MMAL_BUFFER_HEADER_FLAG_TRANSMISSION_FAILED))
complete = 1;
}
else
{
vcos_log_error("Received a encoder 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;
new_buffer = mmal_queue_get(pData->pstate->encoder_pool->queue);
if (new_buffer)
{
status = mmal_port_send_buffer(port, new_buffer);
}
if (!new_buffer || status != MMAL_SUCCESS)
vcos_log_error("Unable to return a buffer to the encoder port");
}
if (complete)
vcos_semaphore_post(&(pData->complete_semaphore));
}
/**
* 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 = 0;
int bytes_to_write = buffer->length;
if (pData->pstate->onlyLuma)
bytes_to_write = port->format->es->video.width * port->format->es->video.height;
if (bytes_to_write && pData->file_handle)
{
mmal_buffer_header_mem_lock(buffer);
bytes_written = fwrite(buffer->data, 1, bytes_to_write, 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 (buffer->length && bytes_written != bytes_to_write)
{
vcos_log_error("Unable to write buffer to file - aborting %d vs %d", bytes_written, bytes_to_write);
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));
}
}
/**
* buffer header callback function for video
*
* @param port Pointer to port from which callback originated
* @param buffer mmal buffer header pointer
*/
static void video_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
{
MMAL_BUFFER_HEADER_T *new_buffer;
PORT_USERDATA *pData = (PORT_USERDATA *)port->userdata;
if (pData)
{
if (buffer->length)
{
mmal_buffer_header_mem_lock(buffer);
//
// *** PR : OPEN CV Stuff here !
//
int w=pData->pstate->width; // get image size
int h=pData->pstate->height;
int h4=h/4;
memcpy(py->imageData,buffer->data,w*h); // read Y
if (pData->pstate->graymode==0)
{
memcpy(pu->imageData,buffer->data+w*h,w*h4); // read U
memcpy(pv->imageData,buffer->data+w*h+w*h4,w*h4); // read v
cvResize(pu, pu_big, CV_INTER_NN);
cvResize(pv, pv_big, CV_INTER_NN); //CV_INTER_LINEAR looks better but it's slower
cvMerge(py, pu_big, pv_big, NULL, image);
cvCvtColor(image,dstImage,CV_YCrCb2RGB); // convert in RGB color space (slow)
cvShowImage("camcvWin", dstImage );
}
else
{
cvShowImage("camcvWin", py); // display only gray channel
}
cvWaitKey(1);
nCount++; // count frames displayed
mmal_buffer_header_mem_unlock(buffer);
}
else vcos_log_error("buffer null");
}
else
{
vcos_log_error("Received a encoder 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;
new_buffer = mmal_queue_get(pData->pstate->video_pool->queue);
if (new_buffer)
status = mmal_port_send_buffer(port, new_buffer);
if (!new_buffer || status != MMAL_SUCCESS)
vcos_log_error("Unable to return a buffer to the encoder port");
}
}
// ------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//
static void camera_video_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
{
PORT_USERDATA *userdata = (PORT_USERDATA *) port->userdata;
static struct timespec t1;
struct timespec t2;
static int frame_count = 0;
static int frame_post_count = 0;
if (frame_count == 0) {
clock_gettime(CLOCK_MONOTONIC, &t1);
}
frame_count++;
//if(1){
if( (CALC_FPS) && (frame_count % (VIDEO_FPS*2) == 0) ){
// print framerate every n frame
clock_gettime(CLOCK_MONOTONIC, &t2);
float d = (t2.tv_sec + t2.tv_nsec / 1000000000.0) - (t1.tv_sec + t1.tv_nsec / 1000000000.0);
float fps = 0.0;
if (d > 0) {
fps = frame_count / d;
} else {
fps = frame_count;
}
userdata->video_fps = fps;
//fprintf(stderr, " Frame = %d, Frame Post %d, Framerate = %.0f fps \n", frame_count, frame_post_count, fps);
}
//if(1){
if(userdata->motion > 0){
MMAL_BUFFER_HEADER_T *output_buffer = mmal_queue_get(userdata->encoder_input_pool->queue);
if(output_buffer){
/*if( (CALC_FPS) && (frame_count % (VIDEO_FPS*2) == 0) )
{
fprintf(stderr, "image: mmal_queue_get have valid buffer, output_buffer->data=0x%02X, output_buffer->length=%d, output_buffer->alloc_size=%d, buffer->length=%d \n", output_buffer->data, output_buffer->length, output_buffer->alloc_size, buffer->length );
}*/
mmal_buffer_header_mem_lock(buffer);
memcpy(output_buffer->data, buffer->data, buffer->length);
output_buffer->length = buffer->length;
mmal_buffer_header_mem_unlock(buffer);
if (mmal_port_send_buffer(userdata->encoder_input_port, output_buffer) != MMAL_SUCCESS) {
fprintf(stderr, "ERROR: Unable to send buffer \n");
}
}
userdata->motion--;
}
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_POOL_T *pool = userdata->camera_video_port_pool;
new_buffer = mmal_queue_get(pool->queue);
if (new_buffer) {
status = mmal_port_send_buffer(port, new_buffer);
}
if (!new_buffer || status != MMAL_SUCCESS) {
fprintf(stderr, "[%s]Unable to return a buffer to the video port\n", __func__);
}
}
}
/**
* buffer header callback function for encoder
*
* 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) {
MMAL_BUFFER_HEADER_T *new_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 && pData->pstate->isInit) {
int bytes_written = buffer->length;
if (buffer->length) {
mmal_buffer_header_mem_lock(buffer);
memcpy(&(pData->buffer[pData->frame & 1][pData->id]), buffer->data, buffer->length);
pData->id += bytes_written;
mmal_buffer_header_mem_unlock(buffer);
}
if (bytes_written != buffer->length) {
vcos_log_error("Failed to write buffer data (%d from %d)- aborting", bytes_written, buffer->length);
pData->abort = 1;
}
if (buffer->flags & (MMAL_BUFFER_HEADER_FLAG_FRAME_END | MMAL_BUFFER_HEADER_FLAG_TRANSMISSION_FAILED))
complete = 1;
if (complete) {
sensor_msgs::Image msg;
msg.header.seq = pData->frame;
msg.header.frame_id = tf_prefix;
msg.header.frame_id.append("/camera");
msg.header.stamp = ros::Time::now();
msg.height = pData->pstate->height;
msg.width = pData->pstate->width;
msg.encoding = "bgra8";
msg.is_bigendian = 0;
msg.step = pData->pstate->width*4;
msg.data.insert(msg.data.end(), pData->buffer[pData->frame & 1], &(pData->buffer[pData->frame & 1][pData->id]));
image_pub.publish(msg);
c_info.header.seq = pData->frame;
c_info.header.stamp = msg.header.stamp;
c_info.header.frame_id = msg.header.frame_id;
camera_info_pub.publish(c_info);
pData->frame++;
pData->id = 0;
}
} else {
vcos_log_error("Received a encoder 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;
new_buffer = mmal_queue_get(pData->pstate->camera_pool->queue);
if (new_buffer)
status = mmal_port_send_buffer(port, new_buffer);
if (!new_buffer || status != MMAL_SUCCESS)
vcos_log_error("Unable to return a buffer to the encoder port");
} else {
ROS_INFO("oups");
}
}
static void encoder_buffer_callback(
MMAL_PORT_T *port,
MMAL_BUFFER_HEADER_T *buffer
)
{
MMAL_BUFFER_HEADER_T *new_buffer;
static int buffer_count = 0;
static sSX_CAMERA_HW_BUFFER *hw_buf = NULL;
static unsigned index = 0;
// We pass our file handle and other stuff in via the userdata field.
printf("callback current time = %d\n", get_time_ns());
PORT_USERDATA *pData = (PORT_USERDATA *) port->userdata;
assert(pData != NULL);
int bytes_written = buffer->length;
vcos_assert(pData->file_handle);
assert(buffer->length > 0);
// Get frame end.
unsigned char frame_end = 0;
if(buffer->flags & MMAL_BUFFER_HEADER_FLAG_FRAME_END)
{
frame_end = 1;
}
unsigned char config = 0;
if(buffer->flags & MMAL_BUFFER_HEADER_FLAG_CONFIG)
{
config = 1;
}
#if 0
printf("frame_end = %d\n", buffer->flags & MMAL_BUFFER_HEADER_FLAG_FRAME_END);
printf("config = %d\n", buffer->flags & MMAL_BUFFER_HEADER_FLAG_CONFIG);
printf("buffer received: len = %d, frame_end = %d\n",
buffer->length, frame_end);
#endif
if(hw_buf == NULL)
{
index = 0;
hw_buf = malloc(sizeof(sSX_CAMERA_HW_BUFFER));
}
mmal_buffer_header_mem_lock(buffer);
unsigned int offset = 0;
if( (buffer->data[0] == 0x00)
&& (buffer->data[1] == 0x00)
&& (buffer->data[2] == 0x00)
&& (buffer->data[3] == 0x01))
{
offset = 4;
}
assert((index + buffer->length) <= SX_CAMERA_HW_NAL_LEN_MAX);
memcpy(&hw_buf->nal[index],
buffer->data + offset,
buffer->length - offset);
index += (buffer->length - offset);
mmal_buffer_header_mem_unlock(buffer);
// release buffer back to the pool
mmal_buffer_header_release(buffer);
if(frame_end || config)
{
hw_buf->nal_len = index;
sx_queue_push(f_nal_queue, hw_buf);
hw_buf = NULL;
// printf("(sx_camera_hw): Queue nal unit [len = %d]\n", index);
}
#if 0
printf("frame end = %d\n",
frame_end);
printf("data len = %d\n",
buffer->length);
printf("0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n",
buffer->data[0],
buffer->data[1],
buffer->data[2],
buffer->data[3],
buffer->data[4],
buffer->data[5],
buffer->data[6],
buffer->data[7]);
mmal_buffer_header_mem_lock(buffer);
bytes_written = fwrite(buffer->data, 1, buffer->length, pData->file_handle);
mmal_buffer_header_mem_unlock(buffer);
#endif
// and send one back to the port (if still open)
if (port->is_enabled)
{
MMAL_STATUS_T status;
new_buffer = mmal_queue_get(pData->pstate->encoder_pool->queue);
if (new_buffer)
{
status = mmal_port_send_buffer(port, new_buffer);
assert(status == MMAL_SUCCESS);
}
}
}
static void camera_video_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer) {
static int frame_count = 0;
static struct timespec t1;
struct timespec t2;
uint8_t *local_overlay_buffer;
//fprintf(stderr, "INFO:%s\n", __func__);
if (frame_count == 0) {
clock_gettime(CLOCK_MONOTONIC, &t1);
}
clock_gettime(CLOCK_MONOTONIC, &t2);
int d = t2.tv_sec - t1.tv_sec;
MMAL_BUFFER_HEADER_T *new_buffer;
MMAL_BUFFER_HEADER_T *output_buffer = 0;
PORT_USERDATA *userdata = (PORT_USERDATA *) port->userdata;
MMAL_POOL_T *pool = userdata->camera_video_port_pool;
frame_count++;
output_buffer = mmal_queue_get(userdata->encoder_input_pool->queue);
//Set pointer to latest updated/drawn double buffer to local pointer
if (userdata->overlay == 0) {
local_overlay_buffer = userdata->overlay_buffer;
}
else {
local_overlay_buffer = userdata->overlay_buffer2;
}
//Try to some colors http://en.wikipedia.org/wiki/YUV
int chrominance_offset = userdata->width * userdata->height;
int v_offset = chrominance_offset / 4;
int chroma = 0;
if (output_buffer) {
mmal_buffer_header_mem_lock(buffer);
memcpy(output_buffer->data, buffer->data, buffer->length);
// dim
int x, y;
for (x = 0; x < 600; x++) {
for (y = 0; y < 100; y++) {
if (local_overlay_buffer[(y * 600 + x) * 4] > 0) {
//copy luma Y
output_buffer->data[y * userdata->width + x ] = 0xdf;
//pointer to chrominance U/V
chroma= y / 2 * userdata->width / 2 + x / 2 + chrominance_offset;
//just guessing colors
output_buffer->data[chroma] = 0x38 ;
output_buffer->data[chroma+v_offset] = 0xb8 ;
}
}
}
output_buffer->length = buffer->length;
mmal_buffer_header_mem_unlock(buffer);
if (mmal_port_send_buffer(userdata->encoder_input_port, output_buffer) != MMAL_SUCCESS) {
fprintf(stderr, "ERROR: Unable to send buffer \n");
}
} else {
fprintf(stderr, "ERROR: mmal_queue_get (%d)\n", output_buffer);
}
if (frame_count % 10 == 0) {
// print framerate every n frame
clock_gettime(CLOCK_MONOTONIC, &t2);
float d = (t2.tv_sec + t2.tv_nsec / 1000000000.0) - (t1.tv_sec + t1.tv_nsec / 1000000000.0);
float fps = 0.0;
if (d > 0) {
fps = frame_count / d;
} else {
fps = frame_count;
}
userdata->fps = fps;
//fprintf(stderr, " Frame = %d, Framerate = %.1f fps \n", frame_count, fps);
}
mmal_buffer_header_release(buffer);
// and send one back to the port (if still open)
if (port->is_enabled) {
MMAL_STATUS_T status;
new_buffer = mmal_queue_get(pool->queue);
if (new_buffer) {
status = mmal_port_send_buffer(port, new_buffer);
}
if (!new_buffer || status != MMAL_SUCCESS) {
fprintf(stderr, "Error: Unable to return a buffer to the video port\n");
}
}
}
