Add custom_test folder

Examples/Linux/Makefile

@@ -4,6 +4,7 @@
 all:
 	@$(MAKE) -C ../../ARDroneLib/Soft/Build USE_LINUX=yes
 	@$(MAKE) -C Navigation/Build USE_LINUX=yes 
+	@$(MAKE) -C custom_test/Build USE_LINUX=yes
 	@$(MAKE) -C sdk_demo/Build USE_LINUX=yes
 	@$(MAKE) -C video_demo/Build USE_LINUX=yes
 #	@$(MAKE) -C Testbenches/multiconfiguration/Build USE_LINUX=yes
@@ -12,6 +13,7 @@ all:
 $(MAKECMDGOALS):
 	@$(MAKE) -C ../../ARDroneLib/Soft/Build USE_LINUX=yes $(MAKECMDGOALS)
 	@$(MAKE) -C Navigation/Build USE_LINUX=yes $(MAKECMDGOALS)
+	@$(MAKE) -C custom_test/Build USE_LINUX=yes $(MAKECMDGOALS)
 	@$(MAKE) -C sdk_demo/Build USE_LINUX=yes $(MAKECMDGOALS)
 	@$(MAKE) -C video_demo/Build USE_LINUX=yes $(MAKECMDGOALS)
 #	@$(MAKE) -C Testbenches/multiconfiguration/Build USE_LINUX=yes $(MAKECMDGOALS)

Examples/Linux/custom_test/Build/Makefile

+SDK_PATH:=$(shell pwd)/../../../../ARDroneLib
+PC_TARGET=yes
+USE_LINUX=yes
+
+ifdef MYKONOS
+   include $(ARDRONE_CUSTOM_CONFIG)
+   include $(ARDRONE_BUILD_CONFIG)
+else
+   include $(SDK_PATH)/Soft/Build/custom.makefile
+   include $(SDK_PATH)/Soft/Build/config.makefile
+endif
+
+ifeq "$(RELEASE_BUILD)" "yes"
+   ARDRONE_TARGET_DIR=$(shell pwd)/../../Build/Release
+else
+   ARDRONE_TARGET_DIR=$(shell pwd)/../../Build/Debug
+endif
+
+TARGET=linux_custom_test
+
+SRC_DIR:=$(shell pwd)/../Sources
+
+# Define application source files
+GENERIC_BINARIES_SOURCE_DIR:=$(SRC_DIR)
+
+GENERIC_BINARIES_COMMON_SOURCE_FILES+=			\
+   Navdata/navdata.c    \
+   Video/video_stage.c
+
+GENERIC_INCLUDES+=					\
+	$(SRC_DIR) \
+	$(LIB_DIR) \
+	$(SDK_PATH)/Soft/Common \
+	$(SDK_PATH)/Soft/Lib
+
+GENERIC_TARGET_BINARIES_PREFIX=
+
+GENERIC_TARGET_BINARIES_DIR=$(ARDRONE_TARGET_DIR)
+
+GENERIC_BINARIES_SOURCE_ENTRYPOINTS+=			\
+   ardrone_testing_tool.c
+
+GENERIC_INCLUDES:=$(addprefix -I,$(GENERIC_INCLUDES))
+
+GENERIC_LIB_PATHS=-L$(GENERIC_TARGET_BINARIES_DIR)
+GENERIC_LIBS=-lpc_ardrone -lgtk-x11-2.0 -lrt
+
+SDK_FLAGS+="USE_APP=yes"
+SDK_FLAGS+="APP_ID=linux_custom_test"
+
+export GENERIC_CFLAGS
+export GENERIC_LIBS
+export GENERIC_LIB_PATHS
+export GENERIC_INCLUDES
+export GENERIC_BINARIES_SOURCE_DIR
+export GENERIC_BINARIES_COMMON_SOURCE_FILES
+export GENERIC_TARGET_BINARIES_PREFIX
+export GENERIC_TARGET_BINARIES_DIR
+export GENERIC_BINARIES_SOURCE_ENTRYPOINTS
+
+# Bug fix ...
+export GENERIC_LIBRARY_SOURCE_DIR=$(GENERIC_BINARIES_SOURCE_DIR)
+
+
+.PHONY: $(TARGET) build_libs
+
+all: build_libs $(TARGET)
+
+$(TARGET):
+	@$(MAKE) -C $(SDK_PATH)/VP_SDK/Build $(TMP_SDK_FLAGS) $(SDK_FLAGS) $(MAKECMDGOALS) USE_LINUX=yes
+	mv $(ARDRONE_TARGET_DIR)/ardrone_testing_tool $(TARGET)
+	mv $(TARGET) $(ARDRONE_TARGET_DIR)/
+
+$(MAKECMDGOALS): build_libs
+	@$(MAKE) -C $(SDK_PATH)/VP_SDK/Build $(TMP_SDK_FLAGS) $(SDK_FLAGS) $(MAKECMDGOALS) USE_LINUX=yes
+
+build_libs:
+	@$(MAKE) -C $(SDK_PATH)/Soft/Build $(TMP_SDK_FLAGS) $(SDK_FLAGS) $(MAKECMDGOALS) USE_LINUX=yes
+

