From ca25991889799e1d0c38e13b732be5103084f85b Mon Sep 17 00:00:00 2001
From: Laurent Eschenauer <laurent@eschenauer.be>
Date: Sun, 2 Jun 2013 10:54:40 +0200
Subject: [PATCH] Added video and PaVE header recording to blackbox

---
 plugins/blackbox/index.js | 53 ++++++++++++++++++++++++++++++++++++---
 1 file changed, 50 insertions(+), 3 deletions(-)

diff --git a/plugins/blackbox/index.js b/plugins/blackbox/index.js
index d3e014b..6c99113 100644
--- a/plugins/blackbox/index.js
+++ b/plugins/blackbox/index.js
@@ -1,12 +1,15 @@
 var fs = require('fs');
 var path = require('path');
 var df = require('dateformat');
+var PaVEParser = require('ar-drone/lib/video/PaVEParser')
 
 var client
   , io
   , config
   , navStream
   , motionStream
+  , videoStream
+  , paveStream
   , recording = false
   ;
 
@@ -16,6 +19,7 @@ function blackbox(name, deps) {
     io = deps.io;
     config = deps.config;
 
+    // Listen to user actions to start/stop recording
     deps.io.sockets.on('connection', function (socket) {
         socket.on('/blackbox/start', function (cmd) {
             _start();
@@ -25,10 +29,18 @@ function blackbox(name, deps) {
         });
     });
 
+    // Listen to navdata and process them
     deps.client.on('navdata', function(data) {
-        _record(data);
+        _writeNavData(data);
     });
 
+    // Process the videostream
+    var parser = new PaVEParser();
+    parser.on('data', function(data) {
+        _writeVideo(data);
+    })
+    client.getVideoStream().pipe(parser);
+
 };
 
 function _start() {
@@ -42,6 +54,8 @@ function _start() {
    var folder = df(new Date(), "yyyy-mm-dd_hh-MM-ss");
    fs.mkdir(path.join(root, folder), function() {
       navStream = fs.createWriteStream(path.join(root, folder, 'navdata.txt'));
+      videoStream = fs.createWriteStream(path.join(root, folder, 'video.h264'));
+      paveStream = fs.createWriteStream(path.join(root, folder, 'paveHeaders.txt'));
       motionStream = fs.createWriteStream(path.join(root, folder, 'motion.txt')); 
       motionStream.write("seq,pitch,roll,yaw,xVelocity,yVelocity,zVelocity,altitude\n", function() {
           recording = true;
@@ -56,10 +70,12 @@ function _stop() {
     recording = false;
     navStream.end();
     motionStream.end();
+    paveStream.end();
+    videoStream.end();
     io.sockets.emit('/message', "Blackbox stopped recording NavData.");
 }
 
-function _record(data) {
+function _writeNavData(data) {
     if (!recording) return;
 
     navStream.write(JSON.stringify(data) + "\n");
@@ -77,7 +93,38 @@ function _record(data) {
     motionStream.write(seq + "," + pitch + "," + roll + "," + yaw + "," + vx + "," + vy + "," + vz + "," + z + "\n");
 }
 
-function _writeHeader(stream, cb) {
+function _writeVideo(data) {
+    if (!recording) return;
+
+    videoStream.write(data.payload);
+    
+    var header = {
+          signature               : data.signature,
+          version                 : data.version,
+          video_codec             : data.video_codec,
+          header_size             : data.header_size,
+          payload_size            : data.payload_size,
+          encoded_stream_width    : data.encoded_stream_width,
+          encoded_stream_height   : data.encoded_stream_height,
+          display_width           : data.display_width,
+          display_height          : data.display_height,
+          frame_number            : data.frame_number,
+          timestamp               : data.timestamp,
+          total_chunks            : data.total_chunks,
+          chunk_index             : data.chunk_index,
+          frame_type              : data.frame_type,
+          control                 : data.control,
+          stream_byte_position_lw : data.stream_byte_position_lw,
+          stream_byte_position_uw : data.stream_byte_position_uw,
+          stream_id               : data.stream_id,
+          total_slices            : data.total_slices,
+          slice_index             : data.slice_index,
+          header1_size            : data.header1_size,
+          header2_size            : data.header2_size,
+          advertised_size         : data.advertised_size,
+    }
+
+    paveStream.write(JSON.stringify(header) + "\n");
 }
 
 module.exports = blackbox;