vcs.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/Examples/Linux/Navigation/Sources/ihm/ihm.c

   1 /*
   2  * @ihm.c
   3  * @author florian.pantaleao.ext@parrot.fr
   4  * @date 2006/11/08
   5  *
   6  * ihm thread main source file
   7  * original version by Marc-Olivier DZEUKOU
   8  *
   9  */
  10
  11
  12 #include <stdio.h>
  13 #include <stdlib.h>
  14 #include <sys/types.h>
  15 #include <sys/fcntl.h>
  16 #include <fcntl.h>
  17 #include <termios.h>
  18 #include <time.h>
  19 #include <unistd.h>
  20 #include <errno.h>
  21 #include <curses.h>
  22 #include <pthread.h>
  23 #include <gtk/gtk.h>
  24 #include <gdk/gdk.h>
  25 #include <SDL/SDL.h>
  26 #include <SDL/SDL_joystick.h>
  27 #include <locale.h>
  28 #include <iwlib.h>
  29 #include <sys/ioctl.h>
  30 #include <net/if.h>
  31 #include <netinet/in.h>
  32 #include <string.h>
  33
  34 #include <VP_Os/vp_os_malloc.h>
  35 #include <VP_Os/vp_os_print.h>
  36 #include <VP_Os/vp_os_signal.h>
  37
  38 #include <ardrone_api.h>
  39 #include <ardrone_tool/ardrone_tool_configuration.h>
  40 #include <UI/ardrone_ini.h>
  41
  42 #include "ihm_config.h"
  43
  44 #define ALTITUDE_LIMITED                3000
  45 #define NO_ALTITUDE                             10000
  46
  47 /*********** constants definition *****************/
  48 //static const gchar* list_item_data_key = "color_key";
  49
  50 enum
  51 {
  52     /* Text related columns */
  53     TEXT_C = 0,    /* Column with text strings */
  54     TEXT_VIS_C,    /* Visibility column for text strings */
  55     TEXT_COL_C,    /* Text color column */
  56
  57     /* Image related columns */
  58     PIXBUF_C,     /* Column with GdkPixbufs */
  59     PIXBUF_VIS_C, /* Visibility column for pixbufs */
  60
  61     /* Progress renderer related columns */
  62     PROGRESS_C,     /* Column with progress information [0, 100] */
  63     PROGRESS_VIS_C, /* Column with progress visibility */
  64
  65     /* Last element of enumeration holds the number of columns */
  66     N_COLS
  67 };
  68
  69
  70 double round(double x); // l'include de math.h ne fonctionne pas
  71
  72 #include "common/common.h"
  73 #include "common/mobile_config.h"
  74 #include "UI/ui.h"
  75
  76 #define KIHM_ALLOC_SHARED_DATA
  77 #include "ihm/ihm.h"
  78 #include "ihm/view_drone_attitude.h"
  79 #include "ihm/ihm_vision.h"
  80
  81 extern int exit_ihm_program;
  82 extern C_RESULT signal_exit();
  83
  84
  85 /* Vision image vars */
  86 extern GtkWidget *ihm_ImageWin, *ihm_ImageEntry[9];
  87 extern int tab_vision_config_params[10];
  88 extern int vision_config_options;
  89 extern int image_vision_window_status, image_vision_window_view;
  90
  91
  92 int ihm_is_initialized=0;
  93
  94 GtkWidget *label_elapsedT;
  95 GtkWidget *entry_PID[NB_ALL_GAINS], *entry_MiscVar[NB_MISC_VARS];
  96 GtkWidget *button_ShowCurve[KIHM_NB_CURVES];
  97 GtkWidget *button_show_image,*button_show_image2,*button_navdatademo,*button_roundel;
  98 GtkWidget *label_mykonos_values[NB_ARDRONE_STATES];
  99 GtkWidget *label_start_button_state, *darea_start_button_state;
 100 #ifdef USE_ARDRONE_VICON
 101 GtkWidget *label_vicon_button_state, *darea_vicon_button_state;
 102 #endif
 103 GtkWidget *entry_ToyConfig[3];
 104 GtkWidget *control_tab[10];
 105 GtkWidget *control_windows;
 106
 107 GtkWidget *control_label = NULL;
 108 GList *list_controllers = NULL;
 109 Controller_info *current_control = NULL;
 110 GHashTable *table1, *table2;
 111 int handler_id[4];
 112
 113
 114
 115 /*Stephane*/
 116 GtkWidget *nbDetectedTags_label;
 117 GtkWidget *activeDetection_label;
 118 #ifdef DEBUG
 119 GtkWidget *detectionHistory_label;
 120 #endif
 121 int detectionHistory[NB_DETECTION_SAMPLES];
 122
 123
 124 int windows_status[KIHM_NB_CURVES];
 125 int tab_g[NB_GAINS];
 126 int tab_ag[NB_GAINS_ALT];
 127 int tab_fp_g[NB_GAINS_FP];
 128 int tab_roundel_g[NB_GAINS_ROUNDEL];
 129 extern int32_t  MiscVar[NB_MISC_VARS];
 130 mobile_config_t *pcfg;
 131
 132 char label_mykonos_state_value[32];
 133 char label_ctrl_state_value[MAX_STR_CTRL_STATE];
 134 char label_detection_state_value[32];
 135
 136 typedef struct {
 137    GtkWidget *duration;
 138    GtkWidget *freq;
 139 }private_data_led;
 140
 141 ihm_time_t ihm_time;
 142
 143 uint32_t value_motor1 = 0, value_motor2 = 0, value_motor3 = 0, value_motor4 = 0; //to send motor commands to the drone for motor tests
 144 uint32_t rotation_speed_m1 = 0, rotation_speed_m2 = 0, rotation_speed_m3 = 0, rotation_speed_m4 = 0; // for motor tests : motor's rotation command speed in turn/min
 145 static bool_t all_motors = FALSE;
 146 static bool_t send_pwm_active = FALSE;
 147 static uint32_t avoid_cycle = 0;
 148
 149
 150 int32_t ihm_start = 0;
 151
 152 void ihm_set_start_button_state( int32_t start )
 153 {
 154   ihm_start = start;
 155 }
 156
 157 static void ihm_sendGains(GtkWidget *widget, gpointer data)
 158 {
 159   api_control_gains_t gains;
 160
 161   DEBUG_PRINT_SDK("PQ_Kp = %04d\tR_Kp = %04d\tR_Ki = %04d\tEA_Kp = %04d\tEA_Ki = %04d\tALT_KP = %04d\
 162                   \tVZ_KP = %04d\tVz_KI = %04d\tALT_TD = %04d\tFP_KP = %04d\tFP_KI = %04d\tB_KP = %04d\tB_KI = %04d\n",
 163                   tab_g[0], tab_g[1], tab_g[2], tab_g[3], tab_g[4],
 164                   tab_ag[0], tab_ag[1], tab_ag[2], tab_ag[3],
 165                   tab_fp_g[0], tab_fp_g[1],
 166                   tab_roundel_g[KIHM_ROUNDELKP], tab_roundel_g[KIHM_ROUNDELKI]);
 167
 168   gains.pq_kp         = tab_g[0];
 169   gains.r_kp          = tab_g[1];
 170   gains.r_ki          = tab_g[2];
 171   gains.ea_kp         = tab_g[3];
 172   gains.ea_ki         = tab_g[4];
 173   gains.alt_kp        = tab_ag[0];
 174   gains.alt_ki        = tab_ag[1];
 175   gains.vz_kp         = tab_ag[2];
 176   gains.vz_ki         = tab_ag[3];
 177   gains.hovering_kp   = tab_fp_g[0];
 178   gains.hovering_ki   = tab_fp_g[1];
 179   gains.hovering_b_kp = tab_roundel_g[KIHM_ROUNDELKP];
 180   gains.hovering_b_ki = tab_roundel_g[KIHM_ROUNDELKI];
 181
 182   ardrone_at_set_control_gains(&gains);
 183
 184   ardrone_at_set_ui_misc(MiscVar[0],MiscVar[1],MiscVar[2],MiscVar[3]);
 185 }
 186
 187 static void ihm_sendFlatTrim(GtkWidget *widget, gpointer data)
 188 {
 189     ardrone_at_set_flat_trim();
 190 }
 191
 192
 193 static void ihm_setConfigParamsWindow(GtkWidget *widget, gpointer data)
 194 {
 195   ihm_config_create_window();
 196   return;
 197 }
 198
 199 // Cedric
 200
 201 static void ihm_closeControlParams(GtkWidget *widget, gpointer data) {
 202   default_control->config = FALSE;
 203   gtk_widget_destroy(control_windows);
 204 }
 205
 206 int button_value(GtkButton *button) {
 207   if (current_control->serial)
 208     return atoi(gtk_button_get_label(button));
 209   int ret = GPOINTER_TO_INT(g_hash_table_lookup(table2, gtk_button_get_label(button)));
 210   if (ret)
 211     return ret;
 212   return (int) gtk_button_get_label(button)[0];
 213 }
 214
 215 static void ihm_setControlParams(GtkWidget *widget, gpointer data) {
 216   int i;
 217   GList *it;
 218
 219   for (it = pcfg->devices; it; it = it->next) {
 220     Controller_info *c;
 221     c = (Controller_info *) it->data;
 222     if (!g_strcmp0(c->name, current_control->name)) {
 223       break;
 224     }
 225   }
 226
 227   // new control not saved in configuration file
 228   if (!it) {
 229     pcfg->devices = g_list_append(pcfg->devices, current_control);
 230   }
 231   default_control->config = FALSE;
 232   default_control = current_control;
 233   pcfg->default_control = current_control;
 234   ardrone_tool_input_remove( &control_device );
 235   ardrone_tool_input_add( &control_device );
 236
 237   for (i = 0; i < NUM_COMMAND; i++) {
 238     pcfg->default_control->commands[i].value = button_value(GTK_BUTTON(control_tab[i]));
 239   }
 240   list_controllers = NULL;
 241   ihm_closeControlParams(widget, data);
 242   gtk_label_set_text(GTK_LABEL(control_label), pcfg->default_control->name);
 243   save_init(current_control);
 244 }
 245
 246 void populate_hash_tables() {
 247   table1 = g_hash_table_new(g_direct_hash, g_direct_equal);
 248   table2 = g_hash_table_new(g_str_hash, g_str_equal);
 249   g_hash_table_insert(table1, GINT_TO_POINTER(8), "BACKSPACE");
 250   g_hash_table_insert(table1, GINT_TO_POINTER(65288), "BACKSPACE");
 251   g_hash_table_insert(table1, GINT_TO_POINTER(13), "RETURN");
 252   g_hash_table_insert(table1, GINT_TO_POINTER(65293), "RETURN");
 253   g_hash_table_insert(table1, GINT_TO_POINTER(65505), "L-SHIFT");
 254   g_hash_table_insert(table1, GINT_TO_POINTER(65506), "R-SHIFT");
 255   g_hash_table_insert(table1, GINT_TO_POINTER(65507), "L-CTRL");
 256   g_hash_table_insert(table1, GINT_TO_POINTER(65508), "R-CTRL");
 257   g_hash_table_insert(table1, GINT_TO_POINTER(65508), "CAPSLOCK");
 258   g_hash_table_insert(table1, GINT_TO_POINTER(65513), "L-ALT");
 259   g_hash_table_insert(table1, GINT_TO_POINTER(65027), "R-ALT");
 260   g_hash_table_insert(table1, GINT_TO_POINTER(65515), "L-SUPER");
 261   g_hash_table_insert(table1, GINT_TO_POINTER(65516), "R-SUPER");
 262   g_hash_table_insert(table1, GINT_TO_POINTER(65288), "BACKSPACE");
 263   g_hash_table_insert(table1, GINT_TO_POINTER(65289), "TAB");
 264   g_hash_table_insert(table1, GINT_TO_POINTER(65299), "PAUSE");
 265   g_hash_table_insert(table1, GINT_TO_POINTER(65300), "BREAK");
 266   g_hash_table_insert(table1, GINT_TO_POINTER(65360), "HOME");
 267   g_hash_table_insert(table1, GINT_TO_POINTER(65361), "LEFT");
 268   g_hash_table_insert(table1, GINT_TO_POINTER(65362), "UP");
 269   g_hash_table_insert(table1, GINT_TO_POINTER(65363), "RIGHT");
 270   g_hash_table_insert(table1, GINT_TO_POINTER(65364), "DOWN");
 271   g_hash_table_insert(table1, GINT_TO_POINTER(65365), "PAGE-UP");
 272   g_hash_table_insert(table1, GINT_TO_POINTER(65366), "PAGE-DOWN");
 273   g_hash_table_insert(table1, GINT_TO_POINTER(65367), "END");
 274   g_hash_table_insert(table1, GINT_TO_POINTER(65379), "INSERT");
 275   g_hash_table_insert(table1, GINT_TO_POINTER(65383), "MENU");
 276   g_hash_table_insert(table1, GINT_TO_POINTER(65421), "ENTER");
 277   g_hash_table_insert(table1, GINT_TO_POINTER(65450), "*");
 278   g_hash_table_insert(table1, GINT_TO_POINTER(65451), "+");
 279   g_hash_table_insert(table1, GINT_TO_POINTER(65453), "-");
 280   g_hash_table_insert(table1, GINT_TO_POINTER(65454), "DECIMAL");
 281   g_hash_table_insert(table1, GINT_TO_POINTER(65455), "/");
 282   g_hash_table_insert(table1, GINT_TO_POINTER(65456), "NUM-0");
 283   g_hash_table_insert(table1, GINT_TO_POINTER(65457), "NUM-1");
 284   g_hash_table_insert(table1, GINT_TO_POINTER(65458), "NUM-2");
 285   g_hash_table_insert(table1, GINT_TO_POINTER(65459), "NUM-3");
 286   g_hash_table_insert(table1, GINT_TO_POINTER(65460), "NUM-4");
 287   g_hash_table_insert(table1, GINT_TO_POINTER(65461), "NUM-5");
 288   g_hash_table_insert(table1, GINT_TO_POINTER(65462), "NUM-6");
 289   g_hash_table_insert(table1, GINT_TO_POINTER(65463), "NUM-7");
 290   g_hash_table_insert(table1, GINT_TO_POINTER(65464), "NUM-8");
 291   g_hash_table_insert(table1, GINT_TO_POINTER(65465), "NUM-9");
 292   g_hash_table_insert(table1, GINT_TO_POINTER(65407), "VERR-NUM");
 293   g_hash_table_insert(table1, GINT_TO_POINTER(65470), "F1");
 294   g_hash_table_insert(table1, GINT_TO_POINTER(65471), "F2");
 295   g_hash_table_insert(table1, GINT_TO_POINTER(65472), "F3");
 296   g_hash_table_insert(table1, GINT_TO_POINTER(65473), "F4");
 297   g_hash_table_insert(table1, GINT_TO_POINTER(65474), "F5");
 298   g_hash_table_insert(table1, GINT_TO_POINTER(65475), "F6");
 299   g_hash_table_insert(table1, GINT_TO_POINTER(65476), "F7");
 300   g_hash_table_insert(table1, GINT_TO_POINTER(65477), "F8");
 301   g_hash_table_insert(table1, GINT_TO_POINTER(65478), "F9");
 302   g_hash_table_insert(table1, GINT_TO_POINTER(65479), "F10");
 303   g_hash_table_insert(table1, GINT_TO_POINTER(65480), "F11");
 304   g_hash_table_insert(table1, GINT_TO_POINTER(65481), "F12");
 305   g_hash_table_insert(table1, GINT_TO_POINTER(65535), "SUPPR");
 306   g_hash_table_insert(table1, GINT_TO_POINTER(32), "SPACE");
 307   g_hash_table_insert(table1, GINT_TO_POINTER(178), "SQUARE");
 308   g_hash_table_insert(table2, "BACKSPACE", GINT_TO_POINTER(8));
 309   g_hash_table_insert(table2, "RETURN", GINT_TO_POINTER(13));
 310   g_hash_table_insert(table2, "L-SHIFT", GINT_TO_POINTER(65505));
 311   g_hash_table_insert(table2, "R-SHIFT", GINT_TO_POINTER(65506));
 312   g_hash_table_insert(table2, "L-CTRL", GINT_TO_POINTER(65507));
 313   g_hash_table_insert(table2, "R-CTRL", GINT_TO_POINTER(65508));
 314   g_hash_table_insert(table2, "CAPSLOCK", GINT_TO_POINTER(65508));
 315   g_hash_table_insert(table2, "L-ALT", GINT_TO_POINTER(65513));
 316   g_hash_table_insert(table2, "R-ALT", GINT_TO_POINTER(65027));
 317   g_hash_table_insert(table2, "L-SUPER", GINT_TO_POINTER(65515));
 318   g_hash_table_insert(table2, "R-SUPER", GINT_TO_POINTER(65516));
 319   g_hash_table_insert(table2, "BACKSPACE", GINT_TO_POINTER(65288));
 320   g_hash_table_insert(table2, "TAB", GINT_TO_POINTER(65289));
 321   g_hash_table_insert(table2, "PAUSE", GINT_TO_POINTER(65299));
 322   g_hash_table_insert(table2, "BREAK", GINT_TO_POINTER(65300));
 323   g_hash_table_insert(table2, "HOME", GINT_TO_POINTER(65360));
 324   g_hash_table_insert(table2, "LEFT", GINT_TO_POINTER(65361));
 325   g_hash_table_insert(table2, "UP", GINT_TO_POINTER(65362));
 326   g_hash_table_insert(table2, "RIGHT", GINT_TO_POINTER(65363));
 327   g_hash_table_insert(table2, "DOWN", GINT_TO_POINTER(65364));
 328   g_hash_table_insert(table2, "PAGE-UP", GINT_TO_POINTER(65365));
 329   g_hash_table_insert(table2, "PAGE-DOWN", GINT_TO_POINTER(65366));
 330   g_hash_table_insert(table2, "END", GINT_TO_POINTER(65367));
 331   g_hash_table_insert(table2, "INSERT", GINT_TO_POINTER(65379));
 332   g_hash_table_insert(table2, "MENU", GINT_TO_POINTER(65383));
 333   g_hash_table_insert(table2, "ENTER", GINT_TO_POINTER(65421));
 334   g_hash_table_insert(table2, "*", GINT_TO_POINTER(65450));
 335   g_hash_table_insert(table2, "+", GINT_TO_POINTER(65451));
 336   g_hash_table_insert(table2, "-", GINT_TO_POINTER(65453));
 337   g_hash_table_insert(table2, "DECIMAL", GINT_TO_POINTER(65454));
 338   g_hash_table_insert(table2, "/", GINT_TO_POINTER(65455));
 339   g_hash_table_insert(table2, "NUM-0", GINT_TO_POINTER(65456));
 340   g_hash_table_insert(table2, "NUM-1", GINT_TO_POINTER(65457));
 341   g_hash_table_insert(table2, "NUM-2", GINT_TO_POINTER(65458));
 342   g_hash_table_insert(table2, "NUM-3", GINT_TO_POINTER(65459));
 343   g_hash_table_insert(table2, "NUM-4", GINT_TO_POINTER(65460));
 344   g_hash_table_insert(table2, "NUM-5", GINT_TO_POINTER(65461));
 345   g_hash_table_insert(table2, "NUM-6", GINT_TO_POINTER(65462));
 346   g_hash_table_insert(table2, "NUM-7", GINT_TO_POINTER(65463));
 347   g_hash_table_insert(table2, "NUM-8", GINT_TO_POINTER(65464));
 348   g_hash_table_insert(table2, "NUM-9", GINT_TO_POINTER(65465));
 349   g_hash_table_insert(table2, "VERR-NUM", GINT_TO_POINTER(65407));
 350   g_hash_table_insert(table2, "F1", GINT_TO_POINTER(65470));
 351   g_hash_table_insert(table2, "F2", GINT_TO_POINTER(65471));
 352   g_hash_table_insert(table2, "F3", GINT_TO_POINTER(65472));
 353   g_hash_table_insert(table2, "F4", GINT_TO_POINTER(65473));
 354   g_hash_table_insert(table2, "F5", GINT_TO_POINTER(65474));
 355   g_hash_table_insert(table2, "F6", GINT_TO_POINTER(65475));
 356   g_hash_table_insert(table2, "F7", GINT_TO_POINTER(65476));
 357   g_hash_table_insert(table2, "F8", GINT_TO_POINTER(65477));
 358   g_hash_table_insert(table2, "F9", GINT_TO_POINTER(65478));
 359   g_hash_table_insert(table2, "F10", GINT_TO_POINTER(65479));
 360   g_hash_table_insert(table2, "F11", GINT_TO_POINTER(65480));
 361   g_hash_table_insert(table2, "F12", GINT_TO_POINTER(65481));
 362   g_hash_table_insert(table2, "SUPPR", GINT_TO_POINTER(65535));
 363   g_hash_table_insert(table2, "SPACE", GINT_TO_POINTER(32));
 364   g_hash_table_insert(table2, "SQUARE", GINT_TO_POINTER(178));
 365 }
 366
 367 char* button_name(int button) {
 368   // si numéro de série -> manette
 369   if (current_control->serial)
 370     return g_strdup_printf("%d", button);
 371   char *ret = (char *) g_hash_table_lookup(table1, GINT_TO_POINTER(button));
 372   if (ret)
 373     return ret;
 374   if (button >= 97 && button <= 122)
 375     return g_strdup_printf("%c", button - 32);
 376   return g_strdup_printf("%c", button);
 377 }
 378
 379 static void control_device_update(GtkComboBox *combo, gpointer data) {
 380   gchar *device_name;
 381   device_name = gtk_combo_box_get_active_text(combo);
 382   int i;
 383
 384   GList *it;
 385   for (it = pcfg->devices; it; it = it->next) {
 386     Controller_info *c;
 387     c = (Controller_info *) it->data;
 388     if (!g_strcmp0(c->name, device_name)) {
 389       current_control = c;
 390       for (i = 0; i < NUM_COMMAND; i++) {
 391         printf("%d, %d\n", c->serial, c->commands[i].type);
 392         if ((c->serial && c->commands[i].type != NONE) || !c->serial)
 393           gtk_button_set_label(GTK_BUTTON(control_tab[i]), button_name(c->commands[i].value));
 394         else
 395           gtk_button_set_label(GTK_BUTTON(control_tab[i]), "");
 396       }
 397       break;
 398     }
 399   }
 400   if (it == NULL) {
 401     GList *it;
 402     for (it = list_controllers; it; it = it->next) {
 403       Controller_info *c;
 404       c = (Controller_info *) it->data;
 405       if (!g_strcmp0(c->name, device_name)) {
 406         current_control = c;
 407       }
 408     }
 409     for (i = 0; i < NUM_COMMAND; i++) {
 410       gtk_button_set_label(GTK_BUTTON(control_tab[i]), "");
 411     }
 412   }
 413 }
 414
 415 void toggle_control_callback (GtkWidget *widget, gpointer data) {
 416         SDL_Joystick *js = NULL;
 417         time_t t;
 418         GdkEvent *ge;
 419   int i, j, pos;
 420         Sint16 axis, numAxes = 0, InitAxisPos[256], PrevAxisPos[256];
 421
 422   pos = (int) data;
 423
 424   if (current_control->serial) {
 425     SDL_InitSubSystem(SDL_INIT_JOYSTICK);
 426
 427                 js = SDL_JoystickOpen(current_control->filename[strlen(current_control->filename) - 1] - 48);
 428                 SDL_JoystickEventState(SDL_IGNORE);
 429
 430                 SDL_JoystickUpdate();
 431
 432                 numAxes = SDL_JoystickNumAxes(js);
 433                 if (numAxes > 256) numAxes = 256;
 434
 435                 for (i = 0; i < numAxes; i++) {
 436                         InitAxisPos[i] = PrevAxisPos[i] = SDL_JoystickGetAxis(js, i);
 437                 }
 438   }
 439
 440         t = time(NULL);
 441         while (time(NULL) < t + 10) {
 442                 // check joystick events
 443                 if (js) {
 444                         SDL_JoystickUpdate();
 445
 446                         for (i = 0; i < numAxes; i++) {
 447                                 axis = SDL_JoystickGetAxis(js, i);
 448                                 if (abs(axis) > 16383 && (abs(axis - InitAxisPos[i]) > 4096 || abs(axis - PrevAxisPos[i]) > 4096)) {
 449           for (j = 0; j < NUM_COMMAND ; j++) {
 450             //printf("button : %s %s\n", gtk_button_get_label(GTK_BUTTON(control_tab[j])), button_name(i));
 451             if (current_control->commands[j].type == AXIS && !g_strcmp0(gtk_button_get_label(GTK_BUTTON(control_tab[j])), g_strdup_printf("%d", (i + 1) * (axis > 0 ? 1 : -1)))) {
 452               current_control->commands[j].type = NONE;
 453               current_control->commands[j].value = 0;
 454               gtk_button_set_label(GTK_BUTTON(control_tab[j]), "");
 455               break;
 456             }
 457           }
 458           //printf("axis : %d %d %s %s\n", i, axis, gtk_button_get_label(GTK_BUTTON(control_tab[i])), g_strdup_printf("%d", (i + 1) * (axis > 0 ? 1 : -1)));
 459           gtk_button_set_label(GTK_BUTTON(widget), g_strdup_printf("%d", (i + 1) * (axis > 0 ? 1 : -1)));
 460           current_control->commands[pos].type = AXIS;
 461           current_control->commands[pos].value = i;
 462                                         goto end;
 463                                 }
 464                                 PrevAxisPos[i] = axis;
 465                         }
 466                         for (i = 0; i < SDL_JoystickNumButtons(js); i++) {
 467                                 if (SDL_JoystickGetButton(js, i)) {
 468           for (j = 0; j < NUM_COMMAND ; j++) {
 469             //printf("button : %s %s\n", gtk_button_get_label(GTK_BUTTON(control_tab[j])), button_name(i));
 470             if (current_control->commands[j].type == BUTTON && !g_strcmp0(gtk_button_get_label(GTK_BUTTON(control_tab[j])), button_name(i))) {
 471               current_control->commands[j].type = NONE;
 472               current_control->commands[j].value = -1;
 473               gtk_button_set_label(GTK_BUTTON(control_tab[j]), "");
 474               break;
 475             }
 476           }
 477           current_control->commands[pos].type = BUTTON;
 478           current_control->commands[pos].value = i;
 479           gtk_button_set_label(GTK_BUTTON(widget), g_strdup_printf("%d", i));
 480                                         goto end;
 481                                 }
 482                         }
 483     }
 484
 485                 // check keyboard events
 486                 while ((ge = gdk_event_get()) != NULL) {
 487                         if (ge->type == GDK_KEY_PRESS) {
 488         //g_print("%i, %s\n", ge->key.keyval, ge->key.string);
 489         switch (ge->key.keyval) {
 490           case GDK_Escape :
 491                                     gdk_event_free(ge);
 492                                     goto end;
 493             break;
 494           default :
 495             if (!js) {
 496               for (i = 0; i <= 9 ; i++) {
 497                 if (!g_strcmp0(gtk_button_get_label(GTK_BUTTON(control_tab[i])), button_name(ge->key.keyval))) {
 498                   current_control->commands[i].type = NONE;
 499                   current_control->commands[i].value = 0;
 500                   gtk_button_set_label(GTK_BUTTON(control_tab[i]), "");
 501                   break;
 502                 }
 503               }
 504               current_control->commands[pos].type = NONE;
 505               current_control->commands[pos].value = ge->key.keyval;
 506               gtk_button_set_label(GTK_BUTTON(widget), button_name(ge->key.keyval));
 507                                       gdk_event_free(ge);
 508                                       goto end;
 509                                     }
 510             break;
 511         }
 512                         }
 513                         gdk_event_free(ge);
 514                 }
 515
 516                 usleep(5000);
 517         }
 518 end:
 519         if (js) {
 520                 SDL_JoystickEventState(SDL_IGNORE);
 521                 SDL_JoystickClose(js);
 522         }
 523 }
 524
 525 static void ihm_setUSBConfigParamsWindow(GtkWidget *widget, gpointer data)
 526 {
 527   GtkWidget *hBox, *vBox;
 528   GtkWidget *label_tab[10];
 529   GtkWidget *button_set;
 530   int i = 0, def = 0;
 531
 532   populate_hash_tables();
 533
 534   control_windows = gtk_window_new(GTK_WINDOW_TOPLEVEL);
 535   gtk_window_set_position(GTK_WINDOW(control_windows), GTK_WIN_POS_CENTER_ALWAYS);
 536   gtk_container_set_border_width(GTK_CONTAINER(control_windows), KIHM_BORDER);
 537   gtk_window_set_title(GTK_WINDOW(control_windows), "Control Device Configuration");
 538
 539   vBox = gtk_vbox_new(FALSE, 0);
 540   gtk_container_add(GTK_CONTAINER(control_windows), vBox);
 541
 542   hBox = gtk_hbox_new(FALSE, 0);
 543   gtk_box_pack_start(GTK_BOX(vBox), hBox  , TRUE, TRUE, 1);
 544
 545   // Device Selection Combo
 546   GtkWidget *combo;
 547   combo = gtk_combo_box_new_text();
 548   g_signal_connect( G_OBJECT( combo ), "changed", G_CALLBACK( control_device_update ), NULL );
 549
 550   gtk_combo_box_insert_text( (GtkComboBox*)combo, i++, (const gchar*) "Keyboard");
 551
 552   list_controllers = NULL;
 553   if (search_devices(&list_controllers) != FALSE) {
 554     GList *it;
 555     for (it = list_controllers; it; it = it->next) {
 556       Controller_info *c;
 557       c = (Controller_info *) it->data;
 558       if (!g_strcmp0(c->name, pcfg->default_control->name))
 559         def = i;
 560       gtk_combo_box_insert_text( (GtkComboBox*)combo, i++, (const gchar*) c->name);
 561     }
 562   }
 563   gtk_box_pack_start(GTK_BOX(hBox), combo, FALSE, FALSE, 0);
 564   gtk_widget_show( combo );
 565
 566   default_control->config = TRUE;
 567   current_control = default_control;
 568
 569   label_tab[0] = gtk_label_new("Take-Off / Land");
 570   label_tab[1] = gtk_label_new("Emergency / Reset");
 571   label_tab[2] = gtk_label_new("Pitch Front");
 572   label_tab[3] = gtk_label_new("Pitch Back");
 573   label_tab[4] = gtk_label_new("Roll Left");
 574   label_tab[5] = gtk_label_new("Roll Right");
 575   label_tab[6] = gtk_label_new("Yaw Left");
 576   label_tab[7] = gtk_label_new("Yaw Right");
 577   label_tab[8] = gtk_label_new("Vertical Speed Up");
 578   label_tab[9] = gtk_label_new("Vertical Speed Down");
 579
 580   GtkWidget* table;
 581   table = gtk_table_new (10, 2, FALSE);
 582
 583   for (i = 0; i <= 9; i++) {
 584     control_tab[i] = gtk_button_new();
 585     g_signal_connect_data(G_OBJECT(control_tab[i]), "clicked", GTK_SIGNAL_FUNC(toggle_control_callback), GINT_TO_POINTER(i), NULL, G_CONNECT_AFTER);
 586     gtk_table_attach_defaults(GTK_TABLE(table), label_tab[i], 0, 1, i, i + 1);
 587     gtk_table_attach_defaults(GTK_TABLE(table), control_tab[i], 1, 2, i, i + 1);
 588   }
 589
 590   gtk_box_pack_start(GTK_BOX(vBox), table  , TRUE, TRUE, 2);
 591
 592   hBox = gtk_hbutton_box_new();
 593   gtk_button_box_set_layout (GTK_BUTTON_BOX (hBox), GTK_BUTTONBOX_SPREAD);
 594
 595   gtk_box_pack_start(GTK_BOX(vBox), hBox  , TRUE, TRUE, 0);
 596
 597   button_set = gtk_button_new_with_label("OK");
 598   g_signal_connect(G_OBJECT(button_set), "clicked", G_CALLBACK(ihm_setControlParams), NULL);
 599   gtk_box_pack_start(GTK_BOX(hBox), button_set, TRUE, TRUE, 0);
 600
 601   button_set = gtk_button_new_with_label("Cancel");
 602   g_signal_connect(G_OBJECT(button_set), "clicked", G_CALLBACK(ihm_closeControlParams), NULL);
 603   gtk_box_pack_end(GTK_BOX(hBox), button_set, TRUE, TRUE, 0);
 604
 605   gtk_combo_box_set_active ( (GtkComboBox*)combo, def);
 606   gtk_widget_show_all(control_windows);
 607 }
 608
 609 void config_callback(bool_t result) {
 610   if (result) {
 611     //printf("%f, %f, %f\n", ardrone_control_config.control_yaw, ardrone_control_config.control_vz_max, ardrone_control_config.euler_angle_max);
 612     g_signal_handler_block(control_tab[0], handler_id[0]);
 613     g_signal_handler_block(control_tab[2], handler_id[1]);
 614     g_signal_handler_block(control_tab[3], handler_id[2]);
 615     g_signal_handler_block(control_tab[4], handler_id[3]);
 616     gtk_button_set_label(GTK_BUTTON(control_tab[0]), g_strcmp0(ardrone_control_config.owner_mac, NULL_MAC) ? "ON" : "OFF");
 617     gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(control_tab[0]), g_strcmp0(ardrone_control_config.owner_mac, NULL_MAC) ? TRUE : FALSE);
 618     gtk_entry_set_text(GTK_ENTRY(control_tab[1]), ardrone_control_config.ssid_single_player);
 619     gtk_button_set_label(GTK_BUTTON(control_tab[2]), (ardrone_control_config.altitude_max == ALTITUDE_LIMITED) ? "ON" : "OFF");
 620     gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(control_tab[2]), (ardrone_control_config.altitude_max == ALTITUDE_LIMITED) ? TRUE : FALSE);
 621     gtk_button_set_label(GTK_BUTTON(control_tab[3]), ardrone_control_config.flight_without_shell ? "ON" : "OFF");
 622     gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(control_tab[3]), ardrone_control_config.flight_without_shell ? TRUE : FALSE);
 623     gtk_button_set_label(GTK_BUTTON(control_tab[4]), ardrone_control_config.outdoor ? "ON" : "OFF");
 624     gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(control_tab[4]), ardrone_control_config.outdoor ? TRUE : FALSE);
 625     gtk_range_set_value(GTK_RANGE(control_tab[5]), ardrone_control_config.control_yaw * RAD_TO_DEG);
 626     gtk_range_set_value(GTK_RANGE(control_tab[6]), (float)ardrone_control_config.control_vz_max);
 627     gtk_range_set_value(GTK_RANGE(control_tab[7]), ardrone_control_config.euler_angle_max * RAD_TO_DEG);
 628     g_signal_handler_unblock(control_tab[0], handler_id[0]);
 629     g_signal_handler_unblock(control_tab[2], handler_id[1]);
 630     g_signal_handler_unblock(control_tab[3], handler_id[2]);
 631     g_signal_handler_unblock(control_tab[4], handler_id[3]);
 632   }
 633   result = FALSE;
 634 }
 635
 636 void toggle_button_callback (GtkWidget *widget, gpointer data) {
 637   int32_t limit;
 638   bool_t enabled;
 639
 640   if (gtk_toggle_button_get_active(GTK_TOGGLE_BUTTON(widget))) {
 641     gtk_button_set_label(GTK_BUTTON(widget), "ON");
 642   } else {
 643     gtk_button_set_label(GTK_BUTTON(widget), "OFF");
 644   }
 645   if (GPOINTER_TO_INT(data) == 0) {
 646
 647           char line[512];
 648           char *colon;
 649           char *name;
 650           FILE *fp;
 651     GList* list = NULL;
 652
 653           if(0 == (fp = fopen("/proc/net/dev", "r")))
 654                   return;
 655
 656           while(0 != (name = fgets(line, 512, fp))) {
 657                   while(isspace(name[0])) /* Trim leading whitespace */
 658                           name++;
 659
 660                   colon = strchr (name, ':');
 661                   if(colon) {
 662                           *colon = 0;
 663                           list = g_list_append(list, g_strdup_printf("%s", name));
 664                           printf("%s\n", name);
 665                   }
 666           }
 667           fclose(fp);
 668
 669     char *s;
 670     int skfd = 0;
 671     struct wireless_info *winfo;
 672         winfo = vp_os_malloc(sizeof(struct wireless_info));
 673           vp_os_memset(winfo, 0, sizeof(struct wireless_info));
 674     GList *it;
 675     for (it = list; it; it = it->next) {
 676                   s = g_strdup_printf("%s", (char*) it->data);
 677       skfd = iw_sockets_open();
 678       if (iw_get_basic_config(skfd, s, &(winfo->b)) > -1) {
 679                           if (winfo->b.has_essid) {
 680                             if (!g_strcmp0(winfo->b.essid, ardrone_control_config.ssid_single_player)) {
 681                                     printf("%s\n", winfo->b.essid);
 682                                     break;
 683                                   }
 684                           }
 685                   }
 686     }
 687
 688           iw_sockets_close(skfd);
 689           vp_os_free(winfo);
 690     if (!it) {
 691       printf("ARDrone introuvable\n");
 692       return;
 693     }
 694
 695     char* mac_address = NULL;
 696     struct ifreq ifr;
 697     int fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_IP);
 698
 699     strcpy(ifr.ifr_name, (char*) it->data);
 700     if (0 == ioctl(fd, SIOCGIFHWADDR, &ifr)) {
 701       int i = 0;
 702       mac_address = g_strdup_printf("%02x", (unsigned char) ifr.ifr_addr.sa_data[i]);
 703       for (i = 1; i < 6; ++i)
 704         mac_address = g_strdup_printf("%s:%02x", mac_address, (unsigned char) ifr.ifr_addr.sa_data[i]);
 705     }
 706     strcpy(ardrone_control_config.owner_mac, mac_address);
 707     printf("%s\n", ardrone_control_config.owner_mac);
 708     //printf("Pairing : %s\n", GTK_TOGGLE_BUTTON(control_tab[0])->active?"on":"off");
 709
 710     strcpy(ardrone_control_config.owner_mac, (GTK_TOGGLE_BUTTON(control_tab[0])->active) ? ardrone_control_config.owner_mac : NULL_MAC);
 711     ARDRONE_TOOL_CONFIGURATION_ADDEVENT(owner_mac, ardrone_control_config.owner_mac, NULL);
 712   } else if (GPOINTER_TO_INT(data) == 2) {
 713     //printf("Altitude Limited : %s\n", GTK_TOGGLE_BUTTON(control_tab[2])->active?"on":"off");
 714
 715     limit = (GTK_TOGGLE_BUTTON(control_tab[2])->active) ? ALTITUDE_LIMITED : NO_ALTITUDE;
 716     ardrone_control_config.altitude_max = limit;
 717     ARDRONE_TOOL_CONFIGURATION_ADDEVENT(altitude_max, &ardrone_control_config.altitude_max, NULL);
 718   } else if (GPOINTER_TO_INT(data) == 3) {
 719     //printf("Outdoor Shell : %s\n", GTK_TOGGLE_BUTTON(control_tab[3])->active?"on":"off");
 720
 721     enabled = (GTK_TOGGLE_BUTTON(control_tab[3])->active);
 722     ardrone_control_config.flight_without_shell = enabled;
 723     ARDRONE_TOOL_CONFIGURATION_ADDEVENT(flight_without_shell, &ardrone_control_config.flight_without_shell, NULL);
 724   } else if (GPOINTER_TO_INT(data) == 4) {
 725     //printf("Outdoor Flight : %s\n", GTK_TOGGLE_BUTTON(control_tab[4])->active?"on":"off");
 726
 727     enabled = (GTK_TOGGLE_BUTTON(control_tab[4])->active);
 728     ardrone_control_config.outdoor = enabled;
 729     ARDRONE_TOOL_CONFIGURATION_ADDEVENT(outdoor, &ardrone_control_config.outdoor, NULL);
 730     ARDRONE_TOOL_CONFIGURATION_GET(config_callback);
 731   }
 732 }
 733
 734 static void ihm_setFlightControlParams(GtkWidget *widget, gpointer data) {
 735   float value;
 736   string_t s;
 737
 738   printf("Pairing : %s\n", GTK_TOGGLE_BUTTON(control_tab[0])->active?"on":"off");
 739   printf("Drone Network SSID : %s\n", gtk_entry_get_text(GTK_ENTRY(control_tab[1])));
 740   printf("Altitude Limited : %s\n", GTK_TOGGLE_BUTTON(control_tab[2])->active?"on":"off");
 741   printf("Outdoor Shell : %s\n", GTK_TOGGLE_BUTTON(control_tab[3])->active?"on":"off");
 742   printf("Outdoor Flight : %s\n", GTK_TOGGLE_BUTTON(control_tab[4])->active?"on":"off");
 743   printf("Yaw Speed : %0.2f\n", gtk_range_get_value(GTK_RANGE(control_tab[5])));
 744   printf("Vertical Speed : %0.2f\n", gtk_range_get_value(GTK_RANGE(control_tab[6])));
 745   printf("Max Tilt : %0.2f\n", gtk_range_get_value(GTK_RANGE(control_tab[7])));
 746   //printf("Trim Pitch : %0.2f\n", gtk_range_get_value(GTK_RANGE(control_tab[8])));
 747   //printf("Trim Roll : %0.2f\n", gtk_range_get_value(GTK_RANGE(control_tab[9])));
 748
 749   strcpy(s, gtk_entry_get_text(GTK_ENTRY(control_tab[1])));
 750         strcpy(ardrone_control_config.ssid_single_player, s);
 751         ARDRONE_TOOL_CONFIGURATION_ADDEVENT(ssid_single_player, ardrone_control_config.ssid_single_player, NULL);
 752
 753         value = gtk_range_get_value(GTK_RANGE(control_tab[5]));
 754         ardrone_control_config.control_yaw = value * DEG_TO_RAD;
 755         ARDRONE_TOOL_CONFIGURATION_ADDEVENT(control_yaw, &ardrone_control_config.control_yaw, NULL);
 756
 757         value = gtk_range_get_value(GTK_RANGE(control_tab[6]));
 758         ardrone_control_config.control_vz_max = value;
 759         ARDRONE_TOOL_CONFIGURATION_ADDEVENT(control_vz_max, &ardrone_control_config.control_vz_max, NULL);
 760
 761   value = gtk_range_get_value(GTK_RANGE(control_tab[7]));
 762   ardrone_control_config.euler_angle_max = value * DEG_TO_RAD;
 763   ARDRONE_TOOL_CONFIGURATION_ADDEVENT(euler_angle_max, &ardrone_control_config.euler_angle_max, NULL);
 764
 765   ihm_closeControlParams(widget, data);
 766 }
 767
 768 static void ihm_setDefaultFlightControlParams(GtkWidget *widget, gpointer data) {
 769   printf("%f, %f, %f\n", ardrone_control_config_default.control_yaw, ardrone_control_config_default.control_vz_max, ardrone_control_config_default.euler_angle_max);
 770
 771   ardrone_control_config.altitude_max = ardrone_control_config_default.altitude_max;
 772   ardrone_control_config.flight_without_shell = ardrone_control_config_default.flight_without_shell;
 773   ardrone_control_config.outdoor = ardrone_control_config_default.outdoor;
 774   ardrone_control_config.control_yaw = ardrone_control_config_default.control_yaw;
 775   ardrone_control_config.control_vz_max = ardrone_control_config_default.control_vz_max;
 776   ardrone_control_config.euler_angle_max = ardrone_control_config_default.euler_angle_max;
 777
 778   ARDRONE_TOOL_CONFIGURATION_ADDEVENT(altitude_max, &ardrone_control_config.altitude_max, NULL);
 779   ARDRONE_TOOL_CONFIGURATION_ADDEVENT(flight_without_shell, &ardrone_control_config.flight_without_shell, NULL);
 780   ARDRONE_TOOL_CONFIGURATION_ADDEVENT(outdoor, &ardrone_control_config.outdoor, NULL);
 781   ARDRONE_TOOL_CONFIGURATION_ADDEVENT(control_yaw, &ardrone_control_config.control_yaw, NULL);
 782   ARDRONE_TOOL_CONFIGURATION_ADDEVENT(control_vz_max, &ardrone_control_config.control_vz_max, NULL);
 783   ARDRONE_TOOL_CONFIGURATION_ADDEVENT(euler_angle_max, &ardrone_control_config.euler_angle_max, NULL);
 784   //gtk_range_set_value(GTK_RANGE(control_tab[8]), -5.0);
 785   //gtk_range_set_value(GTK_RANGE(control_tab[9]), -5.0);
 786   ARDRONE_TOOL_CONFIGURATION_GET(config_callback);
 787 }
 788
 789 static void ihm_setFlightControlParamsWindow(GtkWidget *widget, gpointer data)
 790 {
 791   GtkWidget *hBox, *vBox;
 792   GtkWidget *label_tab[10];
 793   GtkWidget *button_set;
 794   GtkObject *scale;
 795   int i;
 796
 797   control_windows = gtk_window_new(GTK_WINDOW_TOPLEVEL);
 798   gtk_window_set_position(GTK_WINDOW(control_windows), GTK_WIN_POS_CENTER_ALWAYS);
 799   gtk_container_set_border_width(GTK_CONTAINER(control_windows), KIHM_BORDER);
 800   gtk_window_set_title(GTK_WINDOW(control_windows), "Flight Control Configuration");
 801
 802   vBox = gtk_vbox_new(FALSE, 0);
 803   gtk_container_add(GTK_CONTAINER(control_windows), vBox);
 804
 805   label_tab[0] = gtk_label_new("Pairing");
 806   label_tab[1] = gtk_label_new("Drone Network SSID");
 807   label_tab[2] = gtk_label_new("Altitude Limited");
 808   label_tab[3] = gtk_label_new("Outdoor Shell");
 809   label_tab[4] = gtk_label_new("Outdoor Flight");
 810   label_tab[5] = gtk_label_new("Yaw Speed (°/s)");
 811   label_tab[6] = gtk_label_new("Vertical Speed (m/s)");
 812   label_tab[7] = gtk_label_new("Max Tilt (°)");
 813   //label_tab[8] = gtk_label_new("Trim Pitch (°)");
 814   //label_tab[9] = gtk_label_new("Trim Roll (°)");
 815
 816   control_tab[0] = gtk_toggle_button_new_with_label("OFF");
 817   control_tab[1] = gtk_entry_new();
 818   control_tab[2] = gtk_toggle_button_new_with_label("OFF");
 819   control_tab[3] = gtk_toggle_button_new_with_label("OFF");
 820   control_tab[4] = gtk_toggle_button_new_with_label("OFF");
 821   scale = gtk_adjustment_new(0.0, 40.0, 350.0, 1.0, 1.0, 0.0);
 822   control_tab[5] = gtk_hscale_new((GtkAdjustment*) scale);
 823   scale = gtk_adjustment_new(0.0, 200.0, 2000.0, 10.0, 10.0, 0.0);
 824   control_tab[6] = gtk_hscale_new((GtkAdjustment*) scale);
 825   scale = gtk_adjustment_new(0.0, 5.0, 30.0, 1.0, 1.0, 0.0);
 826   control_tab[7] = gtk_hscale_new((GtkAdjustment*) scale);
 827
 828   handler_id[0] = g_signal_connect(G_OBJECT(control_tab[0]), "clicked", G_CALLBACK(toggle_button_callback), (gpointer) 0);
 829   handler_id[1] = g_signal_connect(G_OBJECT(control_tab[2]), "clicked", G_CALLBACK(toggle_button_callback), (gpointer) 2);
 830   handler_id[2] = g_signal_connect(G_OBJECT(control_tab[3]), "clicked", G_CALLBACK(toggle_button_callback), (gpointer) 3);
 831   handler_id[3] = g_signal_connect(G_OBJECT(control_tab[4]), "clicked", G_CALLBACK(toggle_button_callback), (gpointer) 4);
 832
 833   ARDRONE_TOOL_CONFIGURATION_GET(config_callback);
 834
 835   GtkWidget* table;
 836   table = gtk_table_new (10, 2, FALSE);
 837
 838   for (i = 0; i <= 7; i++) {
 839     gtk_table_attach_defaults(GTK_TABLE(table), label_tab[i], 0, 1, i, i + 1);
 840     gtk_table_attach_defaults(GTK_TABLE(table), control_tab[i], 1, 2, i, i + 1);
 841   }
 842
 843   gtk_box_pack_start(GTK_BOX(vBox), table  , TRUE, TRUE, 2);
 844
 845   hBox = gtk_hbutton_box_new();
 846   gtk_button_box_set_layout (GTK_BUTTON_BOX (hBox), GTK_BUTTONBOX_SPREAD);
 847
 848   gtk_box_pack_start(GTK_BOX(vBox), hBox  , TRUE, TRUE, 0);
 849
 850   button_set = gtk_button_new_with_label("OK");
 851   g_signal_connect(G_OBJECT(button_set), "clicked", G_CALLBACK(ihm_setFlightControlParams), NULL);
 852   gtk_box_pack_start(GTK_BOX(hBox), button_set, TRUE, TRUE, 0);
 853
 854   button_set = gtk_button_new_with_label("Flat Trim");
 855   g_signal_connect(G_OBJECT(button_set), "clicked", G_CALLBACK(ihm_sendFlatTrim), NULL);
 856   gtk_box_pack_end(GTK_BOX(hBox), button_set, TRUE, TRUE, 0);
 857
 858   button_set = gtk_button_new_with_label("Default");
 859   g_signal_connect(G_OBJECT(button_set), "clicked", G_CALLBACK(ihm_setDefaultFlightControlParams), NULL);
 860   gtk_box_pack_end(GTK_BOX(hBox), button_set, TRUE, TRUE, 0);
 861
 862   button_set = gtk_button_new_with_label("Cancel");
 863   g_signal_connect(G_OBJECT(button_set), "clicked", G_CALLBACK(ihm_closeControlParams), NULL);
 864   gtk_box_pack_end(GTK_BOX(hBox), button_set, TRUE, TRUE, 0);
 865
 866   gtk_widget_show_all(control_windows);
 867 }
 868 /* Cedric fin */
 869
 870 static void ihm_freezeCurves(GtkWidget *widget, gpointer data)
 871 {
 872   ihm_freeze_curves = gtk_toggle_button_get_active(GTK_TOGGLE_BUTTON(widget));
 873 }
 874
 875 static void ihm_showCurves(GtkWidget *widget, gpointer data)
 876 {
 877   int curve = (int) data; int k;
 878   if (gtk_toggle_button_get_active(GTK_TOGGLE_BUTTON(widget))) {
 879     if( !GTK_IS_WIDGET(ihm_CA[curve].PrivateWin) )  {
 880       ihm_initCurves( ihm_CA[curve].PrivateWin, curve); // Recreate window if it has been killed
 881       windows_status[curve] = WINDOW_OPENED;
 882     }
 883     gtk_widget_show_all(ihm_CA[curve].PrivateWin);
 884     for (k=0;k<ihm_CA[curve].nb_range;k++)
 885       if (k != ihm_CA[curve].range) gtk_widget_hide(ihm_CA[curve].tSA[k].vBox_Grad); // Hide unchoosed graduation
 886     ihm_CA[curve].win_view = WINDOW_VISIBLE;
 887   }
 888   else {
 889     if( GTK_IS_WIDGET(ihm_CA[curve].PrivateWin) ) {
 890       gtk_widget_hide(ihm_CA[curve].PrivateWin);
 891       ihm_CA[curve].win_view = WINDOW_HIDE;
 892     }
 893   }
 894 }
 895
 896 static void ihm_plusScale(GtkWidget *widget, gpointer data)
 897 {
 898   SIHM_CurveAttributes *pCA = (SIHM_CurveAttributes*) data;
 899   if (pCA->range < pCA->nb_range-1) {
 900     gtk_widget_hide_all(pCA->tSA[pCA->range].vBox_Grad);
 901     pCA->range++;
 902     gtk_widget_show_all(pCA->tSA[pCA->range].vBox_Grad);
 903   }
 904 }
 905
 906 static void ihm_minusScale(GtkWidget *widget, gpointer data)
 907 {
 908   SIHM_CurveAttributes *pCA = (SIHM_CurveAttributes*) data;
 909   if (pCA->range > 0) {
 910     gtk_widget_hide_all(pCA->tSA[pCA->range].vBox_Grad);
 911     pCA->range--;
 912     gtk_widget_show_all(pCA->tSA[pCA->range].vBox_Grad);
 913   }
 914 }
 915
 916 void ihm_private_window_destroy( SIHM_CurveAttributes *pCA )
 917 {
 918   int k;
 919   for(k=0;k<KIHM_NB_CURVES;k++) {
 920     if( (windows_status[k] == WINDOW_OPENED) && (!GTK_IS_WIDGET(ihm_CA[k].PrivateWin)) ) {
 921       windows_status[k] = WINDOW_CLOSED;
 922       gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(button_ShowCurve[k]), FALSE );
 923     }
 924   }
 925 }
 926
 927 void ihm_main_quit( void )
 928 {
 929   if( image_vision_window_status != WINDOW_CLOSED )
 930     gtk_widget_destroy(ihm_ImageWin);
 931
 932   signal_exit();
 933
 934   gtk_main_quit();
 935 }
 936
 937 //Sandrine : callback functions for motor test
 938
 939 GtkLabel *print_rotation_speed;
 940 gchar * sNom;
 941 void m1_vscale_callback( GtkWidget *widget, gpointer data )
 942 {
 943   value_motor1 = (int)gtk_range_get_value(GTK_RANGE(widget));
 944
 945   if( avoid_cycle < 4 )
 946   {
 947     if( data && all_motors )
 948     {
 949       gtk_range_set_value( GTK_RANGE(data), value_motor1 );
 950
 951       avoid_cycle ++;
 952     }
 953   }
 954   else
 955   {
 956     avoid_cycle = 0;
 957   }
 958         if (TRUE == send_pwm_active){
 959                         ardrone_at_set_pwm(value_motor1, value_motor2, value_motor3, value_motor4);
 960         }
 961         rotation_speed_m1 = 1200 + value_motor1*14;
 962         sNom = g_strdup_printf ("command speed (turn/min) : %d %d %d %d \n",rotation_speed_m1, rotation_speed_m2, rotation_speed_m3, rotation_speed_m4);
 963         gtk_label_set_text(print_rotation_speed, sNom);
 964 }
 965
 966 void m2_vscale_callback( GtkWidget *widget, gpointer data )
 967 {
 968   value_motor2 = (int)gtk_range_get_value(GTK_RANGE(widget));
 969
 970   if( avoid_cycle < 4 )
 971   {
 972     if( data && all_motors )
 973     {
 974       gtk_range_set_value( GTK_RANGE(data), value_motor2 );
 975
 976       avoid_cycle ++;
 977     }
 978   }
 979   else
 980   {
 981     avoid_cycle = 0;
 982   }
 983         if (TRUE == send_pwm_active){
 984                         ardrone_at_set_pwm(value_motor1, value_motor2, value_motor3, value_motor4);
 985         }
 986         rotation_speed_m2 = 1200 + value_motor2*14;
 987         sNom = g_strdup_printf ("command speed (turn/min) : %d %d %d %d \n",rotation_speed_m1, rotation_speed_m2, rotation_speed_m3, rotation_speed_m4);
 988         gtk_label_set_text(print_rotation_speed, sNom);
 989 }
 990
 991 void m3_vscale_callback( GtkWidget *widget, gpointer data )
 992 {
 993   value_motor3 = (int)gtk_range_get_value(GTK_RANGE(widget));
 994
 995   if( avoid_cycle < 4 )
 996   {
 997     if( data && all_motors )
 998     {
 999       gtk_range_set_value( GTK_RANGE(data), value_motor3 );
1000
1001       avoid_cycle ++;
1002     }
1003   }
1004   else
1005   {
1006     avoid_cycle = 0;
1007   }
1008         if (TRUE == send_pwm_active){
1009                         ardrone_at_set_pwm(value_motor1, value_motor2, value_motor3, value_motor4);
1010         }
1011         rotation_speed_m3 = 1200 + value_motor3*14;
1012         sNom = g_strdup_printf ("command speed (turn/min) : %d %d %d %d \n",rotation_speed_m1, rotation_speed_m2, rotation_speed_m3, rotation_speed_m4);
1013         gtk_label_set_text(print_rotation_speed, sNom);
1014 }
1015
1016 void m4_vscale_callback( GtkWidget *widget, gpointer data )
1017 {
1018   value_motor4 = (int)gtk_range_get_value(GTK_RANGE(widget));
1019
1020   if( avoid_cycle < 4 )
1021   {
1022     if( data && all_motors )
1023     {
1024       gtk_range_set_value( GTK_RANGE(data), value_motor4 );
1025
1026       avoid_cycle ++;
1027     }
1028   }
1029   else
1030   {
1031     avoid_cycle = 0;
1032   }
1033         if (TRUE == send_pwm_active){
1034                         ardrone_at_set_pwm(value_motor1, value_motor2, value_motor3, value_motor4);
1035         }
1036         rotation_speed_m4 = 1200 + value_motor4*14;
1037         sNom = g_strdup_printf ("command speed (turn/min) : %d %d %d %d \n",rotation_speed_m1, rotation_speed_m2, rotation_speed_m3, rotation_speed_m4);
1038         gtk_label_set_text(print_rotation_speed, sNom);
1039 }
1040
1041 void mall_check_button_callback( GtkWidget *widget, gpointer data )
1042 {
1043   all_motors = 1 - all_motors;
1044 }
1045
1046 void m_send_pwm_callback( GtkWidget *widget, gpointer data )
1047 {
1048         send_pwm_active = 1 - send_pwm_active;
1049         if (TRUE == send_pwm_active){
1050                 ardrone_at_set_pwm(value_motor1, value_motor2, value_motor3, value_motor4);
1051         }
1052         else {
1053                 ardrone_at_set_pwm(0, 0, 0, 0);
1054         }
1055 }
1056
1057 void m_stop_motors_callback( GtkWidget *widget, gpointer data )
1058 {
1059         ardrone_at_set_pwm(0, 0, 0, 0);
1060 }
1061
1062 void ihm_initCA(SIHM_CurveAttributes *pCA, char* title_str, char* ctrl_lbl, int nb_val, int nb_range)
1063 {
1064   /* initialize a SIHM_CurveAttributes structure */
1065   int k;
1066
1067   strncpy(pCA->title, title_str, sizeof(pCA->title));
1068   pCA->title[sizeof(pCA->title)-1] = 0;
1069   strncpy(pCA->ctrl_lbl, ctrl_lbl, sizeof(pCA->ctrl_lbl));
1070   pCA->ctrl_lbl[sizeof(pCA->ctrl_lbl)-1] = 0;
1071   pCA->show = TRUE;
1072   pCA->nb_val = nb_val;
1073   pCA->legend = (char**) vp_os_malloc(nb_val * sizeof(char*));
1074   for (k=0; k<nb_val; k++) pCA->legend[k] = (char*) vp_os_malloc(32);
1075   pCA->GC = (GdkGC**) vp_os_malloc(nb_val * sizeof(GdkGC*));
1076   pCA->tval = (double**) vp_os_malloc(nb_val * sizeof(double*));
1077   for (k=0; k<nb_val; k++) pCA->tval[k] = (double*) vp_os_malloc(KIHM_N_PT2PLOT*sizeof(double));
1078   pCA->nb_range = nb_range;
1079   pCA->range = pCA->nb_range-1;
1080   pCA->tSA = (SIHM_ScaleAttributes*) vp_os_malloc(nb_range*sizeof(SIHM_ScaleAttributes));
1081
1082   pCA->PrivateWin = gtk_window_new( GTK_WINDOW_TOPLEVEL);
1083   gtk_window_set_position(GTK_WINDOW(pCA->PrivateWin), GTK_WIN_POS_CENTER_ALWAYS);
1084   gtk_window_set_title(GTK_WINDOW(pCA->PrivateWin), title_str);
1085   gtk_signal_connect(GTK_OBJECT(pCA->PrivateWin), "destroy", G_CALLBACK(ihm_private_window_destroy), (gpointer)pCA );
1086   pCA->PrivateVBox = gtk_vbox_new(FALSE, 0);
1087   gtk_container_add(GTK_CONTAINER(pCA->PrivateWin), pCA->PrivateVBox);
1088   pCA->win_view = WINDOW_HIDE;
1089 }
1090
1091
1092 void ihm_createGCs(GtkWidget *Drawable)
1093 {
1094   /* Graphic contexts definition */
1095   /* --------------------------- */
1096   GdkColor Color;
1097
1098   // Red
1099   Color.red   = 0xFFFF;
1100   Color.green = 0;
1101   Color.blue  = 0;
1102   if (gdk_colormap_alloc_color(gdk_colormap_get_system(), &Color, FALSE, TRUE) == FALSE)
1103     perror("gdk_colormap_alloc_color");
1104   ihm_GC[KIHM_GC_RED] = gdk_gc_new(Drawable->window);
1105   gdk_gc_set_foreground(ihm_GC[KIHM_GC_RED], &Color);
1106
1107   // light Red
1108   Color.red   = 0xCFFF;
1109   Color.green = 0;
1110   Color.blue  = 0;
1111   if (gdk_colormap_alloc_color(gdk_colormap_get_system(), &Color, FALSE, TRUE) == FALSE)
1112     perror("gdk_colormap_alloc_color");
1113   ihm_GC[KIHM_GC_LIGHTRED] = gdk_gc_new(Drawable->window);
1114   gdk_gc_set_foreground(ihm_GC[KIHM_GC_LIGHTRED], &Color);
1115
1116   // Green
1117   Color.red   = 0;
1118   Color.green = 0xDFFF;
1119   Color.blue  = 0;
1120   if (gdk_colormap_alloc_color(gdk_colormap_get_system(), &Color, FALSE, TRUE) == FALSE)
1121     perror("gdk_colormap_alloc_color");
1122   ihm_GC[KIHM_GC_GREEN] = gdk_gc_new(Drawable->window);
1123   gdk_gc_set_foreground(ihm_GC[KIHM_GC_GREEN], &Color);
1124
1125   // Blue
1126   Color.red   = 0;
1127   Color.green = 0;
1128   Color.blue  = 0xFFFF;
1129   if (gdk_colormap_alloc_color(gdk_colormap_get_system(), &Color, FALSE, TRUE) == FALSE)
1130     perror("gdk_colormap_alloc_color");
1131   ihm_GC[KIHM_GC_BLUE] = gdk_gc_new(Drawable->window);
1132   gdk_gc_set_foreground(ihm_GC[KIHM_GC_BLUE], &Color);
1133
1134   // Grey
1135   Color.red   = 0xD000;
1136   Color.green = 0xD000;
1137   Color.blue  = 0xD000;
1138   if (gdk_colormap_alloc_color(gdk_colormap_get_system(), &Color, FALSE, TRUE) == FALSE)
1139     perror("gdk_colormap_alloc_color");
1140   ihm_GC[KIHM_GC_GREY] = gdk_gc_new(Drawable->window);
1141   gdk_gc_set_foreground(ihm_GC[KIHM_GC_GREY], &Color);
1142
1143   // Light blue
1144   Color.red   = 0;
1145   Color.green = 0;
1146   Color.blue  = 0xF000;
1147   if (gdk_colormap_alloc_color(gdk_colormap_get_system(), &Color, FALSE, TRUE) == FALSE)
1148     perror("gdk_colormap_alloc_color");
1149   ihm_GC[KIHM_GC_LIGHTBLUE] = gdk_gc_new(Drawable->window);
1150   gdk_gc_set_foreground(ihm_GC[KIHM_GC_LIGHTBLUE], &Color);
1151
1152   // Light grey
1153   Color.red   = 0xF000;
1154   Color.green = 0xF000;
1155   Color.blue  = 0xF000;
1156   if (gdk_colormap_alloc_color(gdk_colormap_get_system(), &Color, FALSE, TRUE) == FALSE)
1157     perror("gdk_colormap_alloc_color");
1158   ihm_GC[KIHM_GC_LIGHTGREY] = gdk_gc_new(Drawable->window);
1159   gdk_gc_set_foreground(ihm_GC[KIHM_GC_LIGHTGREY], &Color);
1160
1161   // Light grey dashed lines
1162   int LineWidth = 1;
1163   ihm_GC[KIHM_GC_DASHLINE] = gdk_gc_new(Drawable->window);
1164   gdk_gc_set_foreground(ihm_GC[KIHM_GC_DASHLINE], &Color);
1165   gdk_gc_set_line_attributes(ihm_GC[KIHM_GC_DASHLINE],
1166                              LineWidth,
1167                              GDK_LINE_ON_OFF_DASH,
1168                              GDK_CAP_NOT_LAST,
1169                              GDK_JOIN_BEVEL);
1170
1171   // Black for individual window GC
1172   Color.red   = 0x0000;
1173   Color.green = 0x0000;
1174   Color.blue  = 0x0000;
1175   if (gdk_colormap_alloc_color(gdk_colormap_get_system(), &Color, FALSE, TRUE) == FALSE)
1176     perror("gdk_colormap_alloc_color");
1177   ihm_GC[KIHM_GC_BLACK] = gdk_gc_new(Drawable->window);
1178   gdk_gc_set_foreground(ihm_GC[KIHM_GC_BLACK], &Color);
1179
1180   // White for individual window GC
1181   Color.red   = 0xFFFF;
1182   Color.green = 0xFFFF;
1183   Color.blue  = 0xFFFF;
1184   if (gdk_colormap_alloc_color(gdk_colormap_get_system(), &Color, FALSE, TRUE) == FALSE)
1185     perror("gdk_colormap_alloc_color");
1186   ihm_GC[KIHM_GC_WHITE] = gdk_gc_new(Drawable->window);
1187   gdk_gc_set_foreground(ihm_GC[KIHM_GC_WHITE], &Color);
1188 }
1189
1190
1191 GtkWidget * ihm_createCurve(SIHM_CurveAttributes *pCA)
1192 {
1193   /* Creation of widget hierarchy for curve display */
1194   /* ---------------------------------------------- */
1195   int k,i;
1196   GtkWidget *frame, *vBox, *hBox_LegendValue, *hBox_Curve, *vBox_Ctrl;
1197   GtkWidget *button_PlusScale, *button_MinusScale, *label_CtrlScale;
1198   GdkGCValues GCval;
1199   GdkColor legend_color;
1200   static PangoFontDescription* pPFD = NULL;
1201
1202   // one-time font allocation
1203   if (pPFD == NULL)
1204     pPFD = pango_font_description_from_string("terminal 12"); // select a standard fixed font
1205
1206   // Legend and value labels
1207   pCA->label_Legend = (GtkWidget**) vp_os_malloc(pCA->nb_val*sizeof(GtkWidget*));
1208   pCA->lblVal  = (GtkWidget**) vp_os_malloc(pCA->nb_val*sizeof(GtkWidget*));
1209   for (k=0; k<pCA->nb_val; k++) {
1210     pCA->label_Legend[k] = gtk_label_new(pCA->legend[k]);
1211     gtk_widget_modify_font(pCA->label_Legend[k], pPFD);
1212     // retrieve foreground colors of GC in 3 steps
1213     //  1) retrieve GC values but only pixel field of foreground is set
1214     //  2) retrieve rgb components with gdk_colormap_query_color
1215     //  3) set foreground color with gtk_widget_modify_fg
1216     gdk_gc_get_values(pCA->GC[k], &GCval);
1217     gdk_colormap_query_color(gdk_gc_get_colormap(pCA->GC[k]),
1218                              GCval.foreground.pixel,
1219                              &legend_color);
1220     gtk_widget_modify_fg(pCA->label_Legend[k], GTK_STATE_NORMAL, &legend_color);
1221     pCA->lblVal[k] = gtk_label_new("...");
1222     gtk_widget_modify_font(pCA->lblVal[k], pPFD);
1223   }
1224
1225   for (i=0;i<pCA->nb_range;i++) {
1226     pCA->tSA[i].vBox_Grad = gtk_vbox_new(FALSE, 0);
1227
1228     pCA->tSA[i].phys_range = pCA->tSA[i].phys_max - pCA->tSA[i].phys_min;
1229     pCA->tSA[i].nb_grad = (int) round(pCA->tSA[i].phys_range / pCA->tSA[i].phys_step) + 1;
1230     pCA->tSA[i].y_orig = KIHM_DAREA_CURVE_Y_OFFSET +
1231       (int) ((pCA->tSA[i].phys_max * KIHM_DAREA_CURVE_Y_SIZE) / pCA->tSA[i].phys_range);
1232     pCA->tSA[i].y_min = pCA->tSA[i].y_orig -
1233       (int) (((pCA->tSA[i].phys_min - pCA->tSA[i].phys_orig) * KIHM_DAREA_CURVE_Y_SIZE) / pCA->tSA[i].phys_range);
1234     pCA->tSA[i].y_max = pCA->tSA[i].y_orig -
1235       (int) (((pCA->tSA[i].phys_max - pCA->tSA[i].phys_orig) * KIHM_DAREA_CURVE_Y_SIZE) / pCA->tSA[i].phys_range);
1236
1237     // Graduation labels and empty boxes to separate graduations regularly
1238     char grad_str[20];
1239     GtkWidget *label_Grad, *hbox_InterGrad;
1240
1241     for (k=0; k<pCA->tSA[i].nb_grad-1; k++) {
1242       sprintf(grad_str, pCA->tSA[i].grad_format, pCA->tSA[i].phys_min + k*pCA->tSA[i].phys_step);
1243       label_Grad = gtk_label_new(grad_str);
1244       hbox_InterGrad = gtk_hbox_new(FALSE,0);
1245       gtk_box_pack_end(GTK_BOX(pCA->tSA[i].vBox_Grad), label_Grad    , FALSE, FALSE, 0);
1246       gtk_box_pack_end(GTK_BOX(pCA->tSA[i].vBox_Grad), hbox_InterGrad,  TRUE, FALSE, 0);
1247     }
1248     k = pCA->tSA[i].nb_grad-1;
1249     sprintf(grad_str, pCA->tSA[i].grad_format, pCA->tSA[i].phys_min + k*pCA->tSA[i].phys_step);
1250     label_Grad = gtk_label_new(grad_str);
1251     gtk_box_pack_end(GTK_BOX(pCA->tSA[i].vBox_Grad), label_Grad, FALSE, FALSE, 0);
1252   }
1253
1254   // scale control
1255   GList* children_list;
1256   button_PlusScale = gtk_button_new_with_label("+");
1257   children_list = gtk_container_get_children(GTK_CONTAINER(button_PlusScale));
1258   gtk_label_set_justify(GTK_LABEL(children_list->data), GTK_JUSTIFY_CENTER);
1259   g_signal_connect(G_OBJECT(button_PlusScale), "clicked", G_CALLBACK(ihm_plusScale), (gpointer)pCA);
1260   button_MinusScale = gtk_button_new_with_label("-");
1261   children_list = gtk_container_get_children(GTK_CONTAINER(button_MinusScale));
1262   gtk_label_set_justify(GTK_LABEL(children_list->data), GTK_JUSTIFY_CENTER);
1263   g_signal_connect(G_OBJECT(button_MinusScale), "clicked", G_CALLBACK(ihm_minusScale), (gpointer)pCA);
1264   label_CtrlScale = gtk_label_new("Scale\nadjust");
1265
1266   // Creation of other widgets
1267   frame = gtk_frame_new(pCA->title);
1268   vBox = gtk_vbox_new(FALSE, 0);
1269   hBox_LegendValue = gtk_hbox_new(FALSE, 0);
1270   hBox_Curve = gtk_hbox_new(FALSE, 0);
1271   vBox_Ctrl = gtk_vbox_new(FALSE, 0);
1272   pCA->DA = gtk_drawing_area_new();
1273   gtk_drawing_area_size((GtkDrawingArea*)pCA->DA,
1274                         KIHM_DAREA_CURVE_X_SIZE,
1275                         KIHM_DAREA_CURVE_Y_SIZE+2*KIHM_DAREA_CURVE_Y_OFFSET);
1276
1277   // Creation of widget hierarchy
1278   gtk_box_pack_start(GTK_BOX(vBox_Ctrl), button_PlusScale , FALSE, FALSE, 5);
1279   gtk_box_pack_start(GTK_BOX(vBox_Ctrl), label_CtrlScale  , FALSE, FALSE, 5);
1280   gtk_box_pack_start(GTK_BOX(vBox_Ctrl), button_MinusScale, FALSE, FALSE, 5);
1281
1282   for (k=0; k<pCA->nb_val; k++) {
1283     gtk_box_pack_start(GTK_BOX(hBox_LegendValue), pCA->label_Legend[k], FALSE, FALSE, 0);
1284     gtk_box_pack_start(GTK_BOX(hBox_LegendValue), pCA->lblVal[k] , FALSE, FALSE, 0);
1285   }
1286
1287   gtk_box_pack_end(GTK_BOX(hBox_Curve), pCA->DA  , FALSE, FALSE, 0);
1288   for (i=0;i<pCA->nb_range;i++)
1289     gtk_box_pack_end(GTK_BOX(hBox_Curve), pCA->tSA[i].vBox_Grad,  TRUE,  TRUE, 0);
1290   gtk_box_pack_end(GTK_BOX(hBox_Curve), vBox_Ctrl,  TRUE,  TRUE, 0);
1291
1292   gtk_box_pack_start(GTK_BOX(vBox), hBox_LegendValue, FALSE, TRUE, 0);
1293   gtk_box_pack_start(GTK_BOX(vBox), hBox_Curve      , FALSE, TRUE, 0);
1294   gtk_container_add(GTK_CONTAINER(frame), vBox);
1295   pCA->top = frame;
1296   gtk_box_pack_start(GTK_BOX(pCA->PrivateVBox), pCA->top, FALSE, TRUE, 5 );
1297
1298         return vBox;
1299 }
1300
1301
1302 void ihm_destroyCurves()
1303 {
1304   int i;
1305   for(i = 0 ; i < KIHM_NB_CURVES ; i++)
1306     {
1307       vp_os_free(ihm_CA[i].label_Legend);
1308       vp_os_free(ihm_CA[i].lblVal);
1309     }
1310 }
1311
1312
1313 void ihm_initCurves(GtkWidget *widget, int curve)
1314 {
1315   int scale_idx;
1316
1317   switch( curve )
1318   {
1319     /* Angular rates display */
1320     /* --------------------- */
1321   case KIHM_CURVE_GYRO:
1322     ihm_initCA(&ihm_CA[KIHM_CURVE_GYRO], GYRO_WIN_TITLE, "Angular rates", 4, 4);
1323     strcpy(ihm_CA[KIHM_CURVE_GYRO].legend[0], "Gyro X (p)");
1324     strcpy(ihm_CA[KIHM_CURVE_GYRO].legend[1], "Gyro Y (q)");
1325     strcpy(ihm_CA[KIHM_CURVE_GYRO].legend[2], "Gyro Z (r)");
1326     strcpy(ihm_CA[KIHM_CURVE_GYRO].legend[3], "Table");
1327     ihm_CA[KIHM_CURVE_GYRO].GC[0] = ihm_GC[KIHM_GC_BLUE];
1328     ihm_CA[KIHM_CURVE_GYRO].GC[1] = ihm_GC[KIHM_GC_GREEN];
1329     ihm_CA[KIHM_CURVE_GYRO].GC[2] = ihm_GC[KIHM_GC_RED];
1330     ihm_CA[KIHM_CURVE_GYRO].GC[3] = ihm_GC[KIHM_GC_BLACK];
1331     scale_idx = 0;
1332     ihm_CA[KIHM_CURVE_GYRO].tSA[scale_idx].phys_orig =    0.0;
1333     ihm_CA[KIHM_CURVE_GYRO].tSA[scale_idx].phys_min  =   -1.0;
1334     ihm_CA[KIHM_CURVE_GYRO].tSA[scale_idx].phys_max  =    1.0;
1335     ihm_CA[KIHM_CURVE_GYRO].tSA[scale_idx].phys_step =    0.2;
1336     strcpy(ihm_CA[KIHM_CURVE_GYRO].tSA[scale_idx].grad_format, "% 6.1f");
1337     scale_idx++;
1338     ihm_CA[KIHM_CURVE_GYRO].tSA[scale_idx].phys_orig =    0.0;
1339     ihm_CA[KIHM_CURVE_GYRO].tSA[scale_idx].phys_min  =   -5.0;
1340     ihm_CA[KIHM_CURVE_GYRO].tSA[scale_idx].phys_max  =    5.0;
1341     ihm_CA[KIHM_CURVE_GYRO].tSA[scale_idx].phys_step =    1.0;
1342     strcpy(ihm_CA[KIHM_CURVE_GYRO].tSA[scale_idx].grad_format, "% 6.1f");
1343     scale_idx++;
1344     ihm_CA[KIHM_CURVE_GYRO].tSA[scale_idx].phys_orig =    0.0;
1345     ihm_CA[KIHM_CURVE_GYRO].tSA[scale_idx].phys_min  =  -20.0;
1346     ihm_CA[KIHM_CURVE_GYRO].tSA[scale_idx].phys_max  =   20.0;
1347     ihm_CA[KIHM_CURVE_GYRO].tSA[scale_idx].phys_step =    5.0;
1348     strcpy(ihm_CA[KIHM_CURVE_GYRO].tSA[scale_idx].grad_format, "% 6.1f");
1349     scale_idx++;
1350     ihm_CA[KIHM_CURVE_GYRO].tSA[scale_idx].phys_orig =    0.0;
1351     ihm_CA[KIHM_CURVE_GYRO].tSA[scale_idx].phys_min  = -100.0;
1352     ihm_CA[KIHM_CURVE_GYRO].tSA[scale_idx].phys_max  =  100.0;
1353     ihm_CA[KIHM_CURVE_GYRO].tSA[scale_idx].phys_step =   20.0;
1354     strcpy(ihm_CA[KIHM_CURVE_GYRO].tSA[scale_idx].grad_format, "% 6.1f");
1355     strcpy(ihm_CA[KIHM_CURVE_GYRO].val_format , "% 7.2f");
1356     ihm_createCurve(&ihm_CA[KIHM_CURVE_GYRO]);
1357     break;
1358
1359
1360     /* Accelerations display */
1361     /* --------------------- */
1362   case KIHM_CURVE_ACC:
1363     ihm_initCA(&ihm_CA[KIHM_CURVE_ACC], ACC_WIN_TITLE , "Accelerations", 3, 2);
1364     strcpy(ihm_CA[KIHM_CURVE_ACC].legend[0], "Acc X");
1365     strcpy(ihm_CA[KIHM_CURVE_ACC].legend[1], "Acc Y");
1366     strcpy(ihm_CA[KIHM_CURVE_ACC].legend[2], "Acc Z");
1367     ihm_CA[KIHM_CURVE_ACC].GC[0] = ihm_GC[KIHM_GC_BLUE];
1368     ihm_CA[KIHM_CURVE_ACC].GC[1] = ihm_GC[KIHM_GC_GREEN];
1369     ihm_CA[KIHM_CURVE_ACC].GC[2] = ihm_GC[KIHM_GC_RED];
1370     scale_idx = 0;
1371     ihm_CA[KIHM_CURVE_ACC].tSA[scale_idx].phys_orig =  0.0;
1372     ihm_CA[KIHM_CURVE_ACC].tSA[scale_idx].phys_min  = -0.2;
1373     ihm_CA[KIHM_CURVE_ACC].tSA[scale_idx].phys_max  =  0.2;
1374     ihm_CA[KIHM_CURVE_ACC].tSA[scale_idx].phys_step =  0.05;
1375     strcpy(ihm_CA[KIHM_CURVE_ACC].tSA[scale_idx].grad_format, "% 5.2f");
1376     scale_idx++;
1377     ihm_CA[KIHM_CURVE_ACC].tSA[scale_idx].phys_orig =  0.0;
1378     ihm_CA[KIHM_CURVE_ACC].tSA[scale_idx].phys_min  = -1.5;
1379     ihm_CA[KIHM_CURVE_ACC].tSA[scale_idx].phys_max  =  1.5;
1380     ihm_CA[KIHM_CURVE_ACC].tSA[scale_idx].phys_step =  0.5;
1381     strcpy(ihm_CA[KIHM_CURVE_ACC].tSA[scale_idx].grad_format, "% 4.1f");
1382     strcpy(ihm_CA[KIHM_CURVE_ACC].val_format , "% 6.3f");
1383     ihm_createCurve(&ihm_CA[KIHM_CURVE_ACC]);
1384     break;
1385
1386     /* theta (pitch) display */
1387     /* --------------------- */
1388     // there are 4 different values: computed with acc or gyros or both and reference
1389     // they are ordered in ihm_CA[KIHM_CURVE_THETA] as follow
1390     //   1) fused values
1391     //   2) reference
1392     //   3) accelerometers
1393     //   4) gyrometers
1394     // 3 and 4 may be optionnal
1395   case KIHM_CURVE_THETA:
1396     ihm_initCA(&ihm_CA[KIHM_CURVE_THETA], THETA_WIN_TITLE, "Theta", 6, 4);
1397     strcpy(ihm_CA[KIHM_CURVE_THETA].legend[0], "Fus");
1398     strcpy(ihm_CA[KIHM_CURVE_THETA].legend[1], "Ref");
1399     strcpy(ihm_CA[KIHM_CURVE_THETA].legend[2], "Acc");
1400     strcpy(ihm_CA[KIHM_CURVE_THETA].legend[3], "Gyr");
1401     strcpy(ihm_CA[KIHM_CURVE_THETA].legend[4], "Table");
1402     strcpy(ihm_CA[KIHM_CURVE_THETA].legend[5], "POLARIS");
1403     ihm_CA[KIHM_CURVE_THETA].GC[0] = ihm_GC[KIHM_GC_RED];
1404     ihm_CA[KIHM_CURVE_THETA].GC[1] = ihm_GC[KIHM_GC_BLACK];
1405     ihm_CA[KIHM_CURVE_THETA].GC[2] = ihm_GC[KIHM_GC_BLUE];
1406     ihm_CA[KIHM_CURVE_THETA].GC[3] = ihm_GC[KIHM_GC_GREEN];
1407     ihm_CA[KIHM_CURVE_THETA].GC[4] = ihm_GC[KIHM_GC_LIGHTRED];
1408     ihm_CA[KIHM_CURVE_THETA].GC[5] = ihm_GC[KIHM_GC_LIGHTBLUE];
1409     scale_idx = 0;
1410     ihm_CA[KIHM_CURVE_THETA].tSA[scale_idx].phys_orig =   0.0;
1411     ihm_CA[KIHM_CURVE_THETA].tSA[scale_idx].phys_min  =  -1.0;
1412     ihm_CA[KIHM_CURVE_THETA].tSA[scale_idx].phys_max  =   1.0;
1413     ihm_CA[KIHM_CURVE_THETA].tSA[scale_idx].phys_step =   0.2;
1414     strcpy(ihm_CA[KIHM_CURVE_THETA].tSA[scale_idx].grad_format, "% 5.1f");
1415     scale_idx++;
1416     ihm_CA[KIHM_CURVE_THETA].tSA[scale_idx].phys_orig =   0.0;
1417     ihm_CA[KIHM_CURVE_THETA].tSA[scale_idx].phys_min  =  -5.0;
1418     ihm_CA[KIHM_CURVE_THETA].tSA[scale_idx].phys_max  =   5.0;
1419     ihm_CA[KIHM_CURVE_THETA].tSA[scale_idx].phys_step =   1.0;
1420     strcpy(ihm_CA[KIHM_CURVE_THETA].tSA[scale_idx].grad_format, "% 5.1f");
1421     scale_idx++;
1422     ihm_CA[KIHM_CURVE_THETA].tSA[scale_idx].phys_orig =   0.0;
1423     ihm_CA[KIHM_CURVE_THETA].tSA[scale_idx].phys_min  = -15.0;
1424     ihm_CA[KIHM_CURVE_THETA].tSA[scale_idx].phys_max  =  15.0;
1425     ihm_CA[KIHM_CURVE_THETA].tSA[scale_idx].phys_step =   3.0;
1426     strcpy(ihm_CA[KIHM_CURVE_THETA].tSA[scale_idx].grad_format, "% 5.1f");
1427     scale_idx++;
1428     ihm_CA[KIHM_CURVE_THETA].tSA[scale_idx].phys_orig =   0.0;
1429     ihm_CA[KIHM_CURVE_THETA].tSA[scale_idx].phys_min  = -45.0;
1430     ihm_CA[KIHM_CURVE_THETA].tSA[scale_idx].phys_max  =  45.0;
1431     ihm_CA[KIHM_CURVE_THETA].tSA[scale_idx].phys_step =  15.0;
1432     strcpy(ihm_CA[KIHM_CURVE_THETA].tSA[scale_idx].grad_format, "% 5.1f");
1433     strcpy(ihm_CA[KIHM_CURVE_THETA].val_format , "% 7.2f");
1434     ihm_createCurve(&ihm_CA[KIHM_CURVE_THETA]);
1435     printf("   ihm_init : theta display\n");
1436     break;
1437
1438     /* phi (roll) display */
1439     /* ------------------ */
1440     // there are 4 different values: computed with acc or gyros or both and reference (see theta comment)
1441   case KIHM_CURVE_PHI:
1442     ihm_initCA(&ihm_CA[KIHM_CURVE_PHI], PHI_WIN_TITLE, "Phi", 6, 4);
1443     strcpy(ihm_CA[KIHM_CURVE_PHI].legend[0], "Fus");
1444     strcpy(ihm_CA[KIHM_CURVE_PHI].legend[1], "Ref");
1445     strcpy(ihm_CA[KIHM_CURVE_PHI].legend[2], "Acc");
1446     strcpy(ihm_CA[KIHM_CURVE_PHI].legend[3], "Gyr");
1447     strcpy(ihm_CA[KIHM_CURVE_PHI].legend[4], "Table");
1448     strcpy(ihm_CA[KIHM_CURVE_PHI].legend[5], "POLARIS");
1449     ihm_CA[KIHM_CURVE_PHI].GC[0] = ihm_GC[KIHM_GC_RED];
1450     ihm_CA[KIHM_CURVE_PHI].GC[1] = ihm_GC[KIHM_GC_BLACK];
1451     ihm_CA[KIHM_CURVE_PHI].GC[2] = ihm_GC[KIHM_GC_BLUE];
1452     ihm_CA[KIHM_CURVE_PHI].GC[3] = ihm_GC[KIHM_GC_GREEN];
1453     ihm_CA[KIHM_CURVE_PHI].GC[4] = ihm_GC[KIHM_GC_LIGHTRED];
1454     ihm_CA[KIHM_CURVE_PHI].GC[5] = ihm_GC[KIHM_GC_LIGHTBLUE];
1455     scale_idx = 0;
1456     ihm_CA[KIHM_CURVE_PHI].tSA[scale_idx].phys_orig =   0.0;
1457     ihm_CA[KIHM_CURVE_PHI].tSA[scale_idx].phys_min  =  -1.0;
1458     ihm_CA[KIHM_CURVE_PHI].tSA[scale_idx].phys_max  =   1.0;
1459     ihm_CA[KIHM_CURVE_PHI].tSA[scale_idx].phys_step =   0.2;
1460     strcpy(ihm_CA[KIHM_CURVE_PHI].tSA[scale_idx].grad_format, "% 5.1f");
1461     scale_idx++;
1462     ihm_CA[KIHM_CURVE_PHI].tSA[scale_idx].phys_orig =   0.0;
1463     ihm_CA[KIHM_CURVE_PHI].tSA[scale_idx].phys_min  =  -5.0;
1464     ihm_CA[KIHM_CURVE_PHI].tSA[scale_idx].phys_max  =   5.0;
1465     ihm_CA[KIHM_CURVE_PHI].tSA[scale_idx].phys_step =   1.0;
1466     strcpy(ihm_CA[KIHM_CURVE_PHI].tSA[scale_idx].grad_format, "% 5.1f");
1467     scale_idx++;
1468     ihm_CA[KIHM_CURVE_PHI].tSA[scale_idx].phys_orig =   0.0;
1469     ihm_CA[KIHM_CURVE_PHI].tSA[scale_idx].phys_min  = -15.0;
1470     ihm_CA[KIHM_CURVE_PHI].tSA[scale_idx].phys_max  =  15.0;
1471     ihm_CA[KIHM_CURVE_PHI].tSA[scale_idx].phys_step =   3.0;
1472     strcpy(ihm_CA[KIHM_CURVE_PHI].tSA[scale_idx].grad_format, "% 5.1f");
1473     scale_idx++;
1474     ihm_CA[KIHM_CURVE_PHI].tSA[scale_idx].phys_orig =   0.0;
1475     ihm_CA[KIHM_CURVE_PHI].tSA[scale_idx].phys_min  = -45.0;
1476     ihm_CA[KIHM_CURVE_PHI].tSA[scale_idx].phys_max  =  45.0;
1477     ihm_CA[KIHM_CURVE_PHI].tSA[scale_idx].phys_step =  15.0;
1478     strcpy(ihm_CA[KIHM_CURVE_PHI].tSA[scale_idx].grad_format, "% 5.1f");
1479     strcpy(ihm_CA[KIHM_CURVE_PHI].val_format , "% 7.2f");
1480     ihm_createCurve(&ihm_CA[KIHM_CURVE_PHI]);
1481     printf("   ihm_init : phi display\n");
1482     break;
1483
1484     /* vbat display */
1485     /* ------------ */
1486   case KIHM_CURVE_VBAT:
1487     ihm_initCA(&ihm_CA[KIHM_CURVE_VBAT], VBAT_WIN_TITLE, "VBAT", 2, 1);
1488     strcpy(ihm_CA[KIHM_CURVE_VBAT].legend[0], "Temperature [K]");
1489     strcpy(ihm_CA[KIHM_CURVE_VBAT].legend[1], "Vbat [%]");
1490     ihm_CA[KIHM_CURVE_VBAT].GC[0] = ihm_GC[KIHM_GC_RED];
1491     ihm_CA[KIHM_CURVE_VBAT].GC[1] = ihm_GC[KIHM_GC_BLUE];
1492     scale_idx = 0;
1493     ihm_CA[KIHM_CURVE_VBAT].tSA[scale_idx].phys_orig =   0.0;
1494     ihm_CA[KIHM_CURVE_VBAT].tSA[scale_idx].phys_min  =   0.0;
1495     ihm_CA[KIHM_CURVE_VBAT].tSA[scale_idx].phys_max  =  105.0;
1496     ihm_CA[KIHM_CURVE_VBAT].tSA[scale_idx].phys_step =   10.0;
1497     strcpy(ihm_CA[KIHM_CURVE_VBAT].tSA[scale_idx].grad_format, "% 5.1f");
1498     strcpy(ihm_CA[KIHM_CURVE_VBAT].val_format , "% 7.2f");
1499     ihm_createCurve(&ihm_CA[KIHM_CURVE_VBAT]);
1500     printf("   ihm_init : Vbat display\n");
1501     break;
1502
1503     /* PWM display */
1504     /* ----------- */
1505   case KIHM_CURVE_PWM:
1506
1507     ihm_initCA(&ihm_CA[KIHM_CURVE_PWM], PWM_WIN_TITLE, "PWM", 4, 1);
1508     strcpy(ihm_CA[KIHM_CURVE_PWM].legend[0], "PWM M1");
1509     strcpy(ihm_CA[KIHM_CURVE_PWM].legend[1], "PWM M2");
1510     strcpy(ihm_CA[KIHM_CURVE_PWM].legend[2], "PWM M3");
1511     strcpy(ihm_CA[KIHM_CURVE_PWM].legend[3], "PWM M4");
1512     ihm_CA[KIHM_CURVE_PWM].GC[0] = ihm_GC[KIHM_GC_RED];
1513     ihm_CA[KIHM_CURVE_PWM].GC[1] = ihm_GC[KIHM_GC_GREEN];
1514     ihm_CA[KIHM_CURVE_PWM].GC[2] = ihm_GC[KIHM_GC_BLUE];
1515     ihm_CA[KIHM_CURVE_PWM].GC[3] = ihm_GC[KIHM_GC_BLACK];
1516     scale_idx = 0;
1517     ihm_CA[KIHM_CURVE_PWM].tSA[scale_idx].phys_orig = 0.0;
1518     ihm_CA[KIHM_CURVE_PWM].tSA[scale_idx].phys_min = 0.0;
1519     ihm_CA[KIHM_CURVE_PWM].tSA[scale_idx].phys_max = 256.0;
1520     ihm_CA[KIHM_CURVE_PWM].tSA[scale_idx].phys_step = 32.0;
1521     strcpy(ihm_CA[KIHM_CURVE_PWM].tSA[scale_idx].grad_format, "% 5.1f");
1522     strcpy(ihm_CA[KIHM_CURVE_PWM].val_format, "% 5.0f");
1523
1524     GtkWidget *pMainVBox; // pointer on window displaying curve
1525
1526
1527     pMainVBox = ihm_createCurve(&ihm_CA[KIHM_CURVE_PWM]);
1528     printf("   ihm_init : PWM display\n");
1529
1530     GtkWidget *hbox_motors = gtk_hbox_new( FALSE, 20 );
1531     GtkWidget *m_fill_space_on_left    = gtk_label_new( "                                       " );
1532     GtkWidget *m1_vscale            = gtk_vscale_new_with_range( 0.0, 255.0, 1.0 );
1533     GtkWidget *m2_vscale            = gtk_vscale_new_with_range( 0.0, 255.0, 1.0 );
1534     GtkWidget *m3_vscale            = gtk_vscale_new_with_range( 0.0, 255.0, 1.0 );
1535         GtkWidget *m4_vscale            = gtk_vscale_new_with_range( 0.0, 255.0, 1.0 );
1536         GtkWidget *mall_check_button    = gtk_check_button_new_with_label( "all motors" );
1537         GtkWidget *m_send_pwm    = gtk_check_button_new_with_label( "send pwm" );
1538         GtkWidget *m_stop_motors    = gtk_check_button_new_with_label( "stop" );
1539         GtkFrame* motors_frame = GTK_FRAME( gtk_frame_new(NULL) );
1540         GtkFrame* speed_frame = GTK_FRAME( gtk_frame_new(NULL) );
1541         // gtk_frame_set_label( motors_frame, "Send pwm to motors" );
1542
1543         gtk_scale_set_draw_value( GTK_SCALE(m1_vscale), TRUE );
1544         gtk_scale_set_draw_value( GTK_SCALE(m2_vscale), TRUE );
1545     gtk_scale_set_draw_value( GTK_SCALE(m3_vscale), TRUE );
1546     gtk_scale_set_draw_value( GTK_SCALE(m4_vscale), TRUE );
1547
1548     gtk_scale_set_value_pos( GTK_SCALE(m1_vscale), GTK_POS_BOTTOM );
1549     gtk_scale_set_value_pos( GTK_SCALE(m2_vscale), GTK_POS_BOTTOM );
1550     gtk_scale_set_value_pos( GTK_SCALE(m3_vscale), GTK_POS_BOTTOM );
1551     gtk_scale_set_value_pos( GTK_SCALE(m4_vscale), GTK_POS_BOTTOM );
1552
1553     gtk_range_set_inverted( GTK_RANGE(m1_vscale), TRUE );
1554     gtk_range_set_inverted( GTK_RANGE(m2_vscale), TRUE );
1555     gtk_range_set_inverted( GTK_RANGE(m3_vscale), TRUE );
1556     gtk_range_set_inverted( GTK_RANGE(m4_vscale), TRUE );
1557
1558     gtk_box_pack_start(GTK_BOX(pMainVBox) , GTK_WIDGET(motors_frame), FALSE, TRUE, 0 );
1559     gtk_box_pack_start(GTK_BOX(pMainVBox) , GTK_WIDGET(speed_frame), FALSE, TRUE, 0 );
1560
1561     gtk_box_pack_start(GTK_BOX(hbox_motors), m_fill_space_on_left , FALSE , TRUE, 0);
1562
1563         gtk_box_pack_start(GTK_BOX(hbox_motors), m1_vscale, FALSE, TRUE, 0 );
1564         gtk_box_pack_start(GTK_BOX(hbox_motors), m2_vscale, FALSE, TRUE, 0 );
1565         gtk_box_pack_start(GTK_BOX(hbox_motors), m3_vscale, FALSE, TRUE, 0 );
1566         gtk_box_pack_start(GTK_BOX(hbox_motors), m4_vscale, FALSE, TRUE, 0 );
1567
1568         gtk_box_pack_start(GTK_BOX(hbox_motors), mall_check_button , FALSE , TRUE, 0);
1569         gtk_box_pack_start(GTK_BOX(hbox_motors), m_send_pwm , FALSE , TRUE, 0);
1570         gtk_box_pack_start(GTK_BOX(hbox_motors), m_stop_motors , FALSE , TRUE, 0);
1571
1572         gtk_widget_set_size_request( GTK_WIDGET(motors_frame), 305, 125);
1573         gtk_container_add( GTK_CONTAINER(motors_frame), hbox_motors );
1574
1575         // !!!!! callbacks !!!!
1576         g_signal_connect( G_OBJECT(m1_vscale), "value-changed", G_CALLBACK(m1_vscale_callback), (gpointer)m2_vscale );
1577         g_signal_connect( G_OBJECT(m2_vscale), "value-changed", G_CALLBACK(m2_vscale_callback), (gpointer)m3_vscale );
1578         g_signal_connect( G_OBJECT(m3_vscale), "value-changed", G_CALLBACK(m3_vscale_callback), (gpointer)m4_vscale );
1579         g_signal_connect( G_OBJECT(m4_vscale), "value-changed", G_CALLBACK(m4_vscale_callback), (gpointer)m1_vscale );
1580         g_signal_connect( G_OBJECT(mall_check_button), "clicked", G_CALLBACK(mall_check_button_callback), NULL );
1581         g_signal_connect( G_OBJECT(m_send_pwm), "clicked", G_CALLBACK(m_send_pwm_callback), NULL );
1582         g_signal_connect( G_OBJECT(m_stop_motors), "clicked", G_CALLBACK(m_stop_motors_callback), NULL );
1583
1584         // affichage des 1200+nb*7*2
1585         GtkWidget *hbox_rotation_speed = gtk_hbox_new( FALSE, 20 );
1586         sNom = g_strdup_printf ("command speed (turn/min) : %d %d %d %d \n",rotation_speed_m1, rotation_speed_m2, rotation_speed_m3, rotation_speed_m4);
1587         print_rotation_speed=gtk_label_new(sNom);
1588         gtk_box_pack_start(GTK_BOX(hbox_rotation_speed), print_rotation_speed , FALSE , TRUE, 0);
1589
1590         gtk_container_add( GTK_CONTAINER(speed_frame), hbox_rotation_speed);
1591         gtk_widget_set_size_request( GTK_WIDGET(speed_frame), 305, 125);
1592
1593         break;
1594
1595
1596   /* Altitude display */
1597   /* ----------- */
1598   case KIHM_CURVE_ALTITUDE:
1599     ihm_initCA(&ihm_CA[KIHM_CURVE_ALTITUDE], ALTITUDE_WIN_TITLE, "Altitude", 4,2);
1600     strcpy(ihm_CA[KIHM_CURVE_ALTITUDE].legend[0], "Measure");
1601     strcpy(ihm_CA[KIHM_CURVE_ALTITUDE].legend[1], "Vision");
1602     strcpy(ihm_CA[KIHM_CURVE_ALTITUDE].legend[2], "Reference");
1603     strcpy(ihm_CA[KIHM_CURVE_ALTITUDE].legend[3], "Raw");
1604     ihm_CA[KIHM_CURVE_ALTITUDE].GC[0] = ihm_GC[KIHM_GC_BLUE];
1605     ihm_CA[KIHM_CURVE_ALTITUDE].GC[1] = ihm_GC[KIHM_GC_GREEN];
1606     ihm_CA[KIHM_CURVE_ALTITUDE].GC[2] = ihm_GC[KIHM_GC_BLACK];
1607     ihm_CA[KIHM_CURVE_ALTITUDE].GC[3] = ihm_GC[KIHM_GC_RED];
1608     scale_idx = 0;
1609     ihm_CA[KIHM_CURVE_ALTITUDE].tSA[scale_idx].phys_orig =   0.0;
1610     ihm_CA[KIHM_CURVE_ALTITUDE].tSA[scale_idx].phys_min  =   0.0;
1611     ihm_CA[KIHM_CURVE_ALTITUDE].tSA[scale_idx].phys_max  = 4000.0;
1612     ihm_CA[KIHM_CURVE_ALTITUDE].tSA[scale_idx].phys_step =  1000.0;
1613     strcpy(ihm_CA[KIHM_CURVE_ALTITUDE].tSA[scale_idx].grad_format, "% 5.1f");
1614     strcpy(ihm_CA[KIHM_CURVE_ALTITUDE].val_format , "% 4.0f");
1615     scale_idx++;
1616     ihm_CA[KIHM_CURVE_ALTITUDE].tSA[scale_idx].phys_orig =   0.0;
1617     ihm_CA[KIHM_CURVE_ALTITUDE].tSA[scale_idx].phys_min  =   0.0;
1618     ihm_CA[KIHM_CURVE_ALTITUDE].tSA[scale_idx].phys_max  = 8000.0;
1619     ihm_CA[KIHM_CURVE_ALTITUDE].tSA[scale_idx].phys_step = 2000.0;
1620     strcpy(ihm_CA[KIHM_CURVE_ALTITUDE].tSA[scale_idx].grad_format, "% 5.1f");
1621     strcpy(ihm_CA[KIHM_CURVE_ALTITUDE].val_format , "% 4.0f");
1622     ihm_createCurve(&ihm_CA[KIHM_CURVE_ALTITUDE]);
1623     printf("   ihm_init : Altitude display\n");
1624     break;
1625
1626   /* VX display */
1627   /* ----------- */
1628   case KIHM_CURVE_VX:
1629     ihm_initCA(&ihm_CA[KIHM_CURVE_VX], VX_WIN_TITLE, "VX", 3, 3);
1630     strcpy(ihm_CA[KIHM_CURVE_VX].legend[0], "VISION");
1631     strcpy(ihm_CA[KIHM_CURVE_VX].legend[1], "FUSION");
1632     strcpy(ihm_CA[KIHM_CURVE_VX].legend[2], "POLARIS");
1633     ihm_CA[KIHM_CURVE_VX].GC[0] = ihm_GC[KIHM_GC_RED];
1634     ihm_CA[KIHM_CURVE_VX].GC[1] = ihm_GC[KIHM_GC_BLUE];
1635     ihm_CA[KIHM_CURVE_VX].GC[2] = ihm_GC[KIHM_GC_GREEN];
1636     scale_idx = 0;
1637     ihm_CA[KIHM_CURVE_VX].tSA[scale_idx].phys_orig =   0.0;
1638     ihm_CA[KIHM_CURVE_VX].tSA[scale_idx].phys_min  =  -100;
1639     ihm_CA[KIHM_CURVE_VX].tSA[scale_idx].phys_max  =   100;
1640     ihm_CA[KIHM_CURVE_VX].tSA[scale_idx].phys_step =   50;
1641     strcpy(ihm_CA[KIHM_CURVE_VX].tSA[scale_idx].grad_format, "% 5.1f");
1642     strcpy(ihm_CA[KIHM_CURVE_VX].val_format , "% 6.4f");
1643     scale_idx++;
1644     ihm_CA[KIHM_CURVE_VX].tSA[scale_idx].phys_orig =   0.0;
1645     ihm_CA[KIHM_CURVE_VX].tSA[scale_idx].phys_min  =  -500;
1646     ihm_CA[KIHM_CURVE_VX].tSA[scale_idx].phys_max  =   500;
1647     ihm_CA[KIHM_CURVE_VX].tSA[scale_idx].phys_step =   250;
1648     strcpy(ihm_CA[KIHM_CURVE_VX].tSA[scale_idx].grad_format, "% 5.1f");
1649     strcpy(ihm_CA[KIHM_CURVE_VX].val_format , "% 6.4f");
1650     scale_idx++;
1651     ihm_CA[KIHM_CURVE_VX].tSA[scale_idx].phys_orig =   0.0;
1652     ihm_CA[KIHM_CURVE_VX].tSA[scale_idx].phys_min  =  -1000;
1653     ihm_CA[KIHM_CURVE_VX].tSA[scale_idx].phys_max  =   1000;
1654     ihm_CA[KIHM_CURVE_VX].tSA[scale_idx].phys_step =   500;
1655     strcpy(ihm_CA[KIHM_CURVE_VX].tSA[scale_idx].grad_format, "% 5.1f");
1656     strcpy(ihm_CA[KIHM_CURVE_VX].val_format , "% 6.4f");
1657     ihm_createCurve(&ihm_CA[KIHM_CURVE_VX]);
1658     printf("   ihm_init : VX display\n");
1659     break;
1660
1661   /* VY display */
1662   /* ----------- */
1663   case KIHM_CURVE_VY:
1664     ihm_initCA(&ihm_CA[KIHM_CURVE_VY], VY_WIN_TITLE, "VY", 3, 3);
1665     strcpy(ihm_CA[KIHM_CURVE_VY].legend[0], "VISION");
1666     strcpy(ihm_CA[KIHM_CURVE_VY].legend[1], "FUSION");
1667     strcpy(ihm_CA[KIHM_CURVE_VY].legend[2], "POLARIS");
1668     ihm_CA[KIHM_CURVE_VY].GC[0] = ihm_GC[KIHM_GC_RED];
1669     ihm_CA[KIHM_CURVE_VY].GC[1] = ihm_GC[KIHM_GC_BLUE];
1670     ihm_CA[KIHM_CURVE_VY].GC[2] = ihm_GC[KIHM_GC_GREEN];
1671     scale_idx = 0;
1672     ihm_CA[KIHM_CURVE_VY].tSA[scale_idx].phys_orig =   0.0;
1673     ihm_CA[KIHM_CURVE_VY].tSA[scale_idx].phys_min  =  -100;
1674     ihm_CA[KIHM_CURVE_VY].tSA[scale_idx].phys_max  =   100;
1675     ihm_CA[KIHM_CURVE_VY].tSA[scale_idx].phys_step =   50;
1676     strcpy(ihm_CA[KIHM_CURVE_VY].tSA[scale_idx].grad_format, "% 5.1f");
1677     strcpy(ihm_CA[KIHM_CURVE_VY].val_format , "% 6.4f");
1678     scale_idx++;
1679     ihm_CA[KIHM_CURVE_VY].tSA[scale_idx].phys_orig =   0.0;
1680     ihm_CA[KIHM_CURVE_VY].tSA[scale_idx].phys_min  =  -500;
1681     ihm_CA[KIHM_CURVE_VY].tSA[scale_idx].phys_max  =   500;
1682     ihm_CA[KIHM_CURVE_VY].tSA[scale_idx].phys_step =   250;
1683     strcpy(ihm_CA[KIHM_CURVE_VY].tSA[scale_idx].grad_format, "% 5.1f");
1684     strcpy(ihm_CA[KIHM_CURVE_VY].val_format , "% 6.4f");
1685     scale_idx++;
1686     ihm_CA[KIHM_CURVE_VY].tSA[scale_idx].phys_orig =   0.0;
1687     ihm_CA[KIHM_CURVE_VY].tSA[scale_idx].phys_min  =  -1000;
1688     ihm_CA[KIHM_CURVE_VY].tSA[scale_idx].phys_max  =   1000;
1689     ihm_CA[KIHM_CURVE_VY].tSA[scale_idx].phys_step =   500;
1690     strcpy(ihm_CA[KIHM_CURVE_VY].tSA[scale_idx].grad_format, "% 5.1f");
1691     strcpy(ihm_CA[KIHM_CURVE_VY].val_format , "% 6.4f");
1692     ihm_createCurve(&ihm_CA[KIHM_CURVE_VY]);
1693     printf("   ihm_init : VY display\n");
1694     break;
1695
1696
1697   /* VZ display */
1698   /* ----------- */
1699   case KIHM_CURVE_VZ:
1700     ihm_initCA(&ihm_CA[KIHM_CURVE_VZ], VZ_WIN_TITLE, "VZ", 4, 3);
1701     strcpy(ihm_CA[KIHM_CURVE_VZ].legend[0], "VISION");
1702     strcpy(ihm_CA[KIHM_CURVE_VZ].legend[1], "FUSION");
1703     strcpy(ihm_CA[KIHM_CURVE_VZ].legend[2], "POLARIS");
1704     strcpy(ihm_CA[KIHM_CURVE_VZ].legend[3], "REF");
1705     ihm_CA[KIHM_CURVE_VZ].GC[0] = ihm_GC[KIHM_GC_RED];
1706     ihm_CA[KIHM_CURVE_VZ].GC[1] = ihm_GC[KIHM_GC_BLUE];
1707     ihm_CA[KIHM_CURVE_VZ].GC[2] = ihm_GC[KIHM_GC_GREEN];
1708     ihm_CA[KIHM_CURVE_VZ].GC[3] = ihm_GC[KIHM_GC_BLACK];
1709     scale_idx = 0;
1710     ihm_CA[KIHM_CURVE_VZ].tSA[scale_idx].phys_orig =   0.0;
1711     ihm_CA[KIHM_CURVE_VZ].tSA[scale_idx].phys_min  =  -100;
1712     ihm_CA[KIHM_CURVE_VZ].tSA[scale_idx].phys_max  =   100;
1713     ihm_CA[KIHM_CURVE_VZ].tSA[scale_idx].phys_step =   50;
1714     strcpy(ihm_CA[KIHM_CURVE_VZ].tSA[scale_idx].grad_format, "% 5.1f");
1715     strcpy(ihm_CA[KIHM_CURVE_VZ].val_format , "% 6.4f");
1716     scale_idx++;
1717     ihm_CA[KIHM_CURVE_VZ].tSA[scale_idx].phys_orig =   0.0;
1718     ihm_CA[KIHM_CURVE_VZ].tSA[scale_idx].phys_min  =  -500;
1719     ihm_CA[KIHM_CURVE_VZ].tSA[scale_idx].phys_max  =   500;
1720     ihm_CA[KIHM_CURVE_VZ].tSA[scale_idx].phys_step =   250;
1721     strcpy(ihm_CA[KIHM_CURVE_VZ].tSA[scale_idx].grad_format, "% 5.1f");
1722     strcpy(ihm_CA[KIHM_CURVE_VZ].val_format , "% 6.4f");
1723     scale_idx++;
1724     ihm_CA[KIHM_CURVE_VZ].tSA[scale_idx].phys_orig =   0.0;
1725     ihm_CA[KIHM_CURVE_VZ].tSA[scale_idx].phys_min  =  -1000;
1726     ihm_CA[KIHM_CURVE_VZ].tSA[scale_idx].phys_max  =   1000;
1727     ihm_CA[KIHM_CURVE_VZ].tSA[scale_idx].phys_step =   500;
1728     strcpy(ihm_CA[KIHM_CURVE_VZ].tSA[scale_idx].grad_format, "% 5.1f");
1729     strcpy(ihm_CA[KIHM_CURVE_VZ].val_format , "% 6.4f");
1730     ihm_createCurve(&ihm_CA[KIHM_CURVE_VZ]);
1731     printf("   ihm_init : VZ display\n");
1732     break;
1733
1734   /* Motor current display */
1735   /* ----------- */
1736   case KIHM_CURVE_CURRENT:
1737     ihm_initCA(&ihm_CA[KIHM_CURVE_CURRENT], CURRENT_WIN_TITLE, "Current", 4, 1);
1738     strcpy(ihm_CA[KIHM_CURVE_CURRENT].legend[0], "Current M1");
1739     strcpy(ihm_CA[KIHM_CURVE_CURRENT].legend[1], "Current M2");
1740     strcpy(ihm_CA[KIHM_CURVE_CURRENT].legend[2], "Current M3");
1741     strcpy(ihm_CA[KIHM_CURVE_CURRENT].legend[3], "Current M4");
1742     ihm_CA[KIHM_CURVE_CURRENT].GC[0] = ihm_GC[KIHM_GC_RED];
1743     ihm_CA[KIHM_CURVE_CURRENT].GC[1] = ihm_GC[KIHM_GC_BLUE];
1744     ihm_CA[KIHM_CURVE_CURRENT].GC[2] = ihm_GC[KIHM_GC_GREEN];
1745     ihm_CA[KIHM_CURVE_CURRENT].GC[3] = ihm_GC[KIHM_GC_BLACK];
1746     scale_idx = 0;
1747     ihm_CA[KIHM_CURVE_CURRENT].tSA[scale_idx].phys_orig =   0.0;
1748     ihm_CA[KIHM_CURVE_CURRENT].tSA[scale_idx].phys_min  =   0;
1749     ihm_CA[KIHM_CURVE_CURRENT].tSA[scale_idx].phys_max  =   5000;
1750     ihm_CA[KIHM_CURVE_CURRENT].tSA[scale_idx].phys_step =   1000;
1751     strcpy(ihm_CA[KIHM_CURVE_CURRENT].tSA[scale_idx].grad_format, "% 5.1f");
1752     strcpy(ihm_CA[KIHM_CURVE_CURRENT].val_format , "% 6.4f");
1753     ihm_createCurve(&ihm_CA[KIHM_CURVE_CURRENT]);
1754     printf("   ihm_init : Current display\n");
1755     break;
1756
1757   default:
1758     printf("Unable to initialize curve: Unknown curve\n");
1759     break;
1760   }
1761 }
1762
1763 static void color_update( GtkComboBox *combo,
1764                           gpointer    data )
1765 {
1766   gchar *color;
1767   gint pos;
1768   int32_t enemy_color;
1769   pos = gtk_combo_box_get_active( combo );
1770   color = gtk_combo_box_get_active_text ( combo );
1771
1772   enemy_color = (int32_t)(pos + 1);
1773   ARDRONE_TOOL_CONFIGURATION_ADDEVENT(enemy_colors, &enemy_color, NULL);
1774
1775   DEBUG_PRINT_SDK ("Enter in color_update with color %s with idx = %d \n", color, pos+1);
1776 }
1777
1778
1779 static void flying_mode_update( GtkComboBox *combo,
1780                           gpointer    data )
1781 {
1782   int32_t pos;
1783   pos = (int32_t)gtk_combo_box_get_active( combo );
1784
1785   if (pos >= 0 && pos < FLYING_MODE_NUM)
1786   {
1787           ARDRONE_TOOL_CONFIGURATION_ADDEVENT(flying_mode, &pos, NULL);
1788           printf("Setting flight mode to %i.\n",pos);
1789   }
1790 }
1791
1792 static void vision_select( GtkComboBox *combo,
1793                            gpointer    data )
1794 {
1795
1796   int32_t pos;
1797   pos = (int32_t)gtk_combo_box_get_active( combo );
1798   printf( "Enter in vision_select with value : %d \n", pos );
1799
1800   //ardrone_at_cad( pos, 0);
1801   ARDRONE_TOOL_CONFIGURATION_ADDEVENT(detect_type, &pos, NULL);
1802
1803   // Enable or disable Cocarde control button depending on choice
1804   if ( (pos == CAD_TYPE_COCARDE) || (pos == CAD_TYPE_ORIENTED_COCARDE) )
1805     gtk_widget_set_sensitive((GtkWidget*)GTK_TOGGLE_BUTTON(button_roundel), TRUE);
1806   else
1807     gtk_widget_set_sensitive((GtkWidget*)GTK_TOGGLE_BUTTON(button_roundel), FALSE);
1808
1809 }
1810
1811
1812 /*Stephane
1813 Callback function for the "Fly with Shell ?" combo box */
1814 static void shell_select( GtkComboBox *combo,
1815                            gpointer    data )
1816 {
1817
1818   gint pos;
1819   int32_t flag;
1820   pos = gtk_combo_box_get_active( combo );
1821   DEBUG_PRINT_SDK( "Enter in shell_select with value : %d \n", pos );
1822
1823   flag = (pos == 0) ? TRUE : FALSE;
1824   ARDRONE_TOOL_CONFIGURATION_ADDEVENT(enemy_without_shell, &flag, NULL);
1825 }
1826
1827 static void anim_select( GtkComboBox *combo,
1828                          gpointer    data )
1829 {
1830   gint pos;
1831   pos = gtk_combo_box_get_active( combo );
1832   DEBUG_PRINT_SDK( "Enter in anim_select with value : %d \n", pos );
1833   char buffer[128];
1834   int res;
1835
1836   if (!(pos < 0 || pos >= ARDRONE_NB_ANIM_MAYDAY))
1837   {
1838           /* Without acknowledgement */
1839           //ardrone_at_set_anim( pos, MAYDAY_TIMEOUT[pos]);
1840
1841           /* With acknowledgement */
1842           res = snprintf(buffer,sizeof(buffer),"%i,%i",pos, MAYDAY_TIMEOUT[pos]);
1843           printf("Sending flight animation : <%s>\n",buffer);
1844           ARDRONE_TOOL_CONFIGURATION_ADDEVENT(flight_anim, buffer, NULL);
1845   }
1846   else
1847   {
1848           printf("[%s:%s:%i] Invalid index : %i.\n",__FILE__,__FUNCTION__,__LINE__,pos);
1849   }
1850 }
1851
1852
1853 static void led_select( GtkComboBox *combo,
1854                          gpointer    data )
1855 {
1856
1857   gint pos;
1858   private_data_led *pLed;
1859   pLed = (private_data_led*)data;
1860   pos = gtk_combo_box_get_active( combo );
1861   if(pLed != NULL )
1862   {
1863      DEBUG_PRINT_SDK( "Enter in led_select with pos=%d, freq=%f, duration=%d\n", pos,
1864                                 (float)gtk_spin_button_get_value( GTK_SPIN_BUTTON(pLed->freq) ),
1865                                 gtk_spin_button_get_value_as_int( GTK_SPIN_BUTTON(pLed->duration) ));
1866      char command[512] = {0};
1867      float freq = (float)gtk_spin_button_get_value( GTK_SPIN_BUTTON(pLed->freq) );
1868      snprintf (command, 512, "%d,%d,%d",
1869                pos,
1870                *(int *)&freq,
1871                gtk_spin_button_get_value_as_int (GTK_SPIN_BUTTON(pLed->duration)));
1872      ARDRONE_TOOL_CONFIGURATION_ADDEVENT (leds_anim, command, NULL);
1873      /*ardrone_at_set_led_animation( pos,
1874                                 (float)gtk_spin_button_get_value( GTK_SPIN_BUTTON(pLed->freq) ),
1875                                 gtk_spin_button_get_value_as_int( GTK_SPIN_BUTTON(pLed->duration) ));*/
1876   }
1877 }
1878
1879 static void ihm_toggle_navdata_demo( GtkComboBox *combo, gpointer data )
1880 {
1881         static int32_t toggled_navdata_demo = 0;
1882         toggled_navdata_demo ^= 1;
1883         ARDRONE_TOOL_CONFIGURATION_ADDEVENT(navdata_demo, &toggled_navdata_demo, NULL);
1884 }
1885
1886 static void ihm_toggle_cocarde_control( GtkComboBox *combo, gpointer data )
1887 {
1888     static int32_t toggled_cocarde_control = 0;
1889     int32_t value;
1890     toggled_cocarde_control ^= 1;
1891     value = (toggled_cocarde_control ? FLYING_MODE_HOVER_ON_TOP_OF_ROUNDEL : FLYING_MODE_FREE_FLIGHT);
1892     ARDRONE_TOOL_CONFIGURATION_ADDEVENT(flying_mode, &value, NULL);
1893 }
1894
1895 void ihm_init()
1896 {
1897   int k;
1898   int ii;
1899
1900   ihm_val_idx = 0;
1901
1902   /* Main window */
1903   /* ----------- */
1904   GtkWidget *MainWin, *vBox;
1905   MainWin = gtk_window_new(GTK_WINDOW_TOPLEVEL);
1906   //  gtk_window_set_position(GTK_WINDOW(MainWin), GTK_WIN_POS_CENTER_ALWAYS);
1907   //  gtk_window_set_default_size(GTK_WINDOW(MainWin), KIHM_WX, KIHM_WY);
1908   gtk_container_set_border_width(GTK_CONTAINER(MainWin), KIHM_BORDER);
1909   gtk_signal_connect( GTK_OBJECT(MainWin),
1910                       "destroy",
1911                       G_CALLBACK(ihm_main_quit),
1912                       NULL );
1913   gtk_window_set_title(GTK_WINDOW(MainWin), "Drone attitude viewer");
1914   gtk_widget_realize(MainWin);
1915   vBox = gtk_vbox_new(FALSE, 0);
1916   printf("   ihm_init : main window\n");
1917
1918   // graphic contexts creation (MainWin must be realized)
1919   ihm_createGCs(MainWin);
1920
1921   /* Control States interface */
1922   /* ------------------------ */
1923
1924   // gtk_box_pack_start(GTK_BOX(vBox), hBox_CtrlState, FALSE, TRUE,  0);
1925
1926   GtkWidget* frame_ctrl_mykonos = gtk_frame_new("Mykonos states");
1927
1928   GtkWidget* hBox_CtrlState = gtk_hbox_new(FALSE, 20);
1929   gtk_container_add( GTK_CONTAINER(frame_ctrl_mykonos), hBox_CtrlState );
1930
1931   GtkWidget* label_mykonos_states[NB_ARDRONE_STATES];
1932   label_mykonos_states[0] = gtk_label_new("Mykonos Global State: ");
1933   label_mykonos_states[1] = gtk_label_new("Mykonos Control State: ");
1934   label_mykonos_states[2] = gtk_label_new("Mykonos Vision State: ");
1935
1936   vp_os_memset(label_mykonos_state_value, 0, sizeof(label_mykonos_state_value));
1937   vp_os_memset(label_ctrl_state_value, 0, sizeof(label_ctrl_state_value));
1938   vp_os_memset(label_vision_state_value, 0, sizeof(label_vision_state_value));
1939
1940   strcat(label_mykonos_state_value, "Not Connected");
1941   strcat(label_ctrl_state_value, "Not Connected");
1942   strcat(label_vision_state_value, "Not Connected");
1943
1944   label_mykonos_values[0] = gtk_label_new(label_mykonos_state_value);
1945   label_mykonos_values[1] = gtk_label_new(label_ctrl_state_value);
1946   label_mykonos_values[2] = gtk_label_new(label_vision_state_value);
1947
1948   // Start button indicating flight or land state
1949   label_start_button_state = gtk_label_new("Start button: ");
1950   darea_start_button_state = gtk_drawing_area_new();
1951   gtk_drawing_area_size((GtkDrawingArea*)darea_start_button_state, START_BUTTON_DA_SIZE, START_BUTTON_DA_SIZE );
1952
1953 #ifdef USE_ARDRONE_VICON
1954   label_vicon_button_state = gtk_label_new("Vicon started: ");
1955   darea_vicon_button_state = gtk_drawing_area_new();
1956   gtk_drawing_area_size((GtkDrawingArea*)darea_vicon_button_state, VICON_BUTTON_DA_SIZE, VICON_BUTTON_DA_SIZE );
1957 #endif
1958
1959   gtk_box_pack_start(GTK_BOX(hBox_CtrlState), label_start_button_state, FALSE, FALSE, 0);
1960   gtk_box_pack_start(GTK_BOX(hBox_CtrlState), darea_start_button_state, FALSE, FALSE, 0);
1961
1962 #ifdef USE_ARDRONE_VICON
1963   gtk_box_pack_start(GTK_BOX(hBox_CtrlState), label_vicon_button_state, FALSE, FALSE, 0);
1964   gtk_box_pack_start(GTK_BOX(hBox_CtrlState), darea_vicon_button_state, FALSE, FALSE, 0);
1965 #endif
1966
1967   gtk_box_pack_start(GTK_BOX(hBox_CtrlState), label_mykonos_states[0], FALSE, FALSE, 0);
1968   gtk_box_pack_start(GTK_BOX(hBox_CtrlState), label_mykonos_values[0], FALSE, FALSE, 0);
1969   gtk_box_pack_start(GTK_BOX(hBox_CtrlState), label_mykonos_states[1], FALSE, FALSE, 0);
1970   gtk_box_pack_start(GTK_BOX(hBox_CtrlState), label_mykonos_values[1], FALSE, FALSE, 0);
1971   gtk_box_pack_start(GTK_BOX(hBox_CtrlState), label_mykonos_states[2], FALSE, FALSE, 0);
1972   gtk_box_pack_start(GTK_BOX(hBox_CtrlState), label_mykonos_values[2], FALSE, FALSE, 0);
1973
1974   /* Main control interface */
1975   /* ---------------------- */
1976   GtkWidget* frame_AngularRateCtrlGain;
1977   GtkWidget* frame_AttitudeCtrlGain;
1978   GtkWidget* frame_MiscellaneousVar;
1979   GtkWidget* frame_AltitudeCtrlGain;
1980   GtkWidget* frame_FixPointCtrlGain;
1981   GtkWidget* frame_CocardeCtrlGain;
1982   GtkWidget* combo_flying_mode;
1983   GtkWidget* button_SendCtrlGains, *button_SendFlatTrims, *button_SetConfigParams;
1984   GtkWidget* label_AngularRateCtrlGain[NB_GAINS_W];
1985   GtkWidget* label_AttitudeCtrlGain[NB_GAINS_EA];
1986   GtkWidget* label_MiscellaneousVar[NB_MISC_VARS];
1987   GtkWidget* label_AltitudeCtrlGain[NB_GAINS_ALT];
1988   GtkWidget* label_FixPointCtrlGain[NB_GAINS_FP];
1989   GtkWidget* label_CocardeCtrlGain[NB_GAINS_ROUNDEL];
1990   GtkWidget* entry_AngularRateCtrlGain[NB_GAINS_W];
1991   GtkWidget* entry_AttitudeCtrlGain[NB_GAINS_EA];
1992   GtkWidget* entry_MiscellaneousVar[NB_MISC_VARS];
1993   GtkWidget* entry_AltitudeCtrlGain[NB_GAINS_ALT];
1994   GtkWidget* entry_FixPointCtrlGain[NB_GAINS_FP];
1995   GtkWidget* entry_CocardeCtrlGain[NB_GAINS_ROUNDEL];
1996   GtkWidget* hBox_CtrlGain;
1997   GtkWidget* hBox_AltCtrlGain;
1998   GtkWidget* hBox_AngularRateCtrlGain;
1999   GtkWidget* hBox_AttitudeCtrlGain;
2000   GtkWidget* hBox_AltitudeCtrlGain;
2001   GtkWidget* hBox_FixPointCtrlGain;
2002   GtkWidget* hBox_CocardeCtrlGain;
2003   GtkWidget* hBox_MiscellaneousVar;
2004   GtkWidget* ctrl_gain_h_separator;
2005
2006    // Frames
2007   frame_AngularRateCtrlGain = gtk_frame_new("Angular rate control gain");
2008   frame_AttitudeCtrlGain    = gtk_frame_new("Attitude control gains");
2009   frame_AltitudeCtrlGain    = gtk_frame_new("Altitude control gains");
2010   frame_FixPointCtrlGain    = gtk_frame_new("Fixed Point control gains");
2011   frame_CocardeCtrlGain     = gtk_frame_new("Cocarde control gains");
2012   frame_MiscellaneousVar    = gtk_frame_new("MiscVars");
2013   printf("   ihm_init : Frames Control gains interface\n");
2014
2015   // Labels
2016   label_AngularRateCtrlGain[0] = gtk_label_new("PQ_Kp");
2017   label_AngularRateCtrlGain[1] = gtk_label_new("  R_Kp");
2018   label_AngularRateCtrlGain[2] = gtk_label_new("  R_Ki");
2019
2020   label_AttitudeCtrlGain[0] = gtk_label_new("  EA_Kp");
2021   label_AttitudeCtrlGain[1] = gtk_label_new("  EA_Ki");
2022
2023   label_AltitudeCtrlGain[0] = gtk_label_new(" A_Kp");
2024   label_AltitudeCtrlGain[1] = gtk_label_new(" A_Ki");
2025   label_AltitudeCtrlGain[2] = gtk_label_new(" Vz_Kp");
2026   label_AltitudeCtrlGain[3] = gtk_label_new(" VZ_Ki");
2027
2028   label_FixPointCtrlGain[0] = gtk_label_new(" H_Kp");
2029   label_FixPointCtrlGain[1] = gtk_label_new(" H_Ki");
2030
2031   label_CocardeCtrlGain[0]  = gtk_label_new(" B_Kp");
2032   label_CocardeCtrlGain[1]  = gtk_label_new(" B_Ki");
2033
2034   label_MiscellaneousVar[0] = gtk_label_new("MV0");
2035   label_MiscellaneousVar[1] = gtk_label_new("MV1");
2036   label_MiscellaneousVar[2] = gtk_label_new("MV2");
2037   label_MiscellaneousVar[3] = gtk_label_new("MV3");
2038
2039   // Fill entries with default values
2040   if (ardrone_control_config.flight_without_shell)
2041   {// Shelless
2042     printf("   ihm_init : Labels Control gains interface (Shelless)\n");
2043     tab_g[0] = CTRL_DEFAULT_NUM_PQ_KP_NO_SHELL;
2044     tab_g[1] = CTRL_DEFAULT_NUM_R_KP;
2045     tab_g[2] = CTRL_DEFAULT_NUM_R_KI;
2046     tab_g[3] = CTRL_DEFAULT_NUM_EA_KP_NO_SHELL;
2047     tab_g[4] = CTRL_DEFAULT_NUM_EA_KI_NO_SHELL;
2048
2049     tab_ag[KIHM_AKP] = CTRL_DEFAULT_NUM_ALT_KP;
2050     tab_ag[KIHM_AKI] = CTRL_DEFAULT_NUM_ALT_KI;
2051     tab_ag[KIHM_AKD] = CTRL_DEFAULT_NUM_VZ_KP;
2052     tab_ag[KIHM_ATD] = CTRL_DEFAULT_NUM_VZ_KI;
2053
2054     tab_fp_g[KIHM_FPKP] = CTRL_DEFAULT_NUM_HOVER_KP_NO_SHELL;
2055     tab_fp_g[KIHM_FPKI] = CTRL_DEFAULT_NUM_HOVER_KI_NO_SHELL;
2056
2057     tab_roundel_g[KIHM_ROUNDELKP] = CTRL_DEFAULT_NUM_HOVER_B_KP_NO_SHELL;
2058     tab_roundel_g[KIHM_ROUNDELKI] = CTRL_DEFAULT_NUM_HOVER_B_KI_NO_SHELL;
2059   }
2060   else
2061   {// With shell
2062     printf("   ihm_init : Labels Control gains interface (with Shell)\n");
2063     tab_g[0] = CTRL_DEFAULT_NUM_PQ_KP_SHELL;
2064     tab_g[1] = CTRL_DEFAULT_NUM_R_KP;
2065     tab_g[2] = CTRL_DEFAULT_NUM_R_KI;
2066     tab_g[3] = CTRL_DEFAULT_NUM_EA_KP_SHELL;
2067     tab_g[4] = CTRL_DEFAULT_NUM_EA_KI_SHELL;
2068
2069     tab_ag[KIHM_AKP] = CTRL_DEFAULT_NUM_ALT_KP;
2070     tab_ag[KIHM_AKI] = CTRL_DEFAULT_NUM_ALT_KI;
2071     tab_ag[KIHM_AKD] = CTRL_DEFAULT_NUM_VZ_KP;
2072     tab_ag[KIHM_ATD] = CTRL_DEFAULT_NUM_VZ_KI;
2073
2074     tab_fp_g[KIHM_FPKP] = CTRL_DEFAULT_NUM_HOVER_KP_SHELL;
2075     tab_fp_g[KIHM_FPKI] = CTRL_DEFAULT_NUM_HOVER_KI_SHELL;
2076
2077     tab_roundel_g[KIHM_ROUNDELKP] = CTRL_DEFAULT_NUM_HOVER_B_KP_SHELL;
2078     tab_roundel_g[KIHM_ROUNDELKI] = CTRL_DEFAULT_NUM_HOVER_B_KI_SHELL;
2079   }
2080
2081   char label_gain[10];
2082
2083   // Entries
2084   for (ii=0; ii<NB_GAINS_W; ii++) {
2085     entry_AngularRateCtrlGain[ii] = gtk_entry_new();
2086     gtk_widget_set_size_request(entry_AngularRateCtrlGain[ii], 60, 20);
2087     sprintf(label_gain, "%d", tab_g[ii]);
2088     gtk_entry_set_text( GTK_ENTRY(entry_AngularRateCtrlGain[ii]), label_gain);
2089   }
2090   for (ii=0; ii<NB_GAINS_EA; ii++) {
2091     entry_AttitudeCtrlGain[ii] = gtk_entry_new();
2092     gtk_widget_set_size_request(entry_AttitudeCtrlGain[ii], 60, 20);
2093     sprintf(label_gain, "%d", tab_g[NB_GAINS_W+ii]);
2094     gtk_entry_set_text( GTK_ENTRY(entry_AttitudeCtrlGain[ii]), label_gain);
2095   }
2096   for (ii=0; ii<NB_GAINS_ALT; ii++) {
2097     entry_AltitudeCtrlGain[ii] = gtk_entry_new();
2098     gtk_widget_set_size_request(entry_AltitudeCtrlGain[ii], 60, 20);
2099     sprintf(label_gain, "%d", tab_ag[ii]);
2100     gtk_entry_set_text( GTK_ENTRY(entry_AltitudeCtrlGain[ii]), label_gain);
2101   }
2102   for (ii=0; ii<NB_GAINS_FP; ii++) {
2103     entry_FixPointCtrlGain[ii] = gtk_entry_new();
2104     gtk_widget_set_size_request(entry_FixPointCtrlGain[ii], 60, 20);
2105     sprintf(label_gain, "%d", tab_fp_g[ii]);
2106     gtk_entry_set_text( GTK_ENTRY(entry_FixPointCtrlGain[ii]), label_gain);
2107   }
2108   for (ii=0; ii<NB_GAINS_ROUNDEL; ii++) {
2109     entry_CocardeCtrlGain[ii] = gtk_entry_new();
2110     gtk_widget_set_size_request(entry_CocardeCtrlGain[ii], 60, 20);
2111     sprintf(label_gain, "%d", tab_roundel_g[ii]);
2112     gtk_entry_set_text( GTK_ENTRY(entry_CocardeCtrlGain[ii]), label_gain);
2113   }
2114   for (ii=0; ii<NB_MISC_VARS; ii++) {
2115     entry_MiscellaneousVar[ii] = gtk_entry_new();
2116     gtk_widget_set_size_request(entry_MiscellaneousVar[ii], 60, 20);
2117     sprintf(label_gain, "%d", MiscVar[ii] );
2118     gtk_entry_set_text( GTK_ENTRY(entry_MiscellaneousVar[ii]), label_gain);
2119   }
2120
2121   //  send_gains(pcfg); // send initial gain values
2122   for (ii=0; ii<NB_GAINS_W; ii++) {
2123     entry_PID[ii] = entry_AngularRateCtrlGain[ii];
2124   }
2125   for (ii=0; ii<NB_GAINS_EA; ii++) {
2126     entry_PID[NB_GAINS_W+ii] = entry_AttitudeCtrlGain[ii];
2127   }
2128   for (ii=0; ii<NB_GAINS_ALT; ii++) {
2129     entry_PID[NB_GAINS+ii] = entry_AltitudeCtrlGain[ii];
2130   }
2131   for (ii=0; ii<NB_GAINS_FP; ii++) {
2132     entry_PID[NB_GAINS+NB_GAINS_ALT+ii] = entry_FixPointCtrlGain[ii];
2133   }
2134   for (ii=0; ii<NB_GAINS_ROUNDEL; ii++) {
2135     entry_PID[NB_GAINS+NB_GAINS_ALT+NB_GAINS_FP+ii] = entry_CocardeCtrlGain[ii];
2136   }
2137
2138   for (ii=0; ii<NB_MISC_VARS; ii++) {
2139     entry_MiscVar[ii] = entry_MiscellaneousVar[ii];
2140   }
2141
2142   printf("   ihm_init : Entries Control gains interface\n");
2143
2144   // Boxes
2145   hBox_CtrlGain            = gtk_hbox_new(FALSE, 20);
2146   hBox_AltCtrlGain         = gtk_hbox_new(FALSE, 20);
2147   hBox_AngularRateCtrlGain = gtk_hbox_new(FALSE, 10);
2148   hBox_AttitudeCtrlGain    = gtk_hbox_new(FALSE, 10);
2149   hBox_AltitudeCtrlGain    = gtk_hbox_new(FALSE, 10);
2150   hBox_FixPointCtrlGain    = gtk_hbox_new(FALSE, 10);
2151   hBox_CocardeCtrlGain     = gtk_hbox_new(FALSE, 10);
2152   hBox_MiscellaneousVar    = gtk_hbox_new(FALSE, 20);
2153   printf("   ihm_init : Boxes Control gains interface\n");
2154
2155   // Separators
2156   ctrl_gain_h_separator = gtk_hseparator_new();
2157
2158   // Combo to choose flying mode
2159   combo_flying_mode = gtk_combo_box_new_text();
2160   g_signal_connect( G_OBJECT( combo_flying_mode ), "changed",
2161                     G_CALLBACK( flying_mode_update ), NULL );
2162   gtk_combo_box_insert_text( (GtkComboBox*)combo_flying_mode, 0, (const gchar*)"Normal");
2163   gtk_combo_box_insert_text( (GtkComboBox*)combo_flying_mode, 1, (const gchar*)"Combined yaw");
2164
2165   // Buttons
2166   button_SendCtrlGains = gtk_button_new_with_label("Send control gains");
2167   GList* children_list = gtk_container_get_children(GTK_CONTAINER(button_SendCtrlGains));
2168   gtk_label_set_justify(GTK_LABEL(children_list->data), GTK_JUSTIFY_CENTER);
2169   /* Connect the "clicked" signal of the button to our callback */
2170   g_signal_connect(G_OBJECT(button_SendCtrlGains), "clicked", G_CALLBACK(ihm_sendGains), (gpointer)NULL);
2171
2172   printf("   ihm_init : Button Control gains interface\n");
2173
2174   button_SetConfigParams = gtk_button_new_with_label("Set configuration params");
2175   g_signal_connect(G_OBJECT(button_SetConfigParams), "clicked", G_CALLBACK(ihm_setConfigParamsWindow), (gpointer)NULL);
2176
2177   // Widget hierarchy construction
2178   for (ii=0; ii<NB_GAINS_W; ii++) {
2179     gtk_box_pack_start(GTK_BOX(hBox_AngularRateCtrlGain), label_AngularRateCtrlGain[ii], FALSE , FALSE, 0);
2180     gtk_box_pack_start(GTK_BOX(hBox_AngularRateCtrlGain), entry_AngularRateCtrlGain[ii], FALSE , FALSE, 0);
2181   }
2182   gtk_container_add(GTK_CONTAINER(frame_AngularRateCtrlGain), hBox_AngularRateCtrlGain);
2183
2184   for (ii=0; ii<NB_MISC_VARS; ii++) {
2185     gtk_box_pack_start(GTK_BOX(hBox_MiscellaneousVar), label_MiscellaneousVar[ii], FALSE , FALSE, 0);
2186     gtk_box_pack_start(GTK_BOX(hBox_MiscellaneousVar), entry_MiscellaneousVar[ii], FALSE , FALSE, 0);
2187   }
2188 //#ifdef DEBUG
2189   gtk_container_add(GTK_CONTAINER(frame_MiscellaneousVar), hBox_MiscellaneousVar);
2190 //#endif
2191
2192   for (ii=0; ii<NB_GAINS_ALT; ii++) {
2193     gtk_box_pack_start(GTK_BOX(hBox_AltitudeCtrlGain), label_AltitudeCtrlGain[ii], FALSE , FALSE, 0);
2194     gtk_box_pack_start(GTK_BOX(hBox_AltitudeCtrlGain),  entry_AltitudeCtrlGain[ii], FALSE , FALSE, 0);
2195   }
2196   gtk_container_add(GTK_CONTAINER(frame_AltitudeCtrlGain), hBox_AltitudeCtrlGain);
2197
2198   for (ii=0; ii<NB_GAINS_FP; ii++) {
2199     gtk_box_pack_start(GTK_BOX(hBox_FixPointCtrlGain), label_FixPointCtrlGain[ii], FALSE , FALSE, 0);
2200     gtk_box_pack_start(GTK_BOX(hBox_FixPointCtrlGain),  entry_FixPointCtrlGain[ii], FALSE , FALSE, 0);
2201   }
2202   gtk_container_add(GTK_CONTAINER(frame_FixPointCtrlGain), hBox_FixPointCtrlGain);
2203
2204   for (ii=0; ii<NB_GAINS_ROUNDEL; ii++) {
2205     gtk_box_pack_start(GTK_BOX(hBox_CocardeCtrlGain), label_CocardeCtrlGain[ii], FALSE , FALSE, 0);
2206     gtk_box_pack_start(GTK_BOX(hBox_CocardeCtrlGain),  entry_CocardeCtrlGain[ii], FALSE , FALSE, 0);
2207   }
2208   gtk_container_add(GTK_CONTAINER(frame_CocardeCtrlGain), hBox_CocardeCtrlGain);
2209
2210   for (ii=0; ii<NB_GAINS_EA; ii++) {
2211     gtk_box_pack_start(GTK_BOX(hBox_AttitudeCtrlGain), label_AttitudeCtrlGain[ii], FALSE , FALSE, 0);
2212     gtk_box_pack_start(GTK_BOX(hBox_AttitudeCtrlGain),  entry_AttitudeCtrlGain[ii], FALSE , FALSE, 0);
2213   }
2214   gtk_container_add(GTK_CONTAINER(frame_AttitudeCtrlGain), hBox_AttitudeCtrlGain);
2215
2216   gtk_box_pack_start(GTK_BOX(hBox_CtrlGain), frame_AngularRateCtrlGain, FALSE , FALSE, 0);
2217   gtk_box_pack_start(GTK_BOX(hBox_CtrlGain), frame_AttitudeCtrlGain   , FALSE , FALSE, 0);
2218   gtk_box_pack_start(GTK_BOX(hBox_CtrlGain), frame_FixPointCtrlGain   , FALSE , FALSE, 0);
2219   gtk_box_pack_start(GTK_BOX(hBox_CtrlGain), frame_CocardeCtrlGain    , FALSE , FALSE, 0);
2220 //#ifdef DEBUG
2221   gtk_box_pack_start(GTK_BOX(hBox_AltCtrlGain), frame_MiscellaneousVar , FALSE , FALSE, 0);
2222 //#endif
2223   gtk_box_pack_start(GTK_BOX(hBox_AltCtrlGain), frame_AltitudeCtrlGain , FALSE , FALSE, 0);
2224   gtk_box_pack_start(GTK_BOX(hBox_AltCtrlGain), combo_flying_mode      , FALSE , FALSE, 0);
2225   gtk_box_pack_start(GTK_BOX(hBox_AltCtrlGain), button_SendCtrlGains   , FALSE , FALSE, 0);
2226   gtk_box_pack_start(GTK_BOX(hBox_AltCtrlGain), button_SetConfigParams , FALSE , FALSE, 0);
2227   printf("   ihm_init : Control gains interface\n");
2228
2229   /* Attitude display */
2230   /* ---------------- */
2231   GtkWidget *frame_Attitude, *hBox_Attitude;
2232   frame_Attitude = gtk_frame_new("Attitude (Euler angles)");
2233   hBox_Attitude  = gtk_hbox_new(FALSE, 0);
2234   ihm_DA_att[KIHM_DA_ATT_PITCH] = gtk_drawing_area_new();
2235   gtk_drawing_area_size((GtkDrawingArea*)ihm_DA_att[KIHM_DA_ATT_PITCH], KIHM_DAREA_ANGLE_X_SIZE, KIHM_DAREA_ANGLE_Y_SIZE);
2236   ihm_DA_att[KIHM_DA_ATT_ROLL ] = gtk_drawing_area_new();
2237   gtk_drawing_area_size((GtkDrawingArea*)ihm_DA_att[KIHM_DA_ATT_ROLL ], KIHM_DAREA_ANGLE_X_SIZE, KIHM_DAREA_ANGLE_Y_SIZE);
2238   ihm_DA_att[KIHM_DA_ATT_YAW  ] = gtk_drawing_area_new();
2239   gtk_drawing_area_size((GtkDrawingArea*)ihm_DA_att[KIHM_DA_ATT_YAW  ], KIHM_DAREA_ANGLE_X_SIZE, KIHM_DAREA_ANGLE_Y_SIZE);
2240   ihm_DA_att[KIHM_DA_ATT_DIR  ] = gtk_drawing_area_new();
2241   gtk_drawing_area_size((GtkDrawingArea*)ihm_DA_att[KIHM_DA_ATT_DIR  ], KIHM_DAREA_ANGLE_X_SIZE, KIHM_DAREA_ANGLE_Y_SIZE);
2242
2243   // Widget hierarchy construction
2244   gtk_box_pack_start(GTK_BOX(hBox_Attitude), ihm_DA_att[KIHM_DA_ATT_PITCH], TRUE, FALSE, 0);
2245   gtk_box_pack_start(GTK_BOX(hBox_Attitude), ihm_DA_att[KIHM_DA_ATT_ROLL ], TRUE, FALSE, 0);
2246   gtk_box_pack_start(GTK_BOX(hBox_Attitude), ihm_DA_att[KIHM_DA_ATT_YAW  ], TRUE, FALSE, 0);
2247   gtk_box_pack_start(GTK_BOX(hBox_Attitude), ihm_DA_att[KIHM_DA_ATT_DIR  ], TRUE, FALSE, 0);
2248   gtk_container_add(GTK_CONTAINER(frame_Attitude), hBox_Attitude);
2249   printf("   ihm_init : Attitude display\n");
2250
2251   // PangoLayouts
2252   ihm_PL_DAtheta = pango_layout_new(gtk_widget_get_pango_context(ihm_DA_att[KIHM_DA_ATT_PITCH]));
2253   ihm_PL_DAphi   = pango_layout_new(gtk_widget_get_pango_context(ihm_DA_att[KIHM_DA_ATT_ROLL ]));
2254   ihm_PL_DApsi   = pango_layout_new(gtk_widget_get_pango_context(ihm_DA_att[KIHM_DA_ATT_YAW  ]));
2255   ihm_PL_DAdir   = pango_layout_new(gtk_widget_get_pango_context(ihm_DA_att[KIHM_DA_ATT_DIR  ]));
2256
2257   /* RadioCommand references display */
2258   /* ------------------------------- */
2259   GtkWidget *frame_RCref;
2260   GtkWidget * box_RCref;
2261
2262   frame_RCref = gtk_frame_new("Euler angles references");
2263   box_RCref = gtk_hbox_new(FALSE, 0);
2264
2265   ihm_label_RCref = gtk_label_new("");
2266   PangoFontDescription* pPFD_RCref = pango_font_description_from_string("terminal 12"); // select a standard fixed font
2267   gtk_widget_modify_font(ihm_label_RCref, pPFD_RCref);
2268   gtk_box_pack_start(GTK_BOX(box_RCref),ihm_label_RCref, FALSE , FALSE, 0);
2269
2270   button_SendFlatTrims = gtk_button_new_with_label("Flat trim");
2271   g_signal_connect(G_OBJECT(button_SendFlatTrims), "clicked", G_CALLBACK(ihm_sendFlatTrim), (gpointer)NULL);
2272   gtk_box_pack_start(GTK_BOX(box_RCref), button_SendFlatTrims     , FALSE , FALSE, 0);
2273
2274   gtk_container_add(GTK_CONTAINER(frame_RCref), box_RCref);
2275
2276   printf("   ihm_init : RadioCommand references display\n");
2277
2278   /* Elapsed time frame */
2279   GtkWidget *frame_elapsedT, *hBox_RCref_ETime;
2280   frame_elapsedT = gtk_frame_new("Time (min:sec)");
2281   hBox_RCref_ETime = gtk_hbox_new(FALSE, 0);
2282   label_elapsedT = gtk_label_new("");
2283   gtk_container_add(GTK_CONTAINER(frame_elapsedT), label_elapsedT);
2284   gtk_box_pack_start(GTK_BOX(hBox_RCref_ETime), frame_RCref   , TRUE, TRUE, 0);
2285   gtk_box_pack_start(GTK_BOX(hBox_RCref_ETime), frame_elapsedT, TRUE, TRUE, 0);
2286
2287   /* Curves creation and initialization */
2288   /* ---------------------------------- */
2289   for (k=0; k<KIHM_NB_CURVES; k++) {
2290     ihm_initCurves( ihm_CA[k].PrivateWin, k);
2291     windows_status[k] = WINDOW_OPENED;
2292   }
2293
2294   /* Curves control display */
2295   /* ---------------------- */
2296   GtkWidget *button_FreezeCurves, *hBox_CurveCtrl[3], *vBox_CurveCtrl ;
2297   GtkWidget *pframe_CurveCtrl;
2298
2299   /* Image display show button */
2300   /* ------------------------- */
2301   GtkWidget *label_to_fill_space;
2302   char show_image_ctrl_lbl[32];
2303
2304   char *ctrl_lbl[KIHM_NB_CURVES] = {"Accelerations        ",
2305                                     "Angular rates        ",
2306                                     "Theta                ",
2307                                     "Phi                  ",
2308                                     "Vbat                 ",
2309                                     "PWM                  ",
2310                                     "Altitude             ",
2311                                     "Vx                   ",
2312                                     "Vy                   ",
2313                                     "Vz                   ",
2314                                     "Current              "};
2315
2316   button_FreezeCurves = gtk_check_button_new_with_label("FREEZE CURVES");
2317   ihm_freeze_curves = FALSE;
2318   /* Connect the "clicked" signal of the button to our callback */
2319   g_signal_connect(G_OBJECT(button_FreezeCurves), "clicked", G_CALLBACK(ihm_freezeCurves), (gpointer)NULL);
2320
2321   strncpy( show_image_ctrl_lbl, "VISION image", sizeof(show_image_ctrl_lbl) );
2322   button_show_image  = gtk_check_button_new_with_label( show_image_ctrl_lbl );
2323   button_show_image2 = gtk_check_button_new_with_label( (const gchar*)"-- Show drone video --" );
2324   button_navdatademo = gtk_check_button_new_with_label( (const gchar*)"Use navdata demo" );
2325
2326   g_signal_connect(G_OBJECT(button_navdatademo), "clicked", G_CALLBACK(ihm_toggle_navdata_demo), (gpointer)NULL);
2327
2328   hBox_CurveCtrl[0] = gtk_hbox_new(FALSE, 0);
2329   hBox_CurveCtrl[1] = gtk_hbox_new(FALSE, 0);
2330   hBox_CurveCtrl[2] = gtk_hbox_new(FALSE, 0);
2331   vBox_CurveCtrl    = gtk_vbox_new(TRUE , 0);
2332   for (k=0; k<KIHM_NB_CURVES; k++) {
2333     strncpy(ihm_CA[k].ctrl_lbl, ctrl_lbl[k], sizeof(ihm_CA[k].ctrl_lbl));
2334     ihm_CA[k].ctrl_lbl[sizeof(ihm_CA[k].ctrl_lbl)-1] = 0;
2335
2336     button_ShowCurve[k] = gtk_check_button_new_with_label(ihm_CA[k].ctrl_lbl);
2337
2338     if (k<6)
2339       gtk_box_pack_start(GTK_BOX(hBox_CurveCtrl[0]), button_ShowCurve[k], FALSE , FALSE, 0);
2340     else
2341       gtk_box_pack_start(GTK_BOX(hBox_CurveCtrl[1]), button_ShowCurve[k], FALSE , FALSE, 0);
2342
2343     g_signal_connect(G_OBJECT(button_ShowCurve[k]), "clicked", G_CALLBACK(ihm_showCurves), (gpointer)k);
2344     gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(button_ShowCurve[k]), FALSE );
2345   }
2346
2347   label_to_fill_space = gtk_label_new("                                      ");
2348
2349   gtk_box_pack_start(GTK_BOX(vBox_CurveCtrl), hBox_CurveCtrl[0], FALSE , FALSE, 0);
2350   gtk_box_pack_start(GTK_BOX(vBox_CurveCtrl), hBox_CurveCtrl[1], FALSE , FALSE, 0);
2351   gtk_box_pack_start(GTK_BOX(vBox_CurveCtrl), hBox_CurveCtrl[2], FALSE , FALSE, 0);
2352
2353   pframe_CurveCtrl = gtk_frame_new("Curves control");
2354
2355   gtk_box_pack_start(GTK_BOX(hBox_CurveCtrl[1]), button_show_image  , FALSE , FALSE, 0);
2356   gtk_box_pack_start(GTK_BOX(hBox_CurveCtrl[1]), label_to_fill_space, FALSE , FALSE, 0); // To fill space
2357   gtk_box_pack_start(GTK_BOX(hBox_CurveCtrl[1]), button_FreezeCurves, FALSE , FALSE, 0);
2358   gtk_container_add(GTK_CONTAINER(pframe_CurveCtrl), vBox_CurveCtrl);
2359 #ifdef DEBUG
2360   /* Logs */
2361   /* ---- */
2362
2363   GtkWidget* frame_logs_mykonos = gtk_frame_new("Logs");
2364   GtkWidget* hBox_logs = gtk_hbox_new(FALSE, 20);
2365   gtk_container_add( GTK_CONTAINER(frame_logs_mykonos), hBox_logs );
2366 #endif
2367   GtkWidget* frame_cad = gtk_frame_new("Camera detect");
2368   GtkWidget* vBox_cad  = gtk_hbox_new(FALSE, 20);
2369   GtkWidget* hBox_cad  = gtk_hbox_new(FALSE, 20);
2370   GtkWidget* hBox_cad2 = gtk_hbox_new(FALSE, 20);
2371
2372   {
2373     //GtkCellRenderer *cell;
2374     GtkWidget* combo,*combo2;
2375     GtkWidget* label,*label2;
2376     private_data_led *ldata;
2377
2378     ldata = vp_os_malloc(sizeof(private_data_led));
2379
2380     // Detection type
2381     label = gtk_label_new( "Select detection:" );
2382     gtk_box_pack_start(GTK_BOX(hBox_cad), label, FALSE, FALSE, 0);
2383     combo = gtk_combo_box_new_text();
2384     g_signal_connect( G_OBJECT( combo ), "changed", G_CALLBACK( vision_select ), NULL );
2385
2386     gtk_combo_box_insert_text( (GtkComboBox*)combo, CAD_TYPE_NONE, (const gchar*)"None");
2387     gtk_combo_box_insert_text( (GtkComboBox*)combo, CAD_TYPE_HORIZONTAL, (const gchar*)"CAD_TYPE_HORIZONTAL");
2388     gtk_combo_box_insert_text( (GtkComboBox*)combo, CAD_TYPE_VERTICAL, (const gchar*)"CAD_TYPE_VERTICAL");
2389     gtk_combo_box_insert_text( (GtkComboBox*)combo, CAD_TYPE_VISION, (const gchar*)"2D tags");
2390     gtk_combo_box_insert_text( (GtkComboBox*)combo, CAD_TYPE_COCARDE, (const gchar*)"Cocarde");
2391     gtk_combo_box_insert_text( (GtkComboBox*)combo, CAD_TYPE_ORIENTED_COCARDE, (const gchar*)"Oriented Cocarde");
2392     gtk_combo_box_insert_text( (GtkComboBox*)combo, CAD_TYPE_STRIPE, (const gchar*)"Ground Stripe (15Hz)");
2393     gtk_combo_box_insert_text( (GtkComboBox*)combo, CAD_TYPE_H_COCARDE, (const gchar*)"Cocarde (front camera)");
2394     gtk_combo_box_insert_text( (GtkComboBox*)combo, CAD_TYPE_H_ORIENTED_COCARDE, (const gchar*)"Oriented Cocarde (front camera)");
2395     gtk_combo_box_insert_text( (GtkComboBox*)combo, CAD_TYPE_STRIPE_V, (const gchar*)"Ground Stripe (60Hz)");
2396     gtk_combo_box_insert_text( (GtkComboBox*)combo, CAD_TYPE_CAP, (const gchar*)"----");
2397     gtk_combo_box_insert_text( (GtkComboBox*)combo, CAD_TYPE_CAP, (const gchar*)" CAP ");
2398
2399
2400     gtk_box_pack_start(GTK_BOX(hBox_cad), combo, FALSE, FALSE, 0);
2401     gtk_widget_show( combo );
2402
2403     // Enemy color
2404
2405     label = gtk_label_new( "Change enemy color:" );
2406     gtk_box_pack_start(GTK_BOX(hBox_cad), label, FALSE, FALSE, 0);
2407
2408     combo = gtk_combo_box_new_text();
2409     g_signal_connect( G_OBJECT( combo ), "changed", G_CALLBACK( color_update ), NULL );
2410     //value start 0 => ARDRONE_DETECTION_COLOR_ORANGE_GREEN - 1
2411     gtk_combo_box_insert_text( (GtkComboBox*)combo, 0, (const gchar*)"green");
2412     gtk_combo_box_insert_text( (GtkComboBox*)combo, 1, (const gchar*)"yellow");
2413     gtk_combo_box_insert_text( (GtkComboBox*)combo, 2, (const gchar*)"blue");
2414
2415     gtk_box_pack_start(GTK_BOX(hBox_cad), combo, FALSE, FALSE, 0);
2416     gtk_widget_show( combo );
2417
2418     /* Stephane */
2419     /* Flight without shell option selection box */
2420     label2 = gtk_label_new( "Nb. tags" );
2421     gtk_box_pack_start(GTK_BOX(hBox_cad2), label2, FALSE, FALSE, 0);
2422     combo2 = gtk_combo_box_new_text();
2423     g_signal_connect( G_OBJECT( combo2 ), "changed", G_CALLBACK( shell_select ), NULL );
2424     gtk_combo_box_insert_text( (GtkComboBox*)combo2, 0, (const gchar*)"1");
2425     gtk_combo_box_insert_text( (GtkComboBox*)combo2, 1, (const gchar*)"2");
2426
2427     gtk_box_pack_start(GTK_BOX(hBox_cad2), combo2, FALSE, FALSE, 0);
2428     gtk_widget_show( combo2 );
2429
2430     /* Checkbox to enable/disable roundel control  */
2431     button_roundel = gtk_check_button_new_with_label( (const gchar*)"Cocarde control" );
2432     gtk_widget_set_sensitive((GtkWidget*)GTK_TOGGLE_BUTTON(button_roundel), FALSE);
2433     g_signal_connect(G_OBJECT(button_roundel), "clicked", G_CALLBACK(ihm_toggle_cocarde_control), (gpointer)NULL);
2434
2435     gtk_box_pack_start(GTK_BOX(hBox_cad2), button_roundel, FALSE, FALSE, 0);
2436
2437     /* Shows the number of detected tags */
2438     nbDetectedTags_label = gtk_label_new((gchar*)"Number of detected tags : uninitialised data.");
2439     activeDetection_label = gtk_label_new((gchar*)"Active detection : uninitialised data.");
2440     gtk_box_pack_start(GTK_BOX(hBox_cad2), activeDetection_label, FALSE, FALSE, 0);
2441     gtk_box_pack_start(GTK_BOX(hBox_cad2), nbDetectedTags_label, FALSE, FALSE, 0);
2442
2443     /* Shows the histoy of detected tags */
2444 #ifdef DEBUG
2445     detectionHistory_label = gtk_label_new((gchar*)"Detected tags in the last second : ?");
2446 #endif
2447
2448     /* Builds the whole frame */
2449
2450     gtk_box_pack_start(GTK_BOX(vBox_cad), hBox_cad, FALSE , FALSE, 0);
2451     gtk_box_pack_start(GTK_BOX(vBox_cad), hBox_cad2, FALSE , FALSE, 0);
2452
2453     gtk_container_add( GTK_CONTAINER(frame_cad), vBox_cad );
2454
2455   }
2456
2457   GtkWidget* frame_ledanim = gtk_frame_new("Play animations");
2458   GtkWidget* hBox_ledanim = gtk_hbox_new(FALSE, 20);
2459   gtk_container_add( GTK_CONTAINER(frame_ledanim), hBox_ledanim );
2460
2461   {
2462     GtkWidget* combo;
2463     GtkWidget* label;
2464
2465     private_data_led *ldata;
2466     ldata = vp_os_malloc(sizeof(private_data_led));
2467
2468     label = gtk_label_new( "Drone:" );
2469     gtk_box_pack_start(GTK_BOX(hBox_ledanim), label, FALSE, FALSE, 0);
2470     combo = gtk_combo_box_new_text();
2471     g_signal_connect( G_OBJECT( combo ), "changed", G_CALLBACK( anim_select ), NULL );
2472
2473     int cpt=0;
2474
2475 #define ADD_ANIMATION_COMBO(x) \
2476     gtk_combo_box_insert_text( (GtkComboBox*)combo, x, (const gchar*)#x); cpt++;
2477
2478     ADD_ANIMATION_COMBO(ARDRONE_ANIM_PHI_M30_DEG);
2479     ADD_ANIMATION_COMBO(ARDRONE_ANIM_PHI_30_DEG);
2480     ADD_ANIMATION_COMBO(ARDRONE_ANIM_THETA_M30_DEG);
2481     ADD_ANIMATION_COMBO(ARDRONE_ANIM_THETA_30_DEG);
2482     ADD_ANIMATION_COMBO(ARDRONE_ANIM_THETA_20DEG_YAW_200DEG);
2483     ADD_ANIMATION_COMBO(ARDRONE_ANIM_THETA_20DEG_YAW_M200DEG);
2484     ADD_ANIMATION_COMBO(ARDRONE_ANIM_TURNAROUND);
2485     ADD_ANIMATION_COMBO(ARDRONE_ANIM_TURNAROUND_GODOWN);
2486     ADD_ANIMATION_COMBO(ARDRONE_ANIM_YAW_SHAKE);
2487     ADD_ANIMATION_COMBO(ARDRONE_ANIM_YAW_DANCE);
2488     ADD_ANIMATION_COMBO(ARDRONE_ANIM_PHI_DANCE);
2489     ADD_ANIMATION_COMBO(ARDRONE_ANIM_THETA_DANCE);
2490     ADD_ANIMATION_COMBO(ARDRONE_ANIM_VZ_DANCE);
2491     ADD_ANIMATION_COMBO(ARDRONE_ANIM_WAVE);
2492     ADD_ANIMATION_COMBO(ARDRONE_ANIM_PHI_THETA_MIXED);
2493     ADD_ANIMATION_COMBO(ARDRONE_ANIM_DOUBLE_PHI_THETA_MIXED);
2494
2495     if (cpt!=ARDRONE_NB_ANIM_MAYDAY)
2496       printf("All animations are not present !!\n");
2497
2498     gtk_box_pack_start(GTK_BOX(hBox_ledanim), combo, FALSE, FALSE, 0);
2499     gtk_widget_show( combo );
2500
2501     label = gtk_label_new( "leds:" );
2502     gtk_box_pack_start(GTK_BOX(hBox_ledanim), label, FALSE, FALSE, 0);
2503     combo = gtk_combo_box_new_text();
2504     g_signal_connect( G_OBJECT( combo ), "changed", G_CALLBACK( led_select ), ldata );
2505     gtk_combo_box_insert_text( (GtkComboBox*)combo, 0, (const gchar*)"BLINK_GREEN_RED");
2506     gtk_combo_box_insert_text( (GtkComboBox*)combo, 1, (const gchar*)"BLINK_GREEN");
2507     gtk_combo_box_insert_text( (GtkComboBox*)combo, 2, (const gchar*)"BLINK_RED");
2508     gtk_combo_box_insert_text( (GtkComboBox*)combo, 3, (const gchar*)"SNAKE_GREEN_RED");
2509     gtk_combo_box_insert_text( (GtkComboBox*)combo, 4, (const gchar*)"FIRE");
2510     gtk_combo_box_insert_text( (GtkComboBox*)combo, 5, (const gchar*)"STANDARD");
2511     gtk_combo_box_insert_text( (GtkComboBox*)combo, 6, (const gchar*)"RED");
2512     gtk_combo_box_insert_text( (GtkComboBox*)combo, 7, (const gchar*)"GREEN");
2513     gtk_combo_box_insert_text( (GtkComboBox*)combo, 8, (const gchar*)"RED_SNAKE");
2514
2515     gtk_box_pack_start(GTK_BOX(hBox_ledanim), combo, FALSE, FALSE, 0);
2516     gtk_widget_show( combo );
2517
2518     label = gtk_label_new( "Duration:" );
2519     gtk_box_pack_start(GTK_BOX(hBox_ledanim), label, FALSE, FALSE, 0);
2520     ldata->duration = gtk_spin_button_new_with_range( 0, 10, 1);
2521     gtk_box_pack_start(GTK_BOX(hBox_ledanim), ldata->duration, FALSE, FALSE, 0);
2522     gtk_widget_show( ldata->duration );
2523
2524     label = gtk_label_new( "Frequence:" );
2525     gtk_box_pack_start(GTK_BOX(hBox_ledanim), label, FALSE, FALSE, 0);
2526     ldata->freq = gtk_spin_button_new_with_range( 0.1, 10.0, 0.1);
2527     gtk_box_pack_start(GTK_BOX(hBox_ledanim), ldata->freq, FALSE, FALSE, 0);
2528     gtk_widget_show( ldata->freq );
2529   }
2530
2531   // Cedric
2532   // USB device control
2533   GtkWidget* frame_configuration = gtk_frame_new("Configuration");
2534   GtkWidget* hBox_configuration = gtk_hbox_new(FALSE, 20);
2535   gtk_container_add( GTK_CONTAINER(frame_configuration), hBox_configuration );
2536
2537   {
2538     GtkWidget *button1, *button2;
2539
2540     control_label = gtk_label_new( pcfg->default_control->name );
2541     gtk_box_pack_start(GTK_BOX(hBox_configuration), control_label, FALSE, FALSE, 2);
2542
2543     button1 = gtk_button_new_with_label("USB configuration");
2544     g_signal_connect(G_OBJECT(button1), "clicked", G_CALLBACK(ihm_setUSBConfigParamsWindow), (gpointer)NULL);
2545
2546     gtk_box_pack_start(GTK_BOX(hBox_configuration), button1  , FALSE, TRUE, 0);
2547
2548     button2 = gtk_button_new_with_label("Flight control settings");
2549     g_signal_connect(G_OBJECT(button2), "clicked", G_CALLBACK(ihm_setFlightControlParamsWindow), NULL);
2550
2551     gtk_box_pack_start(GTK_BOX(hBox_configuration), button2  , FALSE, TRUE, 0);
2552   }
2553
2554   /* Image display main window */
2555   /* ------------------------- */
2556   create_image_window( );
2557
2558   /* vBox (main box) construction */
2559   /* ---------------------------- */
2560   gtk_box_pack_start(GTK_BOX(vBox), button_show_image2   , FALSE, TRUE, 0 );
2561   gtk_box_pack_start(GTK_BOX(vBox), button_navdatademo   , FALSE, TRUE, 0 );
2562   gtk_box_pack_start(GTK_BOX(vBox), frame_ctrl_mykonos   , FALSE, FALSE, 0);
2563   gtk_box_pack_start(GTK_BOX(vBox), hBox_CtrlGain        , FALSE, TRUE, 0 );
2564   gtk_box_pack_start(GTK_BOX(vBox), ctrl_gain_h_separator, FALSE, TRUE, 0 );
2565   gtk_box_pack_start(GTK_BOX(vBox), hBox_AltCtrlGain     , FALSE, TRUE, 0 );
2566   gtk_box_pack_start(GTK_BOX(vBox), frame_Attitude       , FALSE, TRUE, 5 );
2567   gtk_box_pack_start(GTK_BOX(vBox), hBox_RCref_ETime     , FALSE, TRUE, 5 );
2568   gtk_box_pack_start(GTK_BOX(vBox), pframe_CurveCtrl     , FALSE, TRUE, 5 );
2569 #ifdef DEBUG
2570   gtk_box_pack_start(GTK_BOX(vBox), frame_logs_mykonos   , FALSE, TRUE, 0 );
2571 #endif
2572   gtk_box_pack_start(GTK_BOX(vBox), frame_cad            , FALSE, TRUE, 0 );
2573 #ifdef DEBUG
2574   /*Stephane*/ gtk_box_pack_start(GTK_BOX(vBox), detectionHistory_label, FALSE, TRUE, 0 );
2575 #endif
2576   gtk_box_pack_start(GTK_BOX(vBox), frame_ledanim        , FALSE, TRUE, 0 );
2577
2578 #ifdef DEBUG
2579   /*Stephane*/ gtk_widget_show( detectionHistory_label );
2580 #endif
2581   /*Cedric*/ gtk_box_pack_start(GTK_BOX(vBox), frame_configuration, FALSE, TRUE, 0 );
2582
2583   gtk_container_add(GTK_CONTAINER(MainWin), vBox);
2584   gtk_widget_show_all(MainWin);
2585
2586   printf("   ihm_init : main window display\n");
2587   g_timeout_add(100, (GtkFunction)update_display, NULL );
2588 }
2589
2590 void ihm_init_time( void )
2591 {
2592   struct timeval tv;
2593   char tmp[64];
2594
2595   vp_os_memset(&ihm_time,0,sizeof(ihm_time_t));
2596   gettimeofday( &tv, NULL);
2597   sprintf( tmp,"%d.%06d", (int)tv.tv_sec, (int)tv.tv_usec);
2598   ihm_time.time_init = (double)atof( tmp );
2599 }
2600
2601 void ihm_update_time( void )
2602 {
2603   struct timeval tv;
2604   char tmp[64];
2605
2606   gettimeofday( &tv, NULL);
2607   sprintf( tmp,"%d.%06d", (int)tv.tv_sec, (int)tv.tv_usec);
2608   ihm_time.time = (double)atof( tmp ) - ihm_time.time_init;
2609   ihm_time.sec = (int)ihm_time.time;
2610   ihm_time.min = (int)(ihm_time.sec/60);
2611 }
2612
2613 DEFINE_THREAD_ROUTINE(ihm , data)
2614 {
2615         /* IHM initialisation */
2616         printf("\n   IHM initialisation\n\n");
2617         ihm_init_time( );
2618         pcfg = (mobile_config_t *)data;
2619
2620         /* init gtk */
2621         g_thread_init(NULL);
2622         gdk_threads_init();
2623         gtk_init(NULL,NULL);
2624
2625         ihm_init();
2626         setlocale(LC_NUMERIC, "en_GB.UTF-8");
2627         //usleep(1000000);
2628         pcfg->ihm_curve_alloc_OK = 1;
2629
2630         /* IHM main loop */
2631         printf("\n   IHM main loop\n\n");
2632
2633         ihm_is_initialized=1;
2634
2635         // http://library.gnome.org/devel/gtk-faq/stable/x481.html
2636         gdk_threads_enter();
2637         gtk_main();
2638         gdk_threads_leave();
2639
2640         return 0;
2641 }