. o O (테스트 코드 수정)

Examples/Linux/custom_test/Build/Makefile

@@ -16,7 +16,7 @@ else
    ARDRONE_TARGET_DIR=$(shell pwd)/../../Build/Debug
 endif
 
-TARGET=linux_custom_test
+TARGET=drone_control
 
 SRC_DIR:=$(shell pwd)/../Sources
 
@@ -25,7 +25,8 @@ GENERIC_BINARIES_SOURCE_DIR:=$(SRC_DIR)
 
 GENERIC_BINARIES_COMMON_SOURCE_FILES+=			\
    Navdata/navdata.c    \
-   Video/video_stage.c
+   Video/video_stage.c	\
+	 Control/control.c
 
 GENERIC_INCLUDES+=					\
 	$(SRC_DIR) \
@@ -38,7 +39,7 @@ GENERIC_TARGET_BINARIES_PREFIX=
 GENERIC_TARGET_BINARIES_DIR=$(ARDRONE_TARGET_DIR)
 
 GENERIC_BINARIES_SOURCE_ENTRYPOINTS+=			\
-   ardrone_testing_tool.c
+   main.c
 
 GENERIC_INCLUDES:=$(addprefix -I,$(GENERIC_INCLUDES))
 
@@ -46,7 +47,7 @@ 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"
+SDK_FLAGS+="APP_ID=drone_control"
 
 export GENERIC_CFLAGS
 export GENERIC_LIBS
@@ -68,7 +69,7 @@ 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 $(ARDRONE_TARGET_DIR)/main $(TARGET)
 	mv $(TARGET) $(ARDRONE_TARGET_DIR)/
 
 $(MAKECMDGOALS): build_libs

Examples/Linux/custom_test/Sources/Control/control.c

+#include <ardrone_api.h>
+#include <VP_Os/vp_os_print.h>
+#include <ardrone_tool/Navdata/ardrone_academy_navdata.h>
+
+#include "control.h"
+
+C_RESULT open_control_device(void);
+C_RESULT update_control_device(void);
+C_RESULT close_control_device(void);
+
+input_device_t control_device = {
+  "Control",
+  open_control_device,
+  update_control_device,
+  close_control_device
+};
+
+controller_info_t controller_state;
+
+// 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;
+}
+
+C_RESULT open_control_device(void) {
+  control_state = {
+    0, 0, 0, 0, 0, 
+    0, 0, 0, 0, 0, 0
+  };
+  return C_OK;
+}
+
+C_RESULT update_control_device(void) {
+  int key = kbhit();
+
+  if (key == '1') {
+    controller_state.start = !controller_state.start;
+    ardrone_academy_navdata_takeoff();
+  }
+  else if (key == '2') {
+    controller_state.emergency = !controller_state.emergency;
+    ardrone_academy_navdata_emergency();
+  }
+  else {
+    #define _INC(_c, _v) case _c : controller_state. _v += 0.01f; if ( controller_state. _v >  1.0f) controller_state. _v =  1.0f; break
+    #define _DEC(_c, _v) case _c : controller_state. _v -= 0.01f; if ( controller_state. _v < -1.0f) controller_state. _v = -1.0f; break
+    switch (key) {
+      _INC('q', phi);
+      _INC('w', theta);
+      _INC('e', gaz);
+      _INC('r', yaw);
+      _INC('t', magneto_psi);
+      _INC('y', magneto_psi_accuracy);
+
+      _DEC('a', phi);
+      _DEC('s', theta);
+      _DEC('d', gaz);
+      _DEC('f', yaw);
+      _DEC('g', magneto_psi);
+      _DEC('h', magneto_psi_accuracy);
+
+      case '3': controller_state.hover_mode = !controller_state.hover_mode; break;
+      case '4': controller_state.yaw_mode = !controller_state.yaw_mode; break;
+      case '5': controller_state.absolute_mode = !controller_state.absolute_mode; break;
+      default: key = -1;
+    }
+    #undef _INC
+    #undef _DEC
+    if (key != -1) {
+      int32_t flags = (controller_state.hover_mode ? 1 : 0) | (controller_state.yaw_mode ? 2 : 0) | (controller_state.absolute_mode ? 4 : 0);
+      ardrone_tool_set_progressive_cmd(
+        flags, 
+        controller_state.phi, // roll
+        controller_state.theta, // pitch
+        controller_state.gaz,
+        controller_state.yaw,
+        controller_state.magneto_psi, 
+        controller_state.magneto_psi_accuracy
+      );
+    }
+  }
+  
+  return C_OK;
+}
+
+C_RESULT close_control_device(void) {
+  return C_OK;
+}

