before porting video_stage.c from new examples

ARDroneLib/Soft/Lib/ardrone_tool/ardrone_tool.c

@@ -297,7 +297,7 @@ C_RESULT ardrone_tool_shutdown()
 }
 
 #include <locale.h>
-int ardrone_tool_main(int argc, char **argv)
+int ardrone_tool_main(int argc, char **argv, int run_loop)
 {
   C_RESULT res;
   const char* old_locale;
@@ -404,13 +404,15 @@ int ardrone_tool_main(int argc, char **argv)
 		appname = &argv[0][lastSlashPos+1];
 		ardrone_gen_appid (appname, __SDK_VERSION__, app_id, app_name, sizeof (app_name));
 		res = ardrone_tool_init(wifi_ardrone_ip, strlen(wifi_ardrone_ip), NULL, appname, NULL, NULL, NULL, MAX_FLIGHT_STORING_SIZE, NULL);
+        if (run_loop == 1)
+        {
+        while( SUCCEED(res) && ardrone_tool_exit() == FALSE )
+        {
+            res = ardrone_tool_update();
+        }
 
-      while( SUCCEED(res) && ardrone_tool_exit() == FALSE )
-      {
-        res = ardrone_tool_update();
-      }
-
-      res = ardrone_tool_shutdown();
+        res = ardrone_tool_shutdown();
+        }
     }
 
   if( old_locale != NULL )

ARDroneLib/Soft/Lib/ardrone_tool/ardrone_tool.h

@@ -52,7 +52,7 @@ extern void ardrone_tool_display_cmd_line_custom( void ) WEAK;
 extern bool_t ardrone_tool_parse_cmd_line_custom( const char* cmd ) WEAK;
 
 // This is implemented by the library
-int ardrone_tool_main(int argc, char**argv);
+int ardrone_tool_main(int argc, char**argv, int run_loop);
 C_RESULT ardrone_tool_init(const char* ardrone_ip, size_t n,
 		AT_CODEC_FUNCTIONS_PTRS *ptrs, const char *appname,
 		const char *usrname, const char *rootdir, const char *flightdir,
@@ -66,7 +66,7 @@ C_RESULT ardrone_tool_shutdown(void);
 
 void ardrone_tool_init_timers_and_mutex();
 void ardrone_tool_send_com_watchdog(void); // To send it only once
-int main();
+//int main();
 
 // There because not defined in embedded
 void api_configuration_get_ctrl_mode(void);

src/ardrone_driver.cpp

@@ -93,9 +93,9 @@ void ARDroneDriver::run()
 				ARDRONE_TOOL_CONFIGURATION_ADDEVENT (enemy_without_shell, &detect_indoor_hull, NULL);
 			}
 		}
-		if (current_frame_id != last_frame_id)
+		if (true) //(current_frame_id != last_frame_id)
 		{
-			publish_video();
+			//publish_video();
 			publish_navdata();
 			last_frame_id = current_frame_id;
 		}
@@ -327,7 +327,8 @@ void ARDroneDriver::publish_navdata()
 // custom_main
 ////////////////////////////////////////////////////////////////////////////////
 
