working ARDrone video capture example

#include <stdio.h>
#include <stdint.h>

#include <sys/socket.h>
#include <sys/time.h>
#include <netinet/in.h>
#include <netdb.h>
#include <arpa/inet.h>
#include <fcntl.h>

#include <SDL2/SDL.h>

#define DRONE_IP_ADDR "192.168.1.1"
#define DRONE_VID_STREAM_PORT "5555"

extern "C" {
#include <libavcodec/avcodec.h>
#include <libswscale/swscale.h>
#include <libavformat/avformat.h>
}

#define VIDEO_BUFFER_SIZE 40000
#define PaVE_HEADER_LENGTH 68

// Time collection globals
double payload_time = 0;
double decoder_time = 0;
double converter_time = 0;
double gui_time = 0;


typedef struct { //PaVE
uint8_t signature[4]; /* "PaVE" - used to identify the start of
 frame */
uint8_t version; /* Version code */
uint8_t video_codec; /* Codec of the following frame */
uint16_t header_size; /* Size of the parrot_video_encapsulation_t */
uint32_t payload_size; /* Amount of data following this PaVE */
uint16_t encoded_stream_width; /* ex: 640 */
uint16_t encoded_stream_height; /* ex: 368 */
uint16_t display_width; /* ex: 640 */
uint16_t display_height; /* ex: 360 */
uint32_t frame_number; /* Frame position inside the current stream
 */
uint32_t timestamp; /* In milliseconds */
uint8_t total_chuncks; /* Number of UDP packets containing the
 current decodable payload - currently unused */
uint8_t chunck_index; /* Position of the packet - first chunk is #0
 - currenty unused*/
uint8_t frame_type; /* I-frame, P-frame -
 parrot_video_encapsulation_frametypes_t */
uint8_t control; /* Special commands like end
    -of-stream or
    advertised frames */
    uint32_t stream_byte_position_lw; /* Byte position of the current payload in
     the encoded stream - lower 32-bit word */
    uint32_t stream_byte_position_uw; /* Byte position of the current payload in
     the encoded stream - upper 32-bit word */
    uint16_t stream_id; /* This ID indentifies packets that should be
     recorded together */
    uint8_t total_slices; /* number of slices composing the current
     frame */
    uint8_t slice_index; /* position of the current slice in the frame
     */
    uint8_t header1_size; /* H.264 only : size of SPS inside payload -
     no SPS present if value is zero */
    uint8_t header2_size; /* H.264 only : size of PPS inside payload -
     no PPS present if value is zero */
    uint8_t reserved2[2]; /* Padding to align on 48 bytes */
    uint32_t advertised_size; /* Size of frames announced as advertised
     frames */
    uint8_t reserved3[12]; /* Padding to align on 64 bytes */
    uint8_t reserved4[4]; // padding -- added b/c it was in the KIPR library code
}__attribute__ ((packed)) parrot_video_encapsulation_t;

typedef enum { //PaVE codec IDs
    CODEC_UNKNNOWN = 0,
    CODEC_VLIB,
    CODEC_P264,
    CODEC_MPEG4_VISUAL,
    CODEC_MPEG4_AVC
} parrot_video_encapsulation_codecs_t;

typedef enum { //PaVE frame types
    FRAME_TYPE_UNKNNOWN = 0, FRAME_TYPE_IDR_FRAME, /* headers followed by I-frame */
    FRAME_TYPE_I_FRAME, FRAME_TYPE_P_FRAME, FRAME_TYPE_HEADERS
} parrot_video_encapsulation_frametypes_t;

void printPaVE(parrot_video_encapsulation_t PaVE) {
    printf("\n---------------------------\n");

    printf("Codec : %s\n",
            (PaVE.video_codec == CODEC_MPEG4_VISUAL) ?
                    "MP4" :
                    ((PaVE.video_codec == CODEC_MPEG4_AVC) ? "H264" : "Unknown"));

    printf("StreamID : %d \n", PaVE.stream_id);
    printf("Timestamp : %d ms\n", PaVE.timestamp);
    printf("Encoded dims : %d x %d\n", PaVE.encoded_stream_width,
            PaVE.encoded_stream_height);
    printf("Display dims : %d x %d\n", PaVE.display_width, PaVE.display_height);
    ////printf ("Header size  : %d (PaVE size : %d)\n", PaVE.header_size, sizeof (parrot_video_encapsulation_t));
    printf("Header size : %d\n", PaVE.header_size);
    printf("Payload size : %d\n", PaVE.payload_size);
    printf("Size of SPS inside payload : %d\n", PaVE.header1_size);
    printf("Size of PPS inside payload : %d\n", PaVE.header2_size);
    printf("Slices in the frame : %d\n", PaVE.total_slices);
    printf("Frame Type / Number : %s : %d : slide %d/%d\n",
            (PaVE.frame_type == FRAME_TYPE_P_FRAME) ?
                    "P-Frame" :
                    ((PaVE.frame_type == FRAME_TYPE_I_FRAME) ?
                            "I-Frame" : "IDR-Frame"), PaVE.frame_number,
            PaVE.slice_index + 1, PaVE.total_slices);

    printf("---------------------------\n\n");
}