Examples/Linux/custom_test/Sources/Control/control.h

+#ifndef _CONTROL_H_
+#define _CONTROL_H_
+
+#include <ardrone_tool/UI/ardrone_input.h>
+
+extern input_device_t control_device;
+
+typedef struct _controller_info_t {
+  int32_t start;
+  int32_t emergency;
+  int32_t hover_mode;
+  int32_t yaw_mode;
+  int32_t absolute_mode;
+  float32_t phi;
+  float32_t theta;
+  float32_t gaz;
+  float32_t yaw;
+  float32_t magneto_psi;
+  float32_t magneto_psi_accuracy;
+} controller_info_t;
+
+extern controller_info_t controller_state;
+
+#endif // _CONTROL_H_
\ No newline at end of file

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

 #include <ardrone_tool/Navdata/ardrone_navdata_client.h>
 
 #include <Navdata/navdata.h>
+#include <Control/control.h>
 #include <stdio.h>
 
+#define CTRL_STATES_STRING
+#include "control_states.h"
+
+const char* ctrl_state_str(uint32_t ctrl_state)
+{
+  #define MAX_STR_CTRL_STATE 256
+  static char str_ctrl_state[MAX_STR_CTRL_STATE];
+
+  ctrl_string_t* ctrl_string;
+  uint32_t major = ctrl_state >> 16;
+  uint32_t minor = ctrl_state & 0xFFFF;
+
+  if( strlen(ctrl_states[major]) < MAX_STR_CTRL_STATE )
+  {
+    vp_os_memset(str_ctrl_state, 0, sizeof(str_ctrl_state));
+
+    strcat_s(str_ctrl_state, sizeof(str_ctrl_state),ctrl_states[major]);
+    ctrl_string = control_states_link[major];
+
+    if( ctrl_string != NULL && (strlen(ctrl_states[major]) + strlen(ctrl_string[minor]) < MAX_STR_CTRL_STATE) )
+    {
+      strcat_s( str_ctrl_state,sizeof(str_ctrl_state), " | " );
+      strcat_s( str_ctrl_state, sizeof(str_ctrl_state),ctrl_string[minor] );
+    }
+  }
+  else
+  {
+    vp_os_memset( str_ctrl_state, '#', sizeof(str_ctrl_state) );
+  }
+
+  return str_ctrl_state;
+}
+
 /* Initialization local variables before event loop  */
 inline C_RESULT demo_navdata_client_init( void* data )
 {
@@ -14,15 +48,17 @@ inline C_RESULT demo_navdata_client_process( const navdata_unpacked_t* const nav
 {
 	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("===================== Navdata for flight demonstrations =====================\n");
+	printf("Control state : %i                  / Battery level : %i mV\n", ctrl_state_str(nd->ctrl_state), nd->vbat_flying_percentage);
 	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");
+	printf("Orientation   : [Theta] %4.3f  [Phi] %4.3f  [Psi] %4.3f\n", nd->theta, nd->phi, nd->psi);
+	printf("Speed         : [vX] %4.3f  [vY] %4.3f  [vZ] %4.3f\n",nd->vx,nd->vy,nd->vz);
+	printf("============================= Controller states =============================\n");
+	printf("Start : %i                          / Emergency : %i\n", controller_state.start, controller_state.emergency);
+	printf("[Gaz] %4.3f\n", controller_state.gaz);
+	printf("[Theta] %4.3f  [Phi] %4.3f  [Yaw] %4.3f  [magnet-Psi] %4.3f\n", controller_state.theta, controller_state.phi, controller_state.yaw, controller_state.magneto_psi);
+	printf("Mode : [hover] %i  [yaw] %i  [absolute] %i  [psi-accuracy] %4.3f\n", controller_state.hover_mode, controller_state.yaw_mode, controller_state.absolute_mode, controller_state.magneto_psi_accuracy);
+	printf("\033[10A");
 
   return C_OK;
 }

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_

Examples/Linux/custom_test/Sources/ardrone_testing_tool.c → Examples/Linux/custom_test/Sources/main.c

@@ -3,7 +3,8 @@
  * @author sylvain.gaeremynck@parrot.com
  * @date 2009/07/01
  */
-#include <ardrone_testing_tool.h>
+#include <stdio.h>
+#include <VP_Os/vp_os_types.h>
 
 //ARDroneLib
 #include <utils/ardrone_time.h>
@@ -22,32 +23,46 @@
 #include <VP_Os/vp_os_signal.h>
 
 //Local project
+#include <Control/control.h>
 #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);
+static int32_t exit_ihm_program = 1;
 
-  return ch;
+C_RESULT signal_exit()
+{
+  exit_ihm_program = 0;
+  return C_OK;
 }
 
-static int32_t exit_ihm_program = 1;
+void controlCHandler (int signal)
+{
+	static int callCounter=0;
+
+	callCounter++;
+
+	/* In case in unresponsive, force killing the application */
+	if(callCounter>3){
+		printf("Ctrl-C pressed several times. Killing the application ...\n");
+		exit(-1);
+	}
+
+#if CONTROL_C_HANDLER_USES_ARDRONE_TOOL_EXIT
+    signal_exit ();
+#else
+    // Flush all streams before terminating
+    fflush (NULL);
+    usleep (200000); // Wait 200 msec to be sure that flush occured
+    printf ("\nAll files were flushed\n");
+    exit (0);
+#endif
+}
 
 /* Implementing Custom methods for the main function of an ARDrone application */
 int main(int argc, char** argv)
 {
+  signal (SIGABRT, &controlCHandler);
+  signal (SIGTERM, &controlCHandler);
+  signal (SIGINT, &controlCHandler);
 	return ardrone_tool_main(argc, argv);
 }
 
@@ -55,7 +70,7 @@ int main(int argc, char** argv)
 C_RESULT ardrone_tool_init_custom(void)
 {
   /* Registering for a new device of game controller */
-  // ardrone_tool_input_add( &gamepad );
+  ardrone_tool_input_add(&control_device);
 
   /* Start all threads of your application */
   START_THREAD( video_stage, NULL );
@@ -63,69 +78,6 @@ C_RESULT ardrone_tool_init_custom(void)
   return C_OK;
 }
 
-/* The delegate object calls this method every time in the event loop */
-C_RESULT ardrone_tool_update_custom() {
-  static float phi = 0, theta = 0, gaz = 0, yaw = 0, psi = 0, psi_accuracy = 0;
-  static int start = 0, emergency = 0, hover = 0, yaw_mode = 0, abs_ctrl_mode = 0;
-  int key = kbhit();
-
-  if (key == '1') {
-    start = !start;
-    ardrone_tool_set_ui_pad_start(start);
-  }
-  else if (key == '2') {
-    emergency = !emergency;
-    ardrone_tool_set_ui_pad_select(emergency);
-  }
-  else {
-    #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);
-      _INC('t', psi);
-      _INC('y', psi_accuracy);
-
-      _DEC('a', phi);
-      _DEC('s', theta);
-      _DEC('d', gaz);
-      _DEC('f', yaw);
-      _DEC('g', psi);
-      _DEC('h', psi_accuracy);
-
-      case 'h': hover = !hover; break;
-      case 'y': yaw_mode = !yaw_mode; break;
-      default: key = -1;
-    }
-    #undef _INC
-    #undef _DEC
-    if (key != -1) {
-      ardrone_tool_set_progressive_cmd(
-        (hover? 1 : 0) | (yaw_mode? 2 : 0) | (abs_ctrl_mode? 4 : 0),
-        phi, theta, gaz, yaw, psi, psi_accuracy
-      );
-      //ardrone_at_set_progress_cmd((hover? 1 : 0) | (yaw_mode? 2 : 0), phi, theta, gaz, yaw);
-    }
-  }
-
-  if (key != -1) {
-    printf(
-      "  phi theta   gaz   yaw   psi p_acc\n"
-      "%5.1f %5.1f %5.1f %5.1f %5.1f %5.1f%s%s%s%s%s\n",
-      phi, theta, gaz, yaw, psi, psi_accuracy,
-      (start? " / take-off" : " / land"), 
-      (emergency? " / emergency" : ""),
-      (hover? " / hovering" : ""), 
-      (yaw_mode? " / phi+yaw turn" : " / yaw turn"),
-      (abs_ctrl_mode? " / Absolute Control Mode" : "")
-    );
-  }  
-  
-  return C_OK;
-}
-
 /* The delegate object calls this method when the event loop exit */
 C_RESULT ardrone_tool_shutdown_custom(void)
 {
@@ -133,7 +85,7 @@ C_RESULT ardrone_tool_shutdown_custom(void)
   JOIN_THREAD( video_stage );
 
   /* Unregistering for the current device */
-  // ardrone_tool_input_remove( &gamepad );
+  ardrone_tool_input_remove(&control_device);
 
   return C_OK;
 }
@@ -144,13 +96,6 @@ 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 )