Pi Eyes Stage 2

Right, in my last post I had got the raspberry pi camera modules up and running, but hit a bit of a blocker in terms of accessing the actual camera feeds in c++. Fortunately a very clever chap called Pierre Raufast had documented his reworking of the raspivid application here. It'd suffered a little over time, probably just due to newer versions of its dependencies so I redid some of his work and ended up with camcv.c, which sets up the camera and provides a point at which we can access each frame of the camera feed. My next task is to rewrite it from scratch, then experiment with decoding the data in an optimal way. First though, just so this post isn't entirely code - a picture of the latest setup:

My raspberry pi 'stereo camera rig'. Good old balsa wood and insulating tape.

Quick Instructions on getting the code

In this post I get to my first version of a working camera api, which can be downloaded here:

http://www.cheerfulprogrammer.com/downloads/pi_eyes_stage2/picam.zip

Note that to use it, you'll need to download and build the raspberry pi userland code from here. Mine is stored in /opt/vc/userland-master.

I'll go into more details once I have something I'm really happy with!

Writing the actual code...

The basic architecture of the camera system is quite simple once you get over the total lack of documentation of the fairly complex mmal layer... We basically:

• Start up mmal
• Create a 'camera component'
• Tell its 'video output port' to call a callback each time it fills in a new buffer
• In the callback we:
• Lock the buffer
• Read it
• Unlock it
• Give it back to be the port to be recycled
• And when all is done, we kill the camera component and bail out

The callback is called from a seperate thread, so once things are moving the main application can carry on as normal. This lends itself well to a simple initial api of just:

• StartCamera(some setup options + callback pointer)
• StopCamera()

The main bit of code I'm going to keep from the raspberry pi userland code is the raspicamcontrol stuff, which wraps up setting parameters on the camera in a simple api.

.... imagine moments of intense programming with blondie on in the background here ....

It's a few hours later and I've finished revision one. I've got a basic camera api that gets initialised, does stuff for a while then shuts down. Here's the first 'application' that uses it:

#include <stdio.h>
#include <unistd.h>
#include "camera.h"

void CameraCallback(CCamera* cam, const void* buffer, int buffer_length)
{
    printf("Do stuff with %d bytes of data\n",buffer_length);
}

int main(int argc, const char **argv)
{
    printf("PI Cam api tester\n");
    StartCamera(1280,720,30,CameraCallback);
    sleep(10);
    StopCamera();
}

Neat! With that code I run the application and get this print out

PI Cam api tester
Creating video port pool with 3 buffers of size 1382400
mmal: mmal_vc_port_parameter_set: failed to set port parameter 64:0:ENOSYS
mmal: Function not implemented
Sent buffer 0 to video port
Sent buffer 0 to video port
Sent buffer 0 to video port
Camera successfully created
Do stuff with 1382400 bytes of data
Do stuff with 1382400 bytes of data
Do stuff with 1382400 bytes of data
//... repeat every frame for 10 seconds...
Do stuff with 1382400 bytes of data
Do stuff with 1382400 bytes of data
Do stuff with 1382400 bytes of data

Shutting down camera

Most of the magic is inside 2 files:

• CameraControl.h and CameraControl.cpp - basically just a version of the original RaspiCamControl.c without command line parsing in.
• Camera.h and Camera.cpp - my new camera api

I've posted the full files online for download, but will go over the key bits here. The beefy one is the camera initialization - CCamera::Init

Camera Initialisation

Basic setup / creation of camera component

bool CCamera::Init(int width, int height, int framerate, CameraCBFunction callback)
{
    //init broadcom host - QUESTION: can this be called more than once??
    bcm_host_init();

    //store basic parameters
    Width = width;       
    Height = height;
    FrameRate = framerate;
    Callback = callback;

    // Set up the camera_parameters to default
    raspicamcontrol_set_defaults(&CameraParameters);

    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;

    //create the camera component
    status = mmal_component_create(MMAL_COMPONENT_DEFAULT_CAMERA, &camera);
    if (status != MMAL_SUCCESS)
    {
        printf("Failed to create camera component\n");
        return false;
    }

    //check we have output ports
    if (!camera->output_num)
    {
        printf("Camera doesn't have output ports");
        mmal_component_destroy(camera);
        return false;
    }

    //get the 3 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)
    {
        printf("Unable to enable control port : error %d", status);
        mmal_component_destroy(camera);
        return false;
    }

    //  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 = Width;
        cam_config.max_stills_h = Height;
        cam_config.stills_yuv422 = 0;
        cam_config.one_shot_stills = 0;
        cam_config.max_preview_video_w = Width;
        cam_config.max_preview_video_h = 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_RESET_STC;
        mmal_port_parameter_set(camera->control, &cam_config.hdr);
    }