Examples/Linux/custom_test/Sources/Navdata/navdata.c

+#include <ardrone_tool/Navdata/ardrone_navdata_client.h>
+
+#include <Navdata/navdata.h>
+#include <stdio.h>
+
+/* Initialization local variables before event loop  */
+inline C_RESULT demo_navdata_client_init( void* data )
+{
+  return C_OK;
+}
+
+/* Receving navdata during the event loop */
+inline C_RESULT demo_navdata_client_process( const navdata_unpacked_t* const navdata )
+{
+	const navdata_demo_t*nd = &navdata->navdata_demo;
+
+	printf("=====================\nNavdata for flight demonstrations =====================\n\n");
+
+	printf("Control state : %i\n",nd->ctrl_state);
+	printf("Battery level : %i mV\n",nd->vbat_flying_percentage);
+	printf("Orientation   : [Theta] %4.3f  [Phi] %4.3f  [Psi] %4.3f\n",nd->theta,nd->phi,nd->psi);
+	printf("Altitude      : %i\n",nd->altitude);
+	printf("Speed         : [vX] %4.3f  [vY] %4.3f  [vZPsi] %4.3f\n",nd->theta,nd->phi,nd->psi);
+
+	printf("\033[8A");
+
+  return C_OK;
+}
+
+/* Relinquish the local resources after the event loop exit */
+inline C_RESULT demo_navdata_client_release( void )
+{
+  return C_OK;
+}
+
+/* Registering to navdata client */
+BEGIN_NAVDATA_HANDLER_TABLE
+  NAVDATA_HANDLER_TABLE_ENTRY(demo_navdata_client_init, demo_navdata_client_process, demo_navdata_client_release, NULL)
+END_NAVDATA_HANDLER_TABLE
+

Examples/Linux/custom_test/Sources/Navdata/navdata.h

+#ifndef _NAVDATA_H_
+#define _NAVDATA_H_
+
+#include <ardrone_tool/Navdata/ardrone_navdata_client.h>
+
+
+#endif // _NAVDATA_H_

Examples/Linux/custom_test/Sources/Video/video_stage.c