-extern "C" int custom_main(int argc, char** argv)
+//extern "C" int custom_main(int argc, char** argv)
+int main(int argc, char** argv)
 {
 	int res = ardrone_tool_setup_com( NULL );
 
@@ -343,10 +344,10 @@ extern "C" int custom_main(int argc, char** argv)
 	else
 	{
 		//ardrone_tool_init(argc, argv);
-        ardrone_tool_main(argc, argv);
-		ros::init(argc, argv, "ardrone_driver");
-
+        ros::init(argc, argv, "ardrone_driver");
+        ardrone_tool_main(argc, argv, 0);
 		ARDroneDriver().run();
+        
 	}
 
 	return 0;

src/ardrone_sdk.cpp

@@ -9,10 +9,26 @@ navdata_vision_detect_t navdata_detect;
 navdata_time_t arnavtime;
 
 extern "C" {
-
+    //USE PROTO_ for defining if needed
+//    DEFINE_THREAD_ROUTINE(mani , data)
+//    {
+//        while (1) {printf("Hey ...\n");}
+//        return 0;
+//    }
+    
 	C_RESULT ardrone_tool_init_custom(void) {
+        if (IS_ARDRONE2)
+        {
+            ardrone_application_default_config.video_codec = H264_360P_CODEC;
+        }
+        else
+        {
+            ardrone_application_default_config.video_codec = UVLC_CODEC;
+        }
+        ardrone_application_default_config.bitrate_ctrl_mode = 1;
 		ardrone_tool_input_add(&teleop);
 		START_THREAD(video_update_thread, 0);
+        //START_THREAD(mani, 0);
 		return C_OK;
 	}
 
@@ -25,7 +41,6 @@ extern "C" {
 		navdata_phys = pnd->navdata_phys_measures;
 		navdata = pnd->navdata_demo;
 		arnavtime = pnd->navdata_time;
-
 		return C_OK;
 	}
 
@@ -35,6 +50,7 @@ extern "C" {
 
 	BEGIN_THREAD_TABLE
 	THREAD_TABLE_ENTRY(video_update_thread, 20)
+    //THREAD_TABLE_ENTRY(mani, 20)
 	THREAD_TABLE_ENTRY(navdata_update, 20)
 	THREAD_TABLE_ENTRY(ATcodec_Commands_Client, 20)
 	THREAD_TABLE_ENTRY(ardrone_control, 20)

src/ardrone_sdk.h

@@ -1,6 +1,17 @@
 #ifndef _ARDRONE_SDK_H_
 #define _ARDRONE_SDK_H_
 
+#include <stdio.h>
+#include <stdlib.h>
+#include <ctype.h>
+#include <termios.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <sys/time.h>
+#include <time.h>
+
 extern "C" {
 #include <VP_Api/vp_api.h>
 #include <VP_Api/vp_api_error.h>
@@ -26,8 +37,9 @@ extern "C" {
 #else
 #include <VLIB/Stages/vlib_stage_decode.h>
 #endif
+#include <video_encapsulation.h>
 
-#include <ardrone_tool/ardrone_tool.h>
+#include <ardrone_tool/ardrone_version.h>
 #include <ardrone_tool/ardrone_tool_configuration.h>  
 #include <ardrone_tool/Com/config_com.h>
 #include <ardrone_tool/UI/ardrone_input.h>

src/video.cpp

@@ -13,7 +13,9 @@ extern "C" C_RESULT export_stage_open( void *cfg, vp_api_io_data_t *in, vp_api_i
 
 extern "C" C_RESULT export_stage_transform( void *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
 {
-	memcpy(buffer, in->buffers[0], STREAM_WIDTH * STREAM_HEIGHT * 3);
+    fprintf(stderr, "In Transform before copy\n");
+	//memcpy(buffer, in->buffers[0], STREAM_WIDTH * STREAM_HEIGHT * 3);
+    printf("In Transform after copy\n");
 	current_frame_id++;
  	return (SUCCESS);
 }
@@ -33,6 +35,7 @@ const vp_api_stage_funcs_t vp_stages_export_funcs =
 
 DEFINE_THREAD_ROUTINE(video_update_thread, data)
 {
+    printf("Starting video capture thread ...\n");
   C_RESULT res;
 
   vp_api_io_pipeline_t    pipeline;

src/video.h

@@ -3,8 +3,8 @@
 
 #include "ardrone_sdk.h"
 
-#define STREAM_WIDTH QVGA_WIDTH
-#define STREAM_HEIGHT QVGA_HEIGHT
+#define STREAM_WIDTH 640
+#define STREAM_HEIGHT 360
 
 //Vertical Camera standalone
 #define VERTSTREAM_WIDTH 174
@@ -18,10 +18,17 @@
 #define MODE3_PIP_WIDTH 58
 #define MODE3_PIP_HEIGHT 42
 
+extern video_com_multisocket_config_t icc;
+extern parrot_video_encapsulation_codecs_t video_stage_decoder_lastDetectedCodec;
 extern const vp_api_stage_funcs_t vp_stages_export_funcs;
 extern unsigned char buffer[]; // size STREAM_WIDTH * STREAM_HEIGHT * 3
 extern int current_frame_id; // this will be incremented for every frame
 
+static int32_t pixbuf_width = 0;
+static int32_t pixbuf_height = 0;
+static int32_t pixbuf_rowstride = 0;
+static uint8_t* pixbuf_data = NULL;
+
 PROTO_THREAD_ROUTINE(video_update_thread, data);
 
 #endif