This first section is pretty simple, albiet fairly long. It's just:

• Creating the mmal camera component
• Getting the 3 'output ports'. The main one we're interested in is the video port, but as far as I can tell the others still need setting up for correct operation.
• Enabling the 'control' port and providing a callback. This basically gives the camera a way of providing us with info about changes of state. Not doing anything with this yet though.
• Filling out a camera config structure, then sending it to the camera control port

Setting output port formats

Now we have a camera component running, the next step is to configure those output ports:

    // setup preview port format - QUESTION: Needed if we aren't using preview?
    format = preview_port->format;
    format->encoding = MMAL_ENCODING_OPAQUE;
    format->encoding_variant = MMAL_ENCODING_I420;
    format->es->video.width = Width;
    format->es->video.height = Height;
    format->es->video.crop.x = 0;
    format->es->video.crop.y = 0;
    format->es->video.crop.width = Width;
    format->es->video.crop.height = Height;
    format->es->video.frame_rate.num = FrameRate;
    format->es->video.frame_rate.den = 1;
    status = mmal_port_format_commit(preview_port);
    if (status != MMAL_SUCCESS)
    {
        printf("Couldn't set preview port format : error %d", status);
        mmal_component_destroy(camera);
        return false;
    }

    //setup video port format
    format = video_port->format;
    format->encoding = MMAL_ENCODING_I420; //not opaque, as we want to read it!
    format->encoding_variant = MMAL_ENCODING_I420; 
    format->es->video.width = Width;
    format->es->video.height = Height;
    format->es->video.crop.x = 0;
    format->es->video.crop.y = 0;
    format->es->video.crop.width = Width;
    format->es->video.crop.height = Height;
    format->es->video.frame_rate.num = FrameRate;
    format->es->video.frame_rate.den = 1;
    status = mmal_port_format_commit(video_port);
    if (status != MMAL_SUCCESS)
    {
        printf("Couldn't set video port format : error %d", status);
        mmal_component_destroy(camera);
        return false;
    }

    //setup still port format
    format = still_port->format;
    format->encoding = MMAL_ENCODING_OPAQUE;
    format->encoding_variant = MMAL_ENCODING_I420;
    format->es->video.width = Width;
    format->es->video.height = Height;
    format->es->video.crop.x = 0;
    format->es->video.crop.y = 0;
    format->es->video.crop.width = Width;
    format->es->video.crop.height = Height;
    format->es->video.frame_rate.num = 1;
    format->es->video.frame_rate.den = 1;
    status = mmal_port_format_commit(still_port);
    if (status != MMAL_SUCCESS)
    {
        printf("Couldn't set still port format : error %d", status);
        mmal_component_destroy(camera);
        return false;
    }

This is 3 almost identical bits of code - one for the preview port (which would be used for doing the full screen preview of the feed if we were using it), one for the video port (that's the one we're interested in) and one for the still port (presumably for capturing stills). If you read the code it's pretty much just plugging in the numbers provided to configure the camera. The most important part, highlighted in red is where we set the video port format to I420 encoding (the native format of the camera). By setting it correctly, this tells mmal that we will be providing a callback for the video output later, and it'll be wanting all the data thankyou very much! Otherwise it just passes in the buffer headers but no actual output... Point of note - I tried setting the format to ABGR, but the camera just output I420 data in a dodgy layout, so it's going to need converting.

Create a buffer pool for the video port to write to

    //setup video port buffer and a pool to hold them
    video_port->buffer_num = 3;
    video_port->buffer_size = video_port->buffer_size_recommended;
    MMAL_POOL_T* video_buffer_pool;
    printf("Creating video port pool with %d buffers of size %d\n", video_port->buffer_num, video_port->buffer_size);
    video_buffer_pool = mmal_port_pool_create(video_port, video_port->buffer_num, video_port->buffer_size);
    if (!video_buffer_pool)
    {
        printf("Couldn't create video buffer pool\n");
        mmal_component_destroy(camera);
        return false;    
    }

This little chunk is the first properly 'new' bit when compared to the raspivid. It creates a pool of buffers that we'll be providing to the video port to write the frames to. For now we're just creating it, but later we'll pass all the buffers to the video port and then begin capturing! The buffer_num is 3, as that gives you enough time to have the camera writing 1 buffer, while you read another, with an extra one in the middle for safety. The recommended buffer size comes from the format we specified earlier.

Enable stuff

    //enable the camera
    status = mmal_component_enable(camera);
    if (status != MMAL_SUCCESS)
    {
        printf("Couldn't enable camera\n");
        mmal_port_pool_destroy(video_port,video_buffer_pool);
        mmal_component_destroy(camera);
        return false;    
    }

    //apply all camera parameters
    raspicamcontrol_set_all_parameters(camera, &CameraParameters);

    //setup the video buffer callback
    status = mmal_port_enable(video_port, VideoBufferCallback);
    if (status != MMAL_SUCCESS)
    {
        printf("Failed to set video buffer callback\n");
        mmal_port_pool_destroy(video_port,video_buffer_pool);
        mmal_component_destroy(camera);
        return false;    
    }

This pretty simple code enables the camera, sends it a list of setup parameters using the cam control code, then enables the video port. Note the port enable call, which tells the video port about our VideoBufferCallback function, which we want calling for each frame received from the camera.

Give the buffers to the video port

    //send all the buffers in our pool to the video port ready for use
    {
        int num = mmal_queue_length(video_buffer_pool->queue);
        int q;
        for (q=0;q<num;q++)
        {
            MMAL_BUFFER_HEADER_T *buffer = mmal_queue_get(video_buffer_pool->queue);
            if (!buffer)
                printf("Unable to get a required buffer %d from pool queue", q);
            if (mmal_port_send_buffer(video_port, buffer)!= MMAL_SUCCESS)
                printf("Unable to send a buffer to encoder output port (%d)", q);
            printf("Sent buffer %d to video port\n");
        }
    }

OK, so this one looks a bit odd! The basic idea is that we created a pool of 3 buffers earlier, which is basically a queue of pointers to unused blocks of memory. This bit of code removes each buffer from the pool and sends it into the video port. In effect, we're handing the video port the blocks of memory it'll use to store frames in.

Begin capture and return SUCCESS!

    //begin capture
    if (mmal_port_parameter_set_boolean(video_port, MMAL_PARAMETER_CAPTURE, 1) != MMAL_SUCCESS)
    {
        printf("Failed to start capture\n");
        mmal_port_pool_destroy(video_port,video_buffer_pool);
        mmal_component_destroy(camera);
        return false;    
    }

    //store created info
    CameraComponent = camera;
    BufferPool = video_buffer_pool;

    //return success
    printf("Camera successfully created\n");
    return true;

As our final trick, we set the 'capturing' setting to 1, and if all goes well that VideoBufferCallback function should start getting called.

The video callback

What also deserves a mention is the video callback:

void CCamera::OnVideoBufferCallback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
{
    //check if buffer has data in
    if(buffer->length)
    {
        //got data so lock the buffer, call the callback so the application can use it, then unlock
        mmal_buffer_header_mem_lock(buffer);
        Callback(this,buffer->data,buffer->length);
        mmal_buffer_header_mem_unlock(buffer);
    }
    
    // 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(BufferPool->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 video port\n");
    }
}

The first bit should be fairly simple - we check if the buffer has any data in (hopefully it always does!), and if so, lock it, call the users callback (remember they passed it into the Init function) so the application can have it's merry way with the frame data, then unlock it.

The next bit of code is a little more confusing. It's the second part of the buffer management stuff we saw earlier. Once the buffer is used, we first release it. This frees it up and effectively puts it back in the pool from whence it came! However, the video port is now down a buffer, so (if its still open), we pull the buffer back out of the pool and send it back into the video port ready for reuse.

What is interesting here is that we have control over when the buffer is returned to the video port. I can see down the line doing stuff with the gpu, where I extend the life time of a buffer over a frame so compute shaders can do stuff with it!

What's Next?

Well, I can now read data, at 1080p/30hz if I want but the next question is what to do with it! Currently it's in the funky I420 encoding (basically a common form where 1 channel represents the 'brightness' and the other 2 channels represent the colour). To be useful it'll need converting to rgb, and displaying on screen. I know from Pierre's work that opencv isn't ideal for this, so while I'll need it for proper image processing I think I'll have a look at faster ways to get from camera output -> me on the tv!