int frameIsLastFrame (uint8_t *data)
{
int retVal = 0;
if (PAVE_CHECK (data))
{
parrot_video_encapsulation_t *PaVE = (parrot_video_encapsulation_t *)data;
if (PAVE_CTRL_LAST_FRAME_IN_STREAM & PaVE->control)
{
retVal = 1;
}
}
return retVal;
}
C_RESULT video_stage_merge_slices_transform(video_stage_merge_slices_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
bool_t switchBuffers;
bool_t dataReady = 0;
video_stage_merge_slices_buffer_t *buf;
int dataSize;
parrot_video_encapsulation_t *PaVE;
parrot_video_encapsulation_t *mergingPaVE;
out->size = 0;
if(out->status == VP_API_STATUS_INIT)
{
/* By default, feed the next stage with our local output */
out->numBuffers = 1;
out->buffers = cfg->bufs[0].bufferPointer;
out->buffers[0] = cfg->bufs[0].buffer;
out->indexBuffer = 0;
out->lineSize = 0;
out->status = VP_API_STATUS_PROCESSING;
}
// check input
PaVE = (parrot_video_encapsulation_t *) in->buffers[in->indexBuffer];
mergingPaVE = (parrot_video_encapsulation_t *) cfg->bufs[cfg->mergingBuffer].buffer;
VIDEO_SLICE_DEBUG("PAVE frm %d sl %d/%d %s\n ",
PaVE->frame_number,
PaVE->slice_index+1,
PaVE->total_slices,
(PaVE->frame_type == FRAME_TYPE_I_FRAME || PaVE->frame_type == FRAME_TYPE_IDR_FRAME) ? "I-frm":"p-frm" );
if ( (PaVE==NULL) || (!PAVE_CHECK(PaVE)) || (PaVE->total_slices == 1) )
{
// No slices, just send the data to the next stage
/* Feed the next stage with the previous stage's output */
out->buffers = in->buffers;
out->indexBuffer = in->indexBuffer;
out->lineSize = in->lineSize;
out->numBuffers = in->numBuffers;
out->status = in->status;
out->size = in->size;
out->status = VP_API_STATUS_PROCESSING;
/* Get rid of previously accumulated data */
video_stage_merge_slices_reset(cfg);
/* IPHONE DEBUG : set slices miss to 0/1 (to avoid keeping old/fake slice datas) */
DEBUG_totalSlices = 1;
DEBUG_nbSlices = 0;
return C_OK;
}
/*
* Check if the incoming PaVE belongs to the same frame.
*/
switchBuffers = (mergingPaVE) /* At program startup this buffer is empty and switching makes no sense */
&& ( !(pave_is_same_frame(PaVE,mergingPaVE)) );
/* If not the same frame, start building a new one */
if (switchBuffers)
{
if (0 != DEBUG_prevNumber)
{
int DEBUG_tempMiss = (PaVE->frame_number - (DEBUG_prevNumber + 1));
if (0 < DEBUG_tempMiss)
{
DEBUG_missed += DEBUG_tempMiss;
}
}
DEBUG_prevNumber = PaVE->frame_number;
cfg->readyBuffer = cfg->mergingBuffer;
cfg->mergingBuffer = (cfg->mergingBuffer+1)%2;
cfg->bufs[cfg->mergingBuffer].accumulated_size = 0;
cfg->bufs[cfg->mergingBuffer].nb_slices = 0;
}
/* Get a pointer to the buffer to store data to */
buf = &cfg->bufs[cfg->mergingBuffer];
// Check destination buffer size
if (buf->buffer_size < (buf->accumulated_size + in->size))
{
buf->buffer_size = buf->accumulated_size + in->size;
buf->buffer = vp_os_realloc(buf->buffer, buf->buffer_size );
if(!buf->buffer) { return C_FAIL; }
}
/* Copy data */
if (PaVE->slice_index == 0 || buf->accumulated_size==0)
{
/* Copy the entire packet */
if (buf->buffer) { vp_os_memcpy(buf->buffer, PaVE, in->size); }
buf->accumulated_size = in->size;
buf->nb_slices++;
}
else
{
#define mymin(x,y) ( ((x)<(y))?(x):(y) )
/* Copy only the payload */
dataSize = mymin(PaVE->payload_size,in->size); /* safety if the PaVE is corrupted */
if (buf->buffer) { vp_os_memcpy(buf->buffer + buf->accumulated_size, in->buffers[in->indexBuffer] + PaVE->header_size, dataSize); }
buf->accumulated_size += dataSize;
buf->nb_slices++;
}
/* Select the buffer to send to the next stage */
if (PaVE->slice_index == PaVE->total_slices - 1)
{
/* The buffer we just used for merging is ready to be sent */
cfg->readyBuffer = cfg->mergingBuffer;
cfg->mergingBuffer = (cfg->mergingBuffer+1)%2;
cfg->bufs[cfg->mergingBuffer].accumulated_size = 0;
cfg->bufs[cfg->mergingBuffer].nb_slices = 0;
dataReady = 1;
}
else if (switchBuffers)
{
/* Send the previous buffer if it contains something */
dataReady = 1;
}
// If slice was the last one, continue to next stage
if (dataReady)
{
buf = &cfg->bufs[cfg->readyBuffer];
if (buf->buffer && buf->accumulated_size)
{
int totalSlices;
parrot_video_encapsulation_t *newPaVE = (parrot_video_encapsulation_t *) buf->buffer;
/* Save the old value of total slices */
totalSlices = newPaVE->total_slices;
newPaVE->payload_size = buf->accumulated_size - PaVE->header_size;
newPaVE->slice_index = 0;
newPaVE->total_slices = 1;
/* Feed the next stage with our local output */
out->size = buf->accumulated_size;
out->buffers = buf->bufferPointer;
out->buffers[0] = buf->buffer;
out->indexBuffer = 0;
out->numBuffers = 1;
out->lineSize = 0;
out->status = VP_API_STATUS_PROCESSING;
if(!PAVE_CHECK(buf->buffer)) { printf("%s:%d - No PaVE !\n",__FUNCTION__,__LINE__); assert(0==1); }
if (buf->nb_slices != totalSlices) { printf("Missing slices (%d)\n",totalSlices-buf->nb_slices); }
DEBUG_nbSlices = 0.9*DEBUG_nbSlices + 0.1*(totalSlices-buf->nb_slices);
DEBUG_totalSlices = 0.9*DEBUG_totalSlices + 0.1*totalSlices;
}
}
else
{
out->size = 0;
}
if (out->size)
{
PaVE = (parrot_video_encapsulation_t*) out->buffers[0];
VIDEO_SLICE_DEBUG("Switch %d - Sending - PAVE frm %d sl %d/%d\n ",
switchBuffers,
PaVE->frame_number,
PaVE->slice_index+1,
PaVE->total_slices );}
return C_OK;
}
C_RESULT video_stage_encoded_recorder_transform(video_stage_encoded_recorder_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
vp_os_mutex_lock (&out->lock);
vp_os_mutex_lock (&cfg->videoMutex);
// Copy in to out
if (NULL != in && NULL != out)
{
out->numBuffers = in->numBuffers;
out->indexBuffer = in->indexBuffer;
out->lineSize = in->lineSize;
out->size = in->size;
out->status = in->status;
out->buffers = in->buffers;
out->status = VP_API_STATUS_PROCESSING;
}
uint8_t *slice = in->buffers[in->indexBuffer];
parrot_video_encapsulation_t *PaVE = (parrot_video_encapsulation_t *)slice;
if (cfg->lastStreamId == PaVE->stream_id)
{
VER_PRINT ("Got a video slice from an old stream, skipping\n");
vp_os_mutex_unlock (&cfg->videoMutex);
vp_os_mutex_unlock (&out->lock);
return C_OK;
}
if(cfg->startRec == VIDEO_ENCODED_START_RECORD)
{
ardrone_video_error_t vError = ARDRONE_VIDEO_NO_ERROR;
cfg->video = ardrone_video_start (cfg->video_filename, cfg->fps, VIDEO_FORMAT, &vError);
if (ARDRONE_VIDEO_SUCCEEDED (vError) && NULL != cfg->video)
{
cfg->startRec=VIDEO_ENCODED_RECORDING;
cfg->currentStreamId = PaVE->stream_id;
}
else
{
VER_PRINT ("An error occured when creating video");
cfg->startRec=VIDEO_ENCODED_STOPPED;
}
}
if((cfg->startRec == VIDEO_ENCODED_RECORDING) ||
(cfg->startRec == VIDEO_ENCODED_WAITING_STREAM_END))
{
#if USE_LINUX
/* Quick and dirty debugging inside AR.Drone Navigation */
if (PAVE_CHECK(slice))
{
parrot_video_encapsulation_t * pave = (parrot_video_encapsulation_t*) slice;
printf("Recording frame %dx%d num %d \n\033[1A",pave->display_width,pave->display_height,pave->frame_number);
if (pave->control & PAVE_CTRL_LAST_FRAME_IN_STREAM) {
printf("\n\nEnd of stream PaVE received !\n\n");
}
}
#endif
ardrone_video_error_t vError = ardrone_video_addSlice (cfg->video, slice);
if (ARDRONE_VIDEO_FAILED (vError))
{
char errBuf [50] = {0};
ardrone_video_error_string (vError, errBuf, 50);
VER_PRINT ("Error while recording frame : %s", errBuf);
ardrone_video_cleanup (&(cfg->video));
cfg->startRec = VIDEO_ENCODED_STOPPED;
}
if (1 == frameIsLastFrame (slice))
{
VER_PRINT ("Received last frame for current stream, finishing video\n");
video_stage_encoded_recorder_finish (cfg);
}
}
vp_os_mutex_unlock (&cfg->videoMutex);
vp_os_mutex_unlock (&out->lock);
return C_OK;
}
