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

   1 /*
   2  * @view_drone_attitude.c
   3  * @author florian.pantaleao.ext@parrot.fr
   4  * @date 2006/11/08
   5  *
   6  * display of BTT-drone Euler angles and curves of IMU outputs
   7  * original version by Marc-Olivier DZEUKOU
   8  *
   9  */
  10
  11 #include <math.h>
  12
  13 #include "control_states.h"
  14 #include "common/mobile_config.h"
  15 #include "common/common.h"
  16
  17 #include "ihm/ihm.h"
  18 #include "ihm/view_drone_attitude.h"
  19 #include "ihm/ihm_vision.h"
  20 #include "ihm/ihm_stages_o_gtk.h"
  21 #include "UI/ui.h"
  22
  23 #define KIHM_RAD_TO_DEG      (180.0/KIHM_PI)
  24 #define KIHM_MILLIDEG_TO_RAD (KIHM_PI/180000.0)
  25
  26 /* Allocation donnees partagees */
  27
  28
  29 extern int tab_g[NB_GAINS];
  30 extern int tab_ag[NB_GAINS_ALT];
  31 extern int tab_fp_g[NB_GAINS_FP];
  32 extern int tab_roundel_g[NB_GAINS_ROUNDEL];
  33
  34 extern GtkWidget *label_RCref;
  35 extern GtkWidget *ihm_ImageEntry[9], *label_elapsedT;
  36 extern GtkWidget *ihm_ImageEntry[9];
  37 extern GtkLabel  *label_mykonos_values[NB_ARDRONE_STATES];
  38 extern GtkWidget *darea_start_button_state;
  39
  40 #ifdef USE_ARDRONE_VICON
  41 extern GtkWidget *darea_vicon_button_state;
  42 #endif
  43
  44 extern ihm_time_t ihm_time;
  45
  46 extern int image_vision_window_status, image_vision_window_view;
  47 extern int tab_vision_config_params[10];
  48 extern int  MiscVar[NB_MISC_VARS];
  49
  50 extern drone_angular_rate_references_t wref;//, wtrim;
  51 extern drone_euler_angles_references_t earef;//, eatrim;
  52 extern int32_t rc_ref_gaz;
  53 extern double tab_trims[3];
  54
  55 /* Stephane */
  56 #ifdef PC_USE_VISION
  57 extern vp_stages_draw_trackers_config_t draw_trackers_cfg;
  58 #endif
  59 #ifdef DEBUG
  60 extern GtkWidget* detectionHistory_label;
  61 #endif
  62 extern int detectionHistory[NB_DETECTION_SAMPLES];   // stores the number of detected tags during one second
  63 static int nb_tags_detection_samples_received = 0;   // counts the number of detection results diplayed
  64
  65 static int32_t mykonos_control_state = -1;
  66 void set_control_state(int32_t control_state)
  67 {
  68   if( mykonos_control_state == -1 )
  69     mykonos_control_state = control_state;
  70
  71   {
  72     uint32_t major = control_state >> 16;
  73     uint32_t old_major = mykonos_control_state >> 16;
  74     input_state_t* input_state = ardrone_tool_get_input_state();
  75
  76     if( (old_major == CTRL_TRANS_LANDING && major == CTRL_LANDED) || (major == CTRL_DEFAULT && old_major != CTRL_DEFAULT) ){
  77       ardrone_tool_input_reset();
  78      }
  79     if(major==CTRL_DEFAULT && input_state->start ){
  80       ardrone_tool_start_reset();  // the start i reseted if button start is pushed in CTRL_DEFAULT
  81      }
  82   }
  83
  84   mykonos_control_state = control_state;
  85 }
  86
  87 #ifdef USE_ARDRONE_VICON
  88 static int32_t ardrone_vicon_state = -1;
  89 void set_vicon_state(int32_t vicon_state)
  90 {
  91         ardrone_vicon_state = vicon_state;
  92 }
  93 #endif
  94
  95 /* function for curve plotting */
  96 /* --------------------------- */
  97 void plot_curve(SIHM_CurveAttributes *pCA)
  98 {
  99   char str_value[128], str_format[32];
 100   int k, val;
 101   int a, b;
 102   int grad = 0;
 103   SIHM_ScaleAttributes *pSA = &(pCA->tSA[pCA->range]);
 104
 105   /* Clean area with a grey rectangle before drawing */
 106   gdk_draw_rectangle( pCA->DA->window, ihm_GC[KIHM_GC_GREY], TRUE, 0, 0,
 107                       KIHM_DAREA_CURVE_X_SIZE, KIHM_DAREA_CURVE_Y_SIZE+2*KIHM_DAREA_CURVE_Y_OFFSET );
 108
 109   /* Draw horizontal dashed lines */
 110   for(k=0;k<pSA->nb_grad;k++) {
 111     grad = pSA->y_min - (int) ((k*pSA->phys_step * KIHM_DAREA_CURVE_Y_SIZE) / pSA->phys_range);
 112     gdk_draw_line(pCA->DA->window, ihm_GC[KIHM_GC_DASHLINE], 0, grad, KIHM_N_PT2PLOT, grad);
 113   }
 114
 115   for (val=0;val<pCA->nb_val; val++) {
 116     // set curves points
 117     for(k=0;k<KIHM_N_PT2PLOT-1;k++) {
 118       if( k > (ihm_val_idx-1) ) {
 119         a = KIHM_N_PT2PLOT-1-(k-ihm_val_idx);
 120         b = KIHM_N_PT2PLOT-1-(k-(ihm_val_idx-1));
 121       }
 122       else {
 123         a = ihm_val_idx-k-1;
 124         b = ihm_val_idx-k-2;
 125       }
 126
 127       if( k==(ihm_val_idx-1) ) {
 128         a = 0;
 129         b = KIHM_N_PT2PLOT-1;
 130       }
 131
 132       gdk_draw_line(pCA->DA->window,
 133                     pCA->GC[val],
 134                     k,
 135                     pSA->y_orig - (int) ((pCA->tval[val][a] * KIHM_DAREA_CURVE_Y_SIZE) / pSA->phys_range),
 136                     k+1,
 137                     pSA->y_orig - (int) ((pCA->tval[val][b] * KIHM_DAREA_CURVE_Y_SIZE) / pSA->phys_range));
 138     }
 139
 140     // set current value
 141     strcpy(str_format,"%s");
 142     strcat(str_format,pCA->val_format);
 143     strcat(str_format,"%s");
 144     sprintf(str_value, str_format, " ", pCA->tval[val][ihm_val_idx], "        ");
 145     gtk_label_set_text(GTK_LABEL(pCA->lblVal[val]), str_value);
 146   }
 147
 148   /* Show refreshed image*/
 149   gtk_widget_show_all(pCA->DA);
 150 }
 151
 152
 153
 154
 155 /* function for angle drawing */
 156 /* -------------------------- */
 157 void draw_angle(GtkWidget *widget, PangoLayout  *pPL, double *pangle_deg, int angle_sign, char *ptitle)
 158 {
 159   int k;
 160   double angle, cos_angle, sin_angle;
 161 /*   GdkGC *ColorsGC[4] = { pBlueGC, pGreenGC, pRedGC, widget->style->black_gc}; */
 162   GdkGC *ColorsGC[4] = {ihm_GC[KIHM_GC_RED], widget->style->black_gc, ihm_GC[KIHM_GC_BLUE], ihm_GC[KIHM_GC_GREEN]};
 163   char legend[4][10] = {"Fus","Ref","Acc","Gyr"};
 164   char strval[10];
 165   int X, Y, L, x1, x2, y1, y2, xt, yt;
 166
 167   xt = 5; yt = 20;
 168   L = KIHM_DAREA_ANGLE_X_SIZE*3/5;  // Lenght of line
 169   X = KIHM_DAREA_ANGLE_X_SIZE - L/2 - xt;  // X line center position
 170   Y = KIHM_DAREA_ANGLE_Y_SIZE/2;           // Y line center position
 171
 172   /* Clean area with a white rectangle before drawing */
 173   gdk_draw_rectangle( widget->window, widget->style->white_gc, TRUE,
 174                       0,
 175                       0,
 176                       KIHM_DAREA_ANGLE_X_SIZE,
 177                       KIHM_DAREA_ANGLE_Y_SIZE );
 178   gdk_draw_line(widget->window, ihm_GC[KIHM_GC_DASHLINE], 0, Y, KIHM_DAREA_ANGLE_X_SIZE, Y);  // horizontal reference
 179
 180   // graph title
 181   pango_layout_set_alignment(pPL, PANGO_ALIGN_CENTER);
 182   pango_layout_set_width(pPL, KIHM_DAREA_ANGLE_X_SIZE*PANGO_SCALE);
 183   pango_layout_set_text(pPL, ptitle, -1);
 184   gdk_draw_layout(widget->window, widget->style->black_gc, 0, 3, pPL);
 185
 186   /* Draw angle(s), legend(s) and current value(s) */
 187   pango_layout_set_alignment(pPL, PANGO_ALIGN_LEFT);
 188   pango_layout_set_width(pPL, -1); // no line wrap
 189   for(k=0;k<4;k++) {
 190     // angle
 191     angle = pangle_deg[k] * 3.14159 / 180.0;
 192     cos_angle = cos(angle*angle_sign);
 193     sin_angle = sin(angle*angle_sign);
 194     x1 = X + (L/2)*cos_angle;
 195     y1 = Y - (L/2)*sin_angle;
 196     x2 = X - (L/2)*cos_angle;
 197     y2 = Y + (L/2)*sin_angle;
 198     gdk_draw_line(widget->window, ColorsGC[k], (int)x1, (int)y1, (int)x2, (int)y2);
 199
 200     // legend
 201     pango_layout_set_alignment(pPL, PANGO_ALIGN_LEFT);
 202     pango_layout_set_width(pPL, -1); // no line wrap
 203     pango_layout_set_text(pPL, legend[k], -1);
 204     gdk_draw_layout(widget->window, ColorsGC[k], xt, yt, pPL);
 205
 206     // current value
 207     pango_layout_set_alignment(pPL, PANGO_ALIGN_RIGHT);
 208     pango_layout_set_width(pPL, 50*PANGO_SCALE);
 209     if ((angle<1000.0)&&(angle>-1000.0)) {
 210       sprintf(strval, "%7.2f", pangle_deg[k]);
 211       pango_layout_set_text(pPL, strval, -1);
 212     };
 213     gdk_draw_layout(widget->window, widget->style->black_gc, xt+15, yt, pPL);
 214     yt += 18;
 215   }
 216
 217   /* Show refrehed image*/
 218   gtk_widget_show_all(widget);
 219 }
 220
 221
 222
 223 void draw_psi( void )
 224 {
 225   char strval[10];
 226   double psi;
 227   GtkWidget *widget = (GtkWidget*) ihm_DA_att[KIHM_DA_ATT_YAW];
 228
 229   int X, Y, L, l, diameter, xl, yl;
 230   diameter = KIHM_DAREA_ANGLE_Y_SIZE - 30;
 231   L = diameter - 2;
 232   l = 10;
 233   X = KIHM_DAREA_ANGLE_X_SIZE/2 + 20;
 234   Y = KIHM_DAREA_ANGLE_Y_SIZE/2;
 235
 236   psi = ihm_psi * 3.14159 / 180.0;
 237
 238   /* Clean area by a white rectangle before drawing psi */
 239   gdk_draw_rectangle( widget->window, widget->style->white_gc, TRUE,
 240                       0,
 241                       0,
 242                       KIHM_DAREA_ANGLE_X_SIZE,
 243                       KIHM_DAREA_ANGLE_Y_SIZE );
 244
 245   /* Draw circle for psi */
 246   gdk_draw_arc( widget->window, widget->style->black_gc, FALSE,
 247                 X - diameter/2,
 248                 Y - diameter/2,
 249                 diameter,
 250                 diameter,
 251                 0,
 252                 360*64 );
 253
 254   /* Draw psi angle in negative because heading is represented with
 255      negative trigonometric sense. */
 256   GdkPoint tPoints[4];
 257   // North point:
 258   tPoints[0].x = X - (L/2)*sin(-psi);
 259   tPoints[0].y = Y - (L/2)*cos(-psi);
 260   // East point:
 261   tPoints[1].x = X + (l/2)*cos(-psi);
 262   tPoints[1].y = Y - (l/2)*sin(-psi);
 263   // West point:
 264   tPoints[2].x = X - (l/2)*cos(-psi);
 265   tPoints[2].y = Y + (l/2)*sin(-psi);
 266   // South point:
 267   tPoints[3].x = X + (L/2)*sin(-psi);
 268   tPoints[3].y = Y + (L/2)*cos(-psi);
 269
 270   gdk_draw_polygon(widget->window, ihm_GC[KIHM_GC_GREY], TRUE, &tPoints[1], 3);
 271   gdk_draw_polygon(widget->window, ihm_GC[KIHM_GC_RED ], TRUE, tPoints, 3);
 272
 273   // graph title
 274   pango_layout_set_alignment(ihm_PL_DApsi, PANGO_ALIGN_LEFT);
 275   pango_layout_set_width(ihm_PL_DApsi, -1);
 276   pango_layout_set_text(ihm_PL_DApsi, "Psi (Yaw)", -1);
 277   gdk_draw_layout(widget->window, widget->style->black_gc, 20, 3, ihm_PL_DApsi);
 278
 279   // cardinal points
 280   xl = 8; yl = xl*2;
 281   pango_layout_set_alignment(ihm_PL_DApsi, PANGO_ALIGN_LEFT);
 282   pango_layout_set_width(ihm_PL_DApsi, -1); // no line wrap
 283   pango_layout_set_text(ihm_PL_DApsi, "N", -1);
 284   gdk_draw_layout(widget->window, widget->style->black_gc, X-xl/2, Y - diameter/2 - yl, ihm_PL_DApsi);
 285   pango_layout_set_text(ihm_PL_DApsi, "E", -1);
 286   gdk_draw_layout(widget->window, widget->style->black_gc, X + diameter/2 + xl/2, Y-yl/2, ihm_PL_DApsi);
 287   pango_layout_set_text(ihm_PL_DApsi, "S", -1);
 288   gdk_draw_layout(widget->window, widget->style->black_gc, X-xl/2, Y + diameter/2, ihm_PL_DApsi);
 289   pango_layout_set_text(ihm_PL_DApsi, "W", -1);
 290   gdk_draw_layout(widget->window, widget->style->black_gc, X - diameter/2 - 3*xl/2, Y-yl/2, ihm_PL_DApsi);
 291
 292   // current value
 293   pango_layout_set_alignment(ihm_PL_DApsi, PANGO_ALIGN_RIGHT);
 294   pango_layout_set_width(ihm_PL_DApsi, 60*PANGO_SCALE);
 295   sprintf(strval, "%7.2f", ihm_psi);
 296   pango_layout_set_text(ihm_PL_DApsi, strval, -1);
 297   gdk_draw_layout(widget->window, widget->style->black_gc, 5, KIHM_DAREA_ANGLE_Y_SIZE-5-yl, ihm_PL_DApsi);
 298
 299   /* Show refreshed image*/
 300   gtk_widget_show_all( widget );
 301 }
 302
 303 void draw_dir_height( void )
 304 {
 305   char strval[10];
 306   double dir;
 307   GtkWidget *widget = (GtkWidget*) ihm_DA_att[KIHM_DA_ATT_DIR];
 308   int X, Y, L, l, diameter, xl, yl;
 309   diameter = KIHM_DAREA_ANGLE_Y_SIZE - 30;
 310   L = diameter - 2;
 311   l = 10;
 312   X = KIHM_DAREA_ANGLE_X_SIZE/2 + 20;
 313   Y = KIHM_DAREA_ANGLE_Y_SIZE/2;
 314
 315   if( (ihm_dir != NOT_DEF_VAL) && (ihm_dir != KIHM_ZERO_F) )
 316     dir = ihm_dir;
 317   else
 318     dir = KIHM_ZERO_F;
 319
 320
 321 /* Clean area by a white rectangle before drawing dir */
 322   gdk_draw_rectangle( widget->window, widget->style->white_gc, TRUE,
 323                       0,
 324                       0,
 325                       KIHM_DAREA_ANGLE_X_SIZE,
 326                       KIHM_DAREA_ANGLE_Y_SIZE );
 327  /* Draw circle for dir */
 328   gdk_draw_arc( widget->window, widget->style->black_gc, FALSE,
 329                 X - diameter/2,
 330                 Y - diameter/2,
 331                 diameter,
 332                 diameter,
 333                 0,
 334                 360*64 );
 335
 336  /* Draw dir angle in negative because heading is represented with
 337      negative trigonometric sense. */
 338   GdkPoint tPoints[4];
 339   // North point:
 340   tPoints[0].x = X - (L/2)*sin(-dir);
 341   tPoints[0].y = Y - (L/2)*cos(-dir);
 342   // East point:
 343   tPoints[1].x = X + (l/2)*cos(-dir);
 344   tPoints[1].y = Y - (l/2)*sin(-dir);
 345   // West point:
 346   tPoints[2].x = X - (l/2)*cos(-dir);
 347   tPoints[2].y = Y + (l/2)*sin(-dir);
 348   // South point:
 349   tPoints[3].x = X + (L/2)*sin(-dir);
 350   tPoints[3].y = Y + (L/2)*cos(-dir);
 351
 352  if( ihm_dir != NOT_DEF_VAL && ihm_dir != HOVER_VAL) {
 353     gdk_draw_polygon(widget->window, ihm_GC[KIHM_GC_GREY], TRUE, &tPoints[1], 3);
 354     gdk_draw_polygon(widget->window, ihm_GC[KIHM_GC_BLUE], TRUE, tPoints    , 3);
 355   }
 356   else if( ihm_dir == HOVER_VAL) {
 357     gdk_draw_arc( widget->window, ihm_GC[KIHM_GC_GREEN], TRUE,
 358                   X - L/2,
 359                   Y - L/2,
 360                   L,
 361                   L,
 362                   0,
 363                   360*64 );
 364   }
 365   else {
 366     gdk_draw_arc( widget->window, ihm_GC[KIHM_GC_RED], TRUE,
 367                   X - L/2,
 368                   Y - L/2,
 369                   L,
 370                   L,
 371                   0,
 372                   360*64 );
 373   }
 374   // graph title
 375   pango_layout_set_alignment(ihm_PL_DAdir, PANGO_ALIGN_LEFT);
 376   pango_layout_set_width(ihm_PL_DAdir, -1);
 377   pango_layout_set_text(ihm_PL_DAdir, "Dir (Vision)", -1);
 378   gdk_draw_layout(widget->window, widget->style->black_gc, 20, 3, ihm_PL_DAdir);
 379
 380  // cardinal points
 381   xl = 8; yl = xl*2;
 382   pango_layout_set_alignment(ihm_PL_DAdir, PANGO_ALIGN_LEFT);
 383   pango_layout_set_width(ihm_PL_DAdir, -1); // no line wrap
 384   pango_layout_set_text(ihm_PL_DAdir, "X", -1);
 385   gdk_draw_layout(widget->window, widget->style->black_gc, X-xl/2, Y - diameter/2 - yl, ihm_PL_DAdir);
 386   pango_layout_set_text(ihm_PL_DAdir, "Y", -1);
 387   gdk_draw_layout(widget->window, widget->style->black_gc, X + diameter/2 + xl/2, Y-yl/2, ihm_PL_DAdir);
 388   pango_layout_set_text(ihm_PL_DAdir, "-X", -1);
 389   gdk_draw_layout(widget->window, widget->style->black_gc, X-xl/2, Y + diameter/2, ihm_PL_DAdir);
 390   pango_layout_set_text(ihm_PL_DAdir, "-Y", -1);
 391   gdk_draw_layout(widget->window, widget->style->black_gc, X - diameter/2 - 3*xl/2, Y-yl/2, ihm_PL_DAdir);
 392
 393   // current value
 394   pango_layout_set_alignment(ihm_PL_DAdir, PANGO_ALIGN_RIGHT);
 395   pango_layout_set_width(ihm_PL_DAdir, 60*PANGO_SCALE);
 396   if( ihm_dir < 360 ){
 397      sprintf(strval, "%3.2f", KIHM_RAD_TO_DEG*ihm_dir);
 398   }
 399   else sprintf(strval, "%d", 0);
 400   pango_layout_set_text(ihm_PL_DAdir, strval, -1);
 401   gdk_draw_layout(widget->window, widget->style->black_gc, 5, KIHM_DAREA_ANGLE_Y_SIZE-5-yl, ihm_PL_DAdir);
 402
 403   /* Show refreshed image*/
 404   gtk_widget_show_all( widget );
 405 }
 406 void display_RCreferences( void )
 407 {
 408   char str[256];
 409
 410   sprintf(str, "Theta %04.2f ThetaTrim %02.2f   Phi %04.2f PhiTrim %02.2f   dPsi %04.2f  dPsiTrim %04.2f   Gaz %d",
 411           earef.theta/1000.0, tab_trims[0],
 412           earef.phi/1000.0  , tab_trims[1],
 413           wref.r/1000.0     , tab_trims[2],
 414           rc_ref_gaz );
 415   gtk_label_set_text(GTK_LABEL(ihm_label_RCref), str);
 416   gtk_label_set_justify(GTK_LABEL(ihm_label_RCref), GTK_JUSTIFY_LEFT);
 417 }
 418
 419 void display_ElapsedTime( void )
 420 {
 421   char str[256];
 422
 423   ihm_update_time( );
 424   sprintf(str, "%02d:%02d", ihm_time.min, ihm_time.sec - 60*ihm_time.min);
 425   gtk_label_set_text(GTK_LABEL(label_elapsedT), str);
 426   gtk_label_set_justify(GTK_LABEL(label_elapsedT), GTK_JUSTIFY_LEFT);
 427 }
 428
 429
 430 void get_controler_gain(void)
 431 {
 432   int k;
 433   G_CONST_RETURN gchar* CtrlGain[NB_ALL_GAINS];
 434
 435   for (k = 0; k < NB_GAINS; k++) {
 436     CtrlGain[k] = gtk_entry_get_text(GTK_ENTRY(entry_PID[k]));
 437     tab_g[k] = (int) atof(CtrlGain[k]);
 438   }
 439
 440   for (k = 0; k < NB_GAINS_ALT; k++) {
 441     CtrlGain[NB_GAINS + k] = gtk_entry_get_text(GTK_ENTRY(entry_PID[NB_GAINS + k]));
 442     tab_ag[k] = (int) atof(CtrlGain[NB_GAINS + k]);
 443   }
 444
 445   for (k = 0; k < NB_GAINS_FP; k++) {
 446     CtrlGain[NB_GAINS + NB_GAINS_ALT + k] = gtk_entry_get_text(GTK_ENTRY(entry_PID[NB_GAINS + NB_GAINS_ALT + k]));
 447     tab_fp_g[k] = (int) atof(CtrlGain[NB_GAINS + NB_GAINS_ALT + k]);
 448   }
 449
 450   for (k = 0; k < NB_GAINS_ROUNDEL; k++) {
 451     CtrlGain[NB_GAINS + NB_GAINS_ALT + NB_GAINS_FP + k] = gtk_entry_get_text(GTK_ENTRY(entry_PID[NB_GAINS + NB_GAINS_ALT + NB_GAINS_FP + k]));
 452     tab_roundel_g[k] = (int) atof(CtrlGain[NB_GAINS + NB_GAINS_ALT + NB_GAINS_FP + k]);
 453   }
 454
 455 }
 456
 457 void display_ctrl_states()
 458 {
 459  gtk_label_set_label(label_mykonos_values[0], label_mykonos_state_value);
 460  if(label_mykonos_values[1]!=NULL && GTK_IS_LABEL(label_mykonos_values[1]))
 461          gtk_label_set_label(label_mykonos_values[1], label_ctrl_state_value);
 462  gtk_label_set_label(label_mykonos_values[2], label_vision_state_value);
 463 }
 464
 465 void get_misc_var(void)
 466 {
 467   G_CONST_RETURN gchar* miscvar;
 468   int k;
 469
 470   for (k=0;k<NB_MISC_VARS;k++) {
 471                   if (entry_MiscVar[k]!=NULL && GTK_IS_ENTRY(GTK_ENTRY(entry_MiscVar[k]))){
 472           miscvar = gtk_entry_get_text( GTK_ENTRY(entry_MiscVar[k]) );
 473           MiscVar[k] = (int)atof( miscvar );
 474   }}
 475 }
 476
 477 void get_vision_config_params(void)
 478 {
 479
 480   int k;
 481   G_CONST_RETURN gchar* VisionCP[9];
 482
 483    if( image_vision_window_view == WINDOW_VISIBLE) {
 484
 485
 486   for (k=0;k<9;k++) {
 487           if (ihm_ImageEntry[k]!=NULL){
 488           if (GTK_IS_ENTRY(ihm_ImageEntry[k])){
 489                 VisionCP[k] = gtk_entry_get_text( GTK_ENTRY(ihm_ImageEntry[k]) );
 490                 tab_vision_config_params[k] = (int)atof( VisionCP[k] );
 491         }
 492
 493   }}}
 494 }
 495
 496 void display_start_button_state( void )
 497 {
 498   GdkGC *start_button_color, *start_button_border_color;
 499
 500   switch( ihm_start )
 501   {
 502   case 0:
 503     start_button_color = ihm_GC[KIHM_GC_RED];
 504     break;
 505   case 1:
 506     start_button_color = (mykonos_control_state > CTRL_LANDED) ? ihm_GC[KIHM_GC_GREEN] : ihm_GC[KIHM_GC_RED];
 507     break;
 508   default:
 509     start_button_color = ihm_GC[KIHM_GC_RED];
 510     break;
 511   }
 512
 513   start_button_border_color = ihm_GC[KIHM_GC_BLUE];
 514   gdk_draw_arc( darea_start_button_state->window,
 515                 start_button_color,
 516                 TRUE,
 517                 2, 2,
 518                 START_BUTTON_DA_SIZE - 4,
 519                 START_BUTTON_DA_SIZE - 4,
 520                 0,
 521                 360*64 );
 522
 523   gdk_draw_arc( darea_start_button_state->window,
 524                 start_button_border_color,
 525                 FALSE,
 526                 2, 2,
 527                 START_BUTTON_DA_SIZE - 4,
 528                 START_BUTTON_DA_SIZE - 4,
 529                 0,
 530                 360*64 );
 531 }
 532
 533 #ifdef USE_ARDRONE_VICON
 534 void display_vicon_button_state( void )
 535 {
 536   GdkGC *vicon_button_color, *vicon_button_border_color;
 537   vicon_button_color = (ardrone_vicon_state != -1) ? ihm_GC[KIHM_GC_GREEN] : ihm_GC[KIHM_GC_RED];
 538
 539   vicon_button_border_color = ihm_GC[KIHM_GC_BLUE];
 540   gdk_draw_arc( darea_vicon_button_state->window,
 541                 vicon_button_color,
 542                 TRUE,
 543                 2, 2,
 544                 VICON_BUTTON_DA_SIZE - 4,
 545                 VICON_BUTTON_DA_SIZE - 4,
 546                 0,
 547                 360*64 );
 548
 549   gdk_draw_arc( darea_vicon_button_state->window,
 550                 vicon_button_border_color,
 551                 FALSE,
 552                 2, 2,
 553                 VICON_BUTTON_DA_SIZE - 4,
 554                 VICON_BUTTON_DA_SIZE - 4,
 555                 0,
 556                 360*64 );
 557 }
 558 #endif
 559
 560 gboolean update_display(gpointer pData)
 561 {
 562
 563   int k;
 564   double tangle[4];
 565   char str_format[32];
 566   // char str_value[128];
 567 #if defined (PC_USE_VISION) && defined (DEBUG)
 568   /*Stephane*/ int decHist_cnt;
 569 #endif
 570
 571  // printf("%s %i : locking\n",__FUNCTION__,__LINE__);
 572   //vp_os_mutex_lock(&ihm_lock);
 573   //printf("%s %i : locked\n",__FUNCTION__,__LINE__);
 574   gdk_threads_enter(); //http://blogs.operationaldynamics.com/andrew/software/gnome-desktop/gtk-thread-awareness.html
 575
 576
 577   if( image_vision_window_view == WINDOW_VISIBLE )
 578   {
 579     get_vision_config_params();
 580   }
 581   update_vision();
 582   // theta display
 583   tangle[0] = ihm_CA[KIHM_CURVE_THETA].tval[0][ihm_val_idx];
 584   tangle[1] = ihm_CA[KIHM_CURVE_THETA].tval[1][ihm_val_idx];
 585   tangle[2] = ihm_CA[KIHM_CURVE_THETA].tval[2][ihm_val_idx];
 586   tangle[3] = ihm_CA[KIHM_CURVE_THETA].tval[3][ihm_val_idx];
 587   draw_angle(ihm_DA_att[KIHM_DA_ATT_PITCH], ihm_PL_DAtheta, tangle,  1, "Theta (Pitch)");
 588
 589   // phi display
 590   tangle[0] = ihm_CA[KIHM_CURVE_PHI].tval[0][ihm_val_idx];
 591   tangle[1] = ihm_CA[KIHM_CURVE_PHI].tval[1][ihm_val_idx];
 592   tangle[2] = ihm_CA[KIHM_CURVE_PHI].tval[2][ihm_val_idx];
 593   tangle[3] = ihm_CA[KIHM_CURVE_PHI].tval[3][ihm_val_idx];
 594   draw_angle(ihm_DA_att[KIHM_DA_ATT_ROLL ], ihm_PL_DAphi  , tangle, -1, "Phi (Roll)");
 595
 596
 597   // psi display
 598   draw_psi();
 599
 600   // dir display
 601   draw_dir_height();
 602
 603   // Get miscllaneous variables
 604   get_misc_var();
 605
 606   // Radiocommand references display
 607   display_RCreferences();
 608
 609   // Elapsed time display
 610   display_ElapsedTime();
 611
 612   // Control States display
 613   display_ctrl_states();
 614
 615   // Start button state display
 616   display_start_button_state();
 617
 618 #ifdef USE_ARDRONE_VICON
 619   // Vicon button state display
 620   display_vicon_button_state();
 621 #endif
 622
 623 #ifdef PC_USE_VISION
 624  /* Stephane : displays number of detected tags in the main window */
 625  /* The array 'draw_trackers_cfg contains info about tag detection */
 626
 627   #ifdef DEBUG
 628     char nbDetectedTags_label_buffer[1024],nbDetectedTags_label_buffer2[1024];
 629 #endif
 630     /*snprintf(nbDetectedTags_label_buffer,sizeof(nbDetectedTags_label_buffer),"Found : %i",draw_trackers_cfg.detected);
 631     if(nbDetectedTags_label!=NULL)
 632     gtk_label_set_text((GtkLabel*)nbDetectedTags_label,(const gchar*)nbDetectedTags_label_buffer);
 633     */
 634
 635
 636     detectionHistory[nb_tags_detection_samples_received%NB_DETECTION_SAMPLES] = draw_trackers_cfg.detected;
 637     nb_tags_detection_samples_received++;
 638 #ifdef DEBUG
 639     if((nb_tags_detection_samples_received%NB_DETECTION_SAMPLES)==0)
 640     {
 641         snprintf(nbDetectedTags_label_buffer,sizeof(nbDetectedTags_label_buffer),"Previously detected targets : ");
 642         for (decHist_cnt=0;decHist_cnt<NB_DETECTION_SAMPLES;decHist_cnt++)
 643         {
 644             snprintf(nbDetectedTags_label_buffer2,
 645                     sizeof(nbDetectedTags_label_buffer2),
 646                     "%i ",
 647                     detectionHistory[decHist_cnt]);
 648             strncat(nbDetectedTags_label_buffer,
 649                     nbDetectedTags_label_buffer2,
 650                     sizeof(nbDetectedTags_label_buffer)-1);
 651         }
 652         // Displays the history on the bottom of the main window (see detectionHistory_label in ihm_init() )
 653         gtk_label_set_text((GtkLabel*)detectionHistory_label,(const gchar*)nbDetectedTags_label_buffer);
 654     }
 655 #endif
 656 #endif
 657
 658   if (ihm_freeze_curves == FALSE) {
 659     // curve display
 660     for (k=0; k<KIHM_NB_CURVES; k++) {
 661       if( (GTK_IS_WIDGET(ihm_CA[k].PrivateWin)) && (ihm_CA[k].win_view == WINDOW_VISIBLE) ) {
 662         plot_curve(&ihm_CA[k]);
 663         if( k == KIHM_CURVE_VBAT) {
 664           // set current value
 665           strcpy(str_format,"%s");
 666           strcat(str_format,ihm_CA[k].val_format);
 667           strcat(str_format,"%s");
 668           // sprintf(str_value, str_format, " ", ihm_CA[k].tval[2][ihm_val_idx], "        ");
 669           // gtk_label_set_text(GTK_LABEL(ihm_CA[k].lblVal[2]), str_value);
 670         }
 671       }
 672     }
 673   }
 674   get_controler_gain();
 675
 676   gtk_label_set_text((GtkLabel*)activeDetection_label,(const gchar*)label_detection_state_value);
 677
 678  // printf("%s %i : unlocking\n",__FUNCTION__,__LINE__);
 679  // vp_os_mutex_unlock(&ihm_lock);
 680
 681   gdk_threads_leave(); //http://blogs.operationaldynamics.com/andrew/software/gnome-desktop/gtk-thread-awareness.html
 682
 683         return TRUE;
 684 }