int receive(int socketNumber, unsigned char *buffer, int requestSize) {
    int lengthReceived = -1;
    lengthReceived = recv(socketNumber, buffer, requestSize, 0);
    if (lengthReceived < 0) {
        printf("failed to receive assumed PaVE packet\n");
    } else {
        printf("asked for %i bytes, received packet of %i bytes\n", requestSize,
                lengthReceived);
    }
    return lengthReceived;
}

void fetch_and_decode(int socketNumber,AVCodecContext *codecContext, AVFrame *picture) {

    double t = clock();

    parrot_video_encapsulation_t PaVE;
    unsigned char part[VIDEO_BUFFER_SIZE];
    int partLength = -1;
    partLength = receive(socketNumber, part, VIDEO_BUFFER_SIZE);
    if (partLength < 0) {
        printf("did not receive video data\n");
        return;
    }

    memcpy(&PaVE, part, sizeof(parrot_video_encapsulation_t));

    if (strncmp((const char*) PaVE.signature, "PaVE", 4) != 0) {
        printf("PaVE not synchronized, skipping iteration\n");
        return;
    } else {
        printf("PaVE synchronized. YIPEEEEEEEEEEEEEEEEEEEEEEEE\n");
        printPaVE(PaVE);
    }

    uint32_t read = 0;
    unsigned char payload[VIDEO_BUFFER_SIZE];

    memcpy(payload, part + PaVE.header_size, partLength - PaVE.header_size);
    read += partLength - PaVE.header_size;

    double lastTime = clock();


    while (read < PaVE.payload_size && (clock() - lastTime) < 0.1) {
        int payloadLength = -1;
        printf("gathering payload...\n");
        payloadLength = receive(socketNumber, payload + read,
                PaVE.payload_size - read);
        read += payloadLength;
        lastTime = clock();
    }

    if (read < PaVE.payload_size) {
        return; // if the while loop times out before a full frame, need to exit
    }

    payload_time += clock() - t;

    printf("payload complete, attempting to decode frame\n");

    AVPacket avPkt;
    av_init_packet(&avPkt);
    avPkt.data = NULL;
    avPkt.size = 0;

    avPkt.data = payload;
    avPkt.size = PaVE.payload_size;

    printf("avPkt.size = %d\n",avPkt.size);

    int done = 0;
    int ret = -1;

    t = clock();

    ret = avcodec_decode_video2(codecContext, picture, &done, &avPkt);

    decoder_time += clock() - t;

    avPkt.data = NULL;
    avPkt.size = 0;
    av_free_packet(&avPkt);

    if (done == 0 || ret < 0) {
        printf("could not decode frame\n");
        return;
    }


}

///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////