+/*
+ * @video_stage.c
+ * @author marc-olivier.dzeukou@parrot.com
+ * @date 2007/07/27
+ *
+ * ihm vision thread implementation
+ *
+ */
+#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>
+
+#include <VP_Api/vp_api.h>
+#include <VP_Api/vp_api_error.h>
+#include <VP_Api/vp_api_stage.h>
+#include <VP_Api/vp_api_picture.h>
+#include <VP_Stages/vp_stages_io_file.h>
+#include <VP_Stages/vp_stages_i_camif.h>
+
+#include <config.h>
+#include <VP_Os/vp_os_print.h>
+#include <VP_Os/vp_os_malloc.h>
+#include <VP_Os/vp_os_delay.h>
+#include <VP_Stages/vp_stages_yuv2rgb.h>
+#include <VP_Stages/vp_stages_buffer_to_picture.h>
+#include <VLIB/Stages/vlib_stage_decode.h>
+
+#include <ardrone_tool/ardrone_tool.h>
+#include <ardrone_tool/Com/config_com.h>
+
+#ifndef RECORD_VIDEO
+#define RECORD_VIDEO
+#endif
+#ifdef RECORD_VIDEO
+#    include <ardrone_tool/Video/video_stage_recorder.h>
+#endif
+
+#include <ardrone_tool/Video/video_com_stage.h>
+
+#include "Video/video_stage.h"
+
+#define NB_STAGES 10
+
+PIPELINE_HANDLE pipeline_handle;
+
+static uint8_t*  pixbuf_data       = NULL;
+static vp_os_mutex_t  video_update_lock = PTHREAD_MUTEX_INITIALIZER;
+
+C_RESULT output_gtk_stage_open( void *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
+{
+  return (SUCCESS);
+}
+
+C_RESULT output_gtk_stage_transform( void *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
+{
+  vp_os_mutex_lock(&video_update_lock);
+ 
+  /* Get a reference to the last decoded picture */
+  pixbuf_data      = (uint8_t*)in->buffers[0];
+
+  vp_os_mutex_unlock(&video_update_lock);
+
+  return (SUCCESS);
+}
+
+C_RESULT output_gtk_stage_close( void *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
+{
+  return (SUCCESS);
+}
+
+
+const vp_api_stage_funcs_t vp_stages_output_gtk_funcs =
+{
+  NULL,
+  (vp_api_stage_open_t)output_gtk_stage_open,
+  (vp_api_stage_transform_t)output_gtk_stage_transform,
+  (vp_api_stage_close_t)output_gtk_stage_close
+};
+
+DEFINE_THREAD_ROUTINE(video_stage, data)
+{
+  C_RESULT res;
+
+  vp_api_io_pipeline_t    pipeline;
+  vp_api_io_data_t        out;
+  vp_api_io_stage_t       stages[NB_STAGES];
+
+  vp_api_picture_t picture;
+
+  video_com_config_t              icc;
+  vlib_stage_decoding_config_t    vec;
+  vp_stages_yuv2rgb_config_t      yuv2rgbconf;
+#ifdef RECORD_VIDEO
+  video_stage_recorder_config_t   vrc;
+#endif
+  /// Picture configuration
+  picture.format        = PIX_FMT_YUV420P;
+
+  picture.width         = QVGA_WIDTH;
+  picture.height        = QVGA_HEIGHT;
+  picture.framerate     = 30;
+
+  picture.y_buf   = vp_os_malloc( QVGA_WIDTH * QVGA_HEIGHT     );
+  picture.cr_buf  = vp_os_malloc( QVGA_WIDTH * QVGA_HEIGHT / 4 );
+  picture.cb_buf  = vp_os_malloc( QVGA_WIDTH * QVGA_HEIGHT / 4 );
+
+  picture.y_line_size   = QVGA_WIDTH;
+  picture.cb_line_size  = QVGA_WIDTH / 2;
+  picture.cr_line_size  = QVGA_WIDTH / 2;
+
+  vp_os_memset(&icc,          0, sizeof( icc ));
+  vp_os_memset(&vec,          0, sizeof( vec ));
+  vp_os_memset(&yuv2rgbconf,  0, sizeof( yuv2rgbconf ));
+
+  icc.com                 = COM_VIDEO();
+  icc.buffer_size         = 100000;
+  icc.protocol            = VP_COM_UDP;
+  COM_CONFIG_SOCKET_VIDEO(&icc.socket, VP_COM_CLIENT, VIDEO_PORT, wifi_ardrone_ip);
+
+  vec.width               = QVGA_WIDTH;
+  vec.height              = QVGA_HEIGHT;
+  vec.picture             = &picture;
+  vec.block_mode_enable   = TRUE;
+  vec.luma_only           = FALSE;
+
+  yuv2rgbconf.rgb_format = VP_STAGES_RGB_FORMAT_RGB24;
+#ifdef RECORD_VIDEO
+  vrc.fp = NULL;
+#endif
+
+  pipeline.nb_stages = 0;
+
+  stages[pipeline.nb_stages].type    = VP_API_INPUT_SOCKET;
+  stages[pipeline.nb_stages].cfg     = (void *)&icc;
+  stages[pipeline.nb_stages].funcs   = video_com_funcs;
+
+  pipeline.nb_stages++;
+
+#ifdef RECORD_VIDEO
+  stages[pipeline.nb_stages].type    = VP_API_FILTER_DECODER;
+  stages[pipeline.nb_stages].cfg     = (void*)&vrc;
+  stages[pipeline.nb_stages].funcs   = video_recorder_funcs;
+
+  pipeline.nb_stages++;
+#endif // RECORD_VIDEO
+  stages[pipeline.nb_stages].type    = VP_API_FILTER_DECODER;
+  stages[pipeline.nb_stages].cfg     = (void*)&vec;
+  stages[pipeline.nb_stages].funcs   = vlib_decoding_funcs;
+
+  pipeline.nb_stages++;
+
+  stages[pipeline.nb_stages].type    = VP_API_FILTER_YUV2RGB;
+  stages[pipeline.nb_stages].cfg     = (void*)&yuv2rgbconf;
+  stages[pipeline.nb_stages].funcs   = vp_stages_yuv2rgb_funcs;
+
+  pipeline.nb_stages++;
+
+  stages[pipeline.nb_stages].type    = VP_API_OUTPUT_SDL;
+  stages[pipeline.nb_stages].cfg     = NULL;
+  stages[pipeline.nb_stages].funcs   = vp_stages_output_gtk_funcs;
+
+  pipeline.nb_stages++;
+
+  pipeline.stages = &stages[0];
+ 
+  /* Processing of a pipeline */
+  if( !ardrone_tool_exit() )
+  {
+    PRINT("\n   Video stage thread initialisation\n\n");
+
+    res = vp_api_open(&pipeline, &pipeline_handle);
+
+    if( SUCCEED(res) )
+    {
+      int loop = SUCCESS;
+      out.status = VP_API_STATUS_PROCESSING;
+
+      while( !ardrone_tool_exit() && (loop == SUCCESS) )
+      {
+          if( SUCCEED(vp_api_run(&pipeline, &out)) ) {
+            if( (out.status == VP_API_STATUS_PROCESSING || out.status == VP_API_STATUS_STILL_RUNNING) ) {
+              loop = SUCCESS;
+            }
+          }
+          else loop = -1; // Finish this thread
+      }
+
+      vp_api_close(&pipeline, &pipeline_handle);
+    }
+  }
+
+  PRINT("   Video stage thread ended\n\n");
+
+  return (THREAD_RET)0;
+}
+

Examples/Linux/custom_test/Sources/Video/video_stage.h

+#ifndef _IHM_STAGES_O_GTK_H
+#define _IHM_STAGES_O_GTK_H
+
+#include <config.h>
+#include <VP_Api/vp_api_thread_helper.h>
+
+
+PROTO_THREAD_ROUTINE(video_stage, data);
+
+#endif // _IHM_STAGES_O_GTK_H

Examples/Linux/custom_test/Sources/ardrone_testing_tool.c

+/**
+ * @file main.c
+ * @author sylvain.gaeremynck@parrot.com
+ * @date 2009/07/01
+ */
+#include <ardrone_testing_tool.h>
+
+//ARDroneLib
+#include <utils/ardrone_time.h>
+#include <ardrone_tool/Navdata/ardrone_navdata_client.h>
+#include <ardrone_tool/Control/ardrone_control.h>
+#include <ardrone_tool/UI/ardrone_input.h>
+
+//Common
+#include <config.h>
+#include <ardrone_api.h>
+
+//VP_SDK
+#include <ATcodec/ATcodec_api.h>
+#include <VP_Os/vp_os_print.h>
+#include <VP_Api/vp_api_thread_helper.h>
+#include <VP_Os/vp_os_signal.h>
+
+//Local project
+#include <Video/video_stage.h>
+
+// TODO: remove this in final; not required
+// Get keyboard input; return -1 if no input
+#include <termios.h>
+#include <unistd.h>
+int kbhit(void) {
+  struct termios oldt, newt;
+  int ch;
+
+  tcgetattr(STDIN_FILENO, &oldt);
+  newt = oldt;
+
+  newt.c_lflag &= ~( ICANON | ECHO );
+  tcsetattr(STDIN_FILENO, TCSANOW, &newt);
+  ch = getchar();
+  tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
+
+  return ch;
+}
+
+static int32_t exit_ihm_program = 1;
+
+/* Implementing Custom methods for the main function of an ARDrone application */
+int main(int argc, char** argv)
+{
+	return ardrone_tool_main(argc, argv);
+}
+
+/* The delegate object calls this method during initialization of an ARDrone application */
+C_RESULT ardrone_tool_init_custom(void)
+{
+  /* Registering for a new device of game controller */
+  // ardrone_tool_input_add( &gamepad );
+
+  /* Start all threads of your application */
+  START_THREAD( video_stage, NULL );
+  
+  return C_OK;
+}
+
+/* The delegate object calls this method every time in the event loop */
+C_RESULT ardrone_tool_update_custom() { 
+  static float phi = 0f, theta = 0f, gaz = 0f, yaw = 0f;
+  static int hover = 0, yaw_mode = 0;
+  int key = kbhit();
+
+  #define _INC(_c, _v) case _c : _v += 0.1f; if ( _v >  1.0f) _v =  1.0f; break
+  #define _DEC(_c, _v) case _c : _v -= 0.1f; if ( _v < -1.0f) _v = -1.0f; break
+  switch (key) {
+    _INC('q', phi);
+    _INC('w', theta);
+    _INC('e', gaz);
+    _INC('r', yaw);
+    _DEC('a', phi);
+    _DEC('s', theta);
+    _DEC('d', gaz);
+    _DEC('f', yaw);
+    case 'h': hover = !hover; break;
+    case 'y': yaw_mode = !yaw_mode; break;
+    default: key = -1;
+  }
+  #undef _INC
+  #undef _DEC
+
+  if (key != -1) {
+    printf("phi:%4.1f theta:%4.1f gaz:%4.1f yaw:%4.1f %s %s\n",
+    phi, theta, gaz, yaw, (hover? "hover" : "_"), (yaw_mode? "yaw_mode" : "_"));
+  }  
+  ardrone_at_set_progress_cmd((hover? 1 : 0) | (yaw_mode? 2 : 0), phi, theta, gaz, yaw);
+
+  return C_OK;
+}
+
+/* The delegate object calls this method when the event loop exit */
+C_RESULT ardrone_tool_shutdown_custom(void)
+{
+  /* Relinquish all threads of your application */
+  JOIN_THREAD( video_stage );
+
+  /* Unregistering for the current device */
+  // ardrone_tool_input_remove( &gamepad );
+
+  return C_OK;
+}
+
+/* The event loop calls this method for the exit condition */
+bool_t ardrone_tool_exit()
+{
+  return exit_ihm_program == 0;
+}
+
+C_RESULT signal_exit()
+{
+  exit_ihm_program = 0;
+
+  return C_OK;
+}
+
+/* Implementing thread table in which you add routines of your application and those provided by the SDK */
+BEGIN_THREAD_TABLE
+  THREAD_TABLE_ENTRY( ardrone_control, 20 )
+  THREAD_TABLE_ENTRY( navdata_update, 20 )
+  THREAD_TABLE_ENTRY( video_stage, 20 )
+END_THREAD_TABLE
+

Examples/Linux/custom_test/Sources/ardrone_testing_tool.h

+#ifndef _MYKONOS_TESTING_TOOL_H_
+#define _MYKONOS_TESTING_TOOL_H_
+
+#include <stdio.h>
+#include <VP_Os/vp_os_types.h>
+
+C_RESULT signal_exit();
+
+#endif // _MYKONOS_TESTING_TOOL_H_