int main() {

    if (SDL_Init(SDL_INIT_VIDEO) < 0) {
        fprintf(stderr, "Unable to init SDL: %s\n", SDL_GetError());
        return -1;
    }

    printf("\n\n*********************** START ***********************\n\n");

    printf("size of PaVE = %d\n",sizeof(parrot_video_encapsulation_t));

    int socketNumber;
    sockaddr_in myAddr;
    sockaddr_in droneAddr;

    // my sockaddr_in
    myAddr.sin_family = AF_INET;
    myAddr.sin_addr.s_addr = INADDR_ANY; // my IP address
    myAddr.sin_port = htons(atoi(DRONE_VID_STREAM_PORT));

    // the drone's sockaddr_in
    droneAddr.sin_family = AF_INET;
    droneAddr.sin_addr.s_addr = inet_addr(DRONE_IP_ADDR);
    droneAddr.sin_port = htons(atoi(DRONE_VID_STREAM_PORT));

    socketNumber = socket(AF_INET, SOCK_STREAM, 0);

    // bind the socket
    if (bind(socketNumber, (sockaddr*) &myAddr, sizeof(sockaddr_in)) < 0) {
        printf("failed to bind socket\n");
    }

    int result = -1;
    result = connect(socketNumber, (sockaddr*) &droneAddr, sizeof(sockaddr_in));
    if (result != 0) {
        printf("connection NOT established\n");
    }

    printf("booting...\n");
    // send one packet of "some bytes" to drone
    uint8_t gateKeyPkt[4] = { 0x01, 0x00, 0x00, 0x00 };
    int badCheck = -1;
    badCheck = sendto(socketNumber, (void*) gateKeyPkt, 4, 0,
            (sockaddr*) &droneAddr, sizeof(sockaddr_in));

    if (badCheck < 0) {
        printf("Failed to send basic packet\n");
    }

    AVCodec *codec = NULL;
    AVCodecContext *codecContext = NULL;
    AVFrame *picture = NULL;
    AVFrame *pictureRGB = NULL;
    AVFrame *pictureYUV = NULL;
    SwsContext *convertContext_RGB = NULL;
    SwsContext *convertContext_YUV = NULL;

    uint8_t *buffer_RGB = NULL;
    uint8_t *buffer_YUV = NULL;

    // set up codec
    avcodec_register_all();
    av_register_all();
    av_log_set_level(AV_LOG_DEBUG);
    codec = avcodec_find_decoder(CODEC_ID_H264);
    codec->id = CODEC_ID_H264;

    // set up codecContext
    codecContext = avcodec_alloc_context3(codec);
    avcodec_open2(codecContext, codec, 0);
    codecContext->width = 640;
    codecContext->height = 360;
    codecContext->pix_fmt = PIX_FMT_YUV420P;
    codecContext->skip_frame = AVDISCARD_DEFAULT;
    codecContext->error_concealment = FF_EC_GUESS_MVS | FF_EC_DEBLOCK;
    codecContext->skip_loop_filter = AVDISCARD_DEFAULT;
    codecContext->workaround_bugs = FF_BUG_AUTODETECT;
    codecContext->debug = true;

    // allocate video frames and buffer
    picture = avcodec_alloc_frame();
    pictureRGB = avcodec_alloc_frame();
    pictureYUV = avcodec_alloc_frame();

    if (pictureYUV == NULL) {
        printf("Could not allocate YUV frame\n");
        return -1;
    }

    int RGB_size = avpicture_get_size(PIX_FMT_RGB24, codecContext->width,codecContext->height);
    int YUV_size = avpicture_get_size(PIX_FMT_YUV420P, codecContext->width,codecContext->height);
    buffer_RGB = (uint8_t*) av_malloc(RGB_size* sizeof(uint8_t));
    buffer_YUV = (uint8_t*) av_malloc(YUV_size* sizeof(uint8_t));

    // assign parts of buffer to image planes in RGB frame
    avpicture_fill((AVPicture*) pictureRGB, buffer_RGB, PIX_FMT_RGB24,
            codecContext->width, codecContext->height);
    avpicture_fill((AVPicture*) pictureYUV, buffer_YUV, PIX_FMT_YUV420P,
            codecContext->width, codecContext->height);

    // convert context
    convertContext_RGB = sws_getContext(codecContext->width, codecContext->height,
            codecContext->pix_fmt, codecContext->width, codecContext->height,
            PIX_FMT_RGB24, SWS_SPLINE, 0, 0, 0);
    convertContext_YUV = sws_getContext(codecContext->width, codecContext->height,
            codecContext->pix_fmt, codecContext->width, codecContext->height,
            PIX_FMT_YUV420P, SWS_SPLINE, NULL, NULL, NULL);

    //SDL stuff

    SDL_Window *window;
    SDL_Renderer *render;
    SDL_Texture *bmpTexture;
    uint8_t *pixels;
    int pitch;
    int size;
    // allocate window, renderer, texture
    window = SDL_CreateWindow("YUV",SDL_WINDOWPOS_UNDEFINED,SDL_WINDOWPOS_UNDEFINED,
                              codecContext->width,codecContext->height,SDL_WINDOW_SHOWN);
    render = SDL_CreateRenderer(window,-1,SDL_RENDERER_ACCELERATED);
    bmpTexture = SDL_CreateTexture(render,SDL_PIXELFORMAT_YV12,SDL_TEXTUREACCESS_STREAMING,
                                   codecContext->width,codecContext->height);
    size = codecContext->width*codecContext->height;
    if ((window == NULL) | (render == NULL) | (bmpTexture == NULL)) {
        printf("Could not open SDL window\n");
        return -1;
    }

    for (int j = 0; j < 1000; j++) {
        fetch_and_decode(socketNumber, codecContext, picture);

        double t = clock();

        sws_scale(convertContext_YUV,
                  picture->data,
                  picture->linesize,
                  0,
                  codecContext->height,
                  pictureYUV->data,
                  pictureYUV->linesize);

        converter_time += clock() - t;

        //memcpy's hardcoded for YUV only!!!
        SDL_LockTexture(bmpTexture,NULL,(void**)&pixels,&pitch);
        memcpy(pixels,pictureYUV->data[0],size);
        memcpy(pixels+size,pictureYUV->data[2],size/4);
        memcpy(pixels+size*5/4,pictureYUV->data[1],size/4);
        SDL_UnlockTexture(bmpTexture);
        SDL_UpdateTexture(bmpTexture,NULL,pixels,pitch);
        SDL_RenderClear(render);
        SDL_RenderCopy(render,bmpTexture,NULL,NULL);
        SDL_RenderPresent(render);

    }

    avcodec_close(codecContext);
    av_free(codecContext);
    av_free(picture);
    av_free(pictureRGB);
    av_free(buffer_RGB);
    av_free(buffer_YUV);
    av_free(pictureYUV);
    sws_freeContext(convertContext_RGB);
    sws_freeContext(convertContext_YUV);

    double payload_time_avg = payload_time / 1000./CLOCKS_PER_SEC;
    double decoder_time_avg = decoder_time / 1000./CLOCKS_PER_SEC;
    double converter_time_avg = converter_time / 1000./CLOCKS_PER_SEC;

    printf("Average time per payload = %f seconds\n",payload_time_avg);
    printf("Average time per decode = %f seconds\n",decoder_time_avg);
    printf("Average time per conversion = %f seconds\n",converter_time_avg);

    return 1;
}