git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/Examples/Linux/Navigation/Sources/navdata_client/navdata_ihm.c

   1 #include <string.h>
   2 #include <VP_Os/vp_os_malloc.h>
   3 #include <VP_Os/vp_os_print.h>
   4
   5 #include <config.h>
   6
   7 #include "common/common.h"
   8 #include "ihm/ihm.h"
   9 #include "ihm/ihm_vision.h"
  10 #include "ihm/ihm_stages_o_gtk.h"
  11 #include "ihm/view_drone_attitude.h"
  12 #include "navdata_client/navdata_ihm.h"
  13 #include <ardrone_api.h>
  14
  15 #ifdef PC_USE_VISION
  16 #     include <Vision/vision_tracker_engine.h>
  17 #endif
  18
  19 #define CTRL_STATES_STRING
  20 #include "control_states.h"
  21
  22 #define SIGN(x) ( x>=0 ? 1: -1)
  23
  24 #ifdef EXTERNAL_CONTROL
  25
  26 #include <mykonos.h>
  27 #include <mykonos_config.h>
  28 #include <ATcodec/ATcodec_api.h>
  29 #include <Acquisition/acq_fsm.h>
  30 #include <Control/control_fsm.h>
  31
  32 extern C_RESULT set_ui_input(bool_t force_landing);
  33 extern bool_t is_ui_ignored(void);
  34 extern int32_t get_ui_misc0( void );
  35
  36 extern mykonos_t mykonos;
  37 extern int32_t iphone_acceleros_enable;
  38 static uint32_t input_check_cpt = COM_INPUT_CHECK_CPT_THRESHOLD; // Make sure we have no input at startup
  39
  40 static uint32_t ctrl_start, ctrl_current;
  41 static int32_t ctrl_watchdog;
  42 static int32_t cpt_50hz;
  43 static int32_t cpt_10hz;
  44 static acq_t acq;
  45 static ctrl_t ctrl = { 0 };
  46
  47 #endif // ! EXTERNAL_CONTROL
  48
  49 drone_angular_rate_references_t wref;//, wtrim;
  50 drone_euler_angles_references_t earef;//, eatrim;
  51 int32_t rc_ref_gaz;
  52 double tab_trims[3];
  53 int32_t ihm_val_idx = 0;
  54
  55 #ifdef PC_USE_VISION
  56 static vp_stages_draw_trackers_config_t draw_trackers_cfg = { 0 };
  57 #endif
  58
  59
  60
  61 /*============================================================================*/
  62 /**
  63   * Builds a string describing the drone control state.
  64   * @param ctrl_state Integer containing the control state from the unpacked navdata.
  65   * @return A pointer to the built string. The returned address always points to the last built string.
  66   */
  67 const char* ctrl_state_str(uint32_t ctrl_state)
  68 {
  69   static char str_ctrl_state[MAX_STR_CTRL_STATE];
  70
  71   ctrl_string_t* ctrl_string;
  72   uint32_t major = ctrl_state >> 16;
  73   uint32_t minor = ctrl_state & 0xFFFF;
  74
  75   if( strlen(ctrl_states[major]) < MAX_STR_CTRL_STATE )
  76   {
  77     vp_os_memset(str_ctrl_state, 0, sizeof(str_ctrl_state));
  78
  79     strcat(str_ctrl_state, ctrl_states[major]);
  80     ctrl_string = control_states_link[major];
  81
  82     if( ctrl_string != NULL && (strlen(ctrl_states[major]) + strlen(ctrl_string[minor]) < MAX_STR_CTRL_STATE) )
  83     {
  84       strcat( str_ctrl_state, " | " );
  85       strcat( str_ctrl_state, ctrl_string[minor] );
  86     }
  87   }
  88   else
  89   {
  90     vp_os_memset( str_ctrl_state, '#', sizeof(str_ctrl_state) );
  91   }
  92
  93   return str_ctrl_state;
  94 }
  95
  96 #ifdef EXTERNAL_CONTROL
  97   static vision_params_t vision_params;
  98 #endif // ! EXTERNAL_CONTROL
  99
 100
 101
 102 /*============================================================================*/
 103 /**
 104 */
 105 C_RESULT navdata_ihm_init( mobile_config_t* cfg )
 106 {
 107
 108 #ifdef EXTERNAL_CONTROL
 109   // Watchdog init
 110   set_mykonos_state(ARDRONE_CTRL_WATCHDOG_MASK, FALSE);
 111   set_mykonos_state(ARDRONE_ADC_WATCHDOG_MASK,  FALSE);
 112   set_mykonos_state(ARDRONE_COM_WATCHDOG_MASK,  TRUE);  // No input at startup
 113   set_mykonos_state(ARDRONE_COMMAND_MASK,       FALSE);
 114   set_mykonos_state(ARDRONE_TIMER_ELAPSED,      FALSE);
 115
 116   mykonos.vision = &vision_params;
 117
 118   //C_RESULT res;
 119   //adc_t adc;
 120
 121   //adc.running_fsm     = &acq_fsm;
 122
 123   acq.n_accs          = mykonos.accs;
 124   acq.n_gyros         = mykonos.gyros;
 125
 126   ctrl.euler_angles     = (euler_angles_t*) &mykonos.orientation;
 127   ctrl.angular_rates    = (angular_rates_t*) mykonos.gyros;
 128
 129   //fsm_set_param( &adc_fsm, (void*)&adc );
 130   fsm_set_param( &acq_fsm, (void*)&acq );
 131   fsm_set_param( &ctrl_fsm, (void*)&ctrl );
 132
 133   set_mykonos_state(ARDRONE_ACQ_THREAD_ON, TRUE);
 134   set_mykonos_state(ARDRONE_CONTROL_MASK, FALSE); // EULER ANGLE CONTROL
 135   set_mykonos_state(ARDRONE_ALTITUDE_MASK, FALSE); // NO ALTITUDE CONTROL
 136
 137   ctrl_watchdog = 0;
 138   ctrl_start    = 0;
 139   ctrl_current  = 0;
 140
 141   cpt_50hz      = 0;
 142   cpt_10hz      = 0;
 143 #endif // ! EXTERNAL_CONTROL
 144
 145   while (NULL != cfg && !cfg->ihm_curve_alloc_OK) sleep(1);
 146
 147   return C_OK;
 148 }
 149
 150
 151 /*============================================================================*/
 152 /**
 153   * Navdata handling callback
 154   * This callback is registered to ARDroneTool in the BEGIN_NAVDATA_HANDLER_TABLE in navdata_client.c
 155   * @param pnd Pointer to the ready-to-use navdata structure
 156   * @return Always returns C_OK.
 157   */
 158 C_RESULT navdata_ihm_process( const navdata_unpacked_t* const pnd )
 159 {
 160   //char text_buffer[1024];
 161   const navdata_vision_detect_t* pndvision = &pnd->navdata_vision_detect;
 162
 163   gdk_threads_enter();
 164
 165   // States' display update for IHM
 166   vp_os_memset( label_mykonos_state_value, '\0', sizeof(label_mykonos_state_value) );
 167   sprintf(label_mykonos_state_value, "%08x", pnd->ardrone_state);
 168
 169   strcpy(label_ctrl_state_value, ctrl_state_str( pnd->navdata_demo.ctrl_state ));
 170   set_control_state(pnd->navdata_demo.ctrl_state);
 171
 172   ihm_val_idx--;
 173   if (ihm_val_idx < 0) ihm_val_idx = KIHM_N_PT2PLOT-1;
 174
 175   // Angular rates received and displayed in deg/s
 176
 177   ihm_CA[KIHM_CURVE_GYRO].tval[0][ihm_val_idx] = ((double) pnd->navdata_phys_measures.phys_gyros[GYRO_X]);
 178   ihm_CA[KIHM_CURVE_GYRO].tval[1][ihm_val_idx] = ((double) pnd->navdata_phys_measures.phys_gyros[GYRO_Y]);
 179   ihm_CA[KIHM_CURVE_GYRO].tval[2][ihm_val_idx] = ((double) pnd->navdata_phys_measures.phys_gyros[GYRO_Z]);
 180
 181   // Accelerations are received in mg
 182   // They are displayed in g: conversion gain is 1/1000
 183   ihm_CA[KIHM_CURVE_ACC].tval[0][ihm_val_idx] = ((double) pnd->navdata_phys_measures.phys_accs[ACC_X]) / KIHM_MIL_F;
 184   ihm_CA[KIHM_CURVE_ACC].tval[1][ihm_val_idx] = ((double) pnd->navdata_phys_measures.phys_accs[ACC_Y]) / KIHM_MIL_F;
 185   ihm_CA[KIHM_CURVE_ACC].tval[2][ihm_val_idx] = ((double) pnd->navdata_phys_measures.phys_accs[ACC_Z]) / KIHM_MIL_F;
 186
 187   // Filtered  temperature is drawn in the Vbat window
 188   ihm_CA[KIHM_CURVE_VBAT].tval[0][ihm_val_idx] = ((double) (pnd->navdata_phys_measures.accs_temp));   // Filtered temperature
 189
 190   ihm_CA[KIHM_CURVE_VBAT].tval[1][ihm_val_idx] = ((double) (pnd->navdata_demo.vbat_flying_percentage)) ; // Filtered vbat in %
 191
 192   ihm_CA[KIHM_CURVE_THETA].tval[2][ihm_val_idx] = ((double) pnd->navdata_euler_angles.theta_a) / KIHM_MIL_F; // theta accelerometer value
 193   ihm_CA[KIHM_CURVE_PHI  ].tval[2][ihm_val_idx] = ((double) pnd->navdata_euler_angles.phi_a) / KIHM_MIL_F; // phi accelerometer value
 194
 195   ihm_CA[KIHM_CURVE_THETA].tval[0][ihm_val_idx] = ((double) pnd->navdata_demo.theta) / KIHM_MIL_F; // theta fused value
 196   ihm_CA[KIHM_CURVE_PHI  ].tval[0][ihm_val_idx] = ((double) pnd->navdata_demo.phi) / KIHM_MIL_F; // phi fused value
 197   ihm_psi                                       = ((double) pnd->navdata_demo.psi) / KIHM_MIL_F;
 198
 199   earef.theta = pnd->navdata_references.ref_theta;
 200   ihm_CA[KIHM_CURVE_THETA].tval[1][ihm_val_idx] = ((double) earef.theta) / KIHM_MIL_F; // theta reference value
 201
 202   earef.phi = pnd->navdata_references.ref_phi;
 203   ihm_CA[KIHM_CURVE_PHI  ].tval[1][ihm_val_idx] = ((double) earef.phi) / KIHM_MIL_F; // phi reference value
 204
 205   earef.psi = pnd->navdata_references.ref_psi;
 206
 207   wref.p = pnd->navdata_references.ref_roll;
 208   wref.q = pnd->navdata_references.ref_pitch;
 209   wref.r = pnd->navdata_references.ref_yaw;
 210
 211   rc_ref_gaz = pnd->navdata_rc_references.rc_ref_gaz;
 212
 213   ihm_CA[KIHM_CURVE_ALTITUDE].tval[0][ihm_val_idx] = ((double) pnd->navdata_demo.altitude);
 214
 215   ihm_CA[KIHM_CURVE_ALTITUDE].tval[1][ihm_val_idx] = ((double) pnd->navdata_altitude.altitude_vision);
 216   ihm_CA[KIHM_CURVE_ALTITUDE].tval[2][ihm_val_idx] = ((double) pnd->navdata_altitude.altitude_ref);
 217   ihm_CA[KIHM_CURVE_ALTITUDE].tval[3][ihm_val_idx] = ((double) pnd->navdata_altitude.altitude_raw);
 218
 219   ihm_CA[KIHM_CURVE_VZ].tval[1][ihm_val_idx] = (double) (pnd->navdata_altitude.altitude_vz);      // Fusion Vz
 220
 221   tab_trims[0] = (double)pnd->navdata_trims.euler_angles_trim_theta;
 222   tab_trims[1] = (double)pnd->navdata_trims.euler_angles_trim_phi;
 223   tab_trims[2] = (double)pnd->navdata_trims.angular_rates_trim_r;
 224
 225   ihm_CA[KIHM_CURVE_PWM].tval[0][ihm_val_idx] = (double) (pnd->navdata_pwm.motor1); // PWM motor 1
 226   ihm_CA[KIHM_CURVE_PWM].tval[1][ihm_val_idx] = (double) (pnd->navdata_pwm.motor2); // PWM motor 2
 227   ihm_CA[KIHM_CURVE_PWM].tval[2][ihm_val_idx] = (double) (pnd->navdata_pwm.motor3); // PWM motor 3
 228   ihm_CA[KIHM_CURVE_PWM].tval[3][ihm_val_idx] = (double) (pnd->navdata_pwm.motor4); // PWM motor 4
 229   ihm_CA[KIHM_CURVE_CURRENT].tval[0][ihm_val_idx] = (double) (pnd->navdata_pwm.current_motor1); // current motor 1
 230   ihm_CA[KIHM_CURVE_CURRENT].tval[1][ihm_val_idx] = (double) (pnd->navdata_pwm.current_motor2); // current motor 2
 231   ihm_CA[KIHM_CURVE_CURRENT].tval[2][ihm_val_idx] = (double) (pnd->navdata_pwm.current_motor3); // current motor 3
 232   ihm_CA[KIHM_CURVE_CURRENT].tval[3][ihm_val_idx] = (double) (pnd->navdata_pwm.current_motor4); // current motor 4
 233
 234   ihm_CA[KIHM_CURVE_VZ].tval[3][ihm_val_idx] = (double) (pnd->navdata_pwm.vz_ref);  // VZ reference
 235
 236   ihm_CA[KIHM_CURVE_VX].tval[0][ihm_val_idx] = (double) (pnd->navdata_vision_raw.vision_tx_raw);      // VISION trans. en x
 237   ihm_CA[KIHM_CURVE_VY].tval[0][ihm_val_idx] = (double) (pnd->navdata_vision_raw.vision_ty_raw);      // VISION trans. en y
 238   ihm_CA[KIHM_CURVE_VZ].tval[0][ihm_val_idx] = (double) (pnd->navdata_vision_raw.vision_tz_raw);      // VISION trans. en z
 239
 240
 241   if( pnd->vision_defined )
 242   {
 243     if( fabsf(pnd->navdata_vision_raw.vision_tx_raw) <= 10.0f && fabsf(pnd->navdata_vision_raw.vision_ty_raw) <= 10.0f )
 244     {
 245       ihm_dir = HOVER_VAL;
 246     }
 247     else
 248     {
 249       ihm_dir = (double) atan2f(pnd->navdata_vision_raw.vision_ty_raw, pnd->navdata_vision_raw.vision_tx_raw);
 250     }
 251   }
 252   else
 253   {
 254     ihm_dir = NOT_DEF_VAL;
 255   }
 256
 257   //Vision States' display update for IHM
 258   vp_os_memset( label_vision_state_value, '\0', sizeof(label_vision_state_value) );
 259   sprintf(label_vision_state_value, "%08x", pnd->navdata_vision.vision_state);
 260
 261   ihm_CA[KIHM_CURVE_VX].tval[1][ihm_val_idx] = (double) (pnd->navdata_demo.vx);      // Fusion Vx
 262   ihm_CA[KIHM_CURVE_VY].tval[1][ihm_val_idx] = (double) (pnd->navdata_demo.vy);      // Fusion Vy
 263
 264 #ifdef PC_USE_VISION
 265   draw_trackers_cfg.num_points = NUM_MAX_SCREEN_POINTS;
 266   vp_os_memset(&draw_trackers_cfg.locked[0], 0, NUM_MAX_SCREEN_POINTS * sizeof(uint32_t));
 267   vp_os_memcpy(&draw_trackers_cfg.locked[0], pnd->navdata_trackers_send.locked, NUM_MAX_SCREEN_POINTS * sizeof(uint32_t));
 268   vp_os_memset(&draw_trackers_cfg.points[0], 0, NUM_MAX_SCREEN_POINTS * sizeof(screen_point_t));
 269   vp_os_memcpy(&draw_trackers_cfg.points[0], pnd->navdata_trackers_send.point, NUM_MAX_SCREEN_POINTS * sizeof(screen_point_t));
 270
 271   int i;
 272   draw_trackers_cfg.detected = pnd->navdata_vision_detect.nb_detected;
 273   for(i = 0 ; i < pnd->navdata_vision_detect.nb_detected ; i++)
 274   {
 275     draw_trackers_cfg.type[i]           = pnd->navdata_vision_detect.type[i];
 276     draw_trackers_cfg.patch_center[i].x = pnd->navdata_vision_detect.xc[i];
 277     draw_trackers_cfg.patch_center[i].y = pnd->navdata_vision_detect.yc[i];
 278     draw_trackers_cfg.width[i]          = pnd->navdata_vision_detect.width[i];
 279     draw_trackers_cfg.height[i]         = pnd->navdata_vision_detect.height[i];
 280   }
 281   set_draw_trackers_config(&draw_trackers_cfg);
 282 #endif
 283
 284   /*Stephane*/
 285
 286           extern char label_detection_state_value[32];
 287           switch(pnd->navdata_demo.detection_camera_type)
 288           {
 289                   case CAD_TYPE_NONE:  snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Detection disabled"); break;
 290                   case CAD_TYPE_VISION:   snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Searching 2D tags"); break;
 291                   case CAD_TYPE_COCARDE:  snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Searching roundels"); break;
 292                   case CAD_TYPE_ORIENTED_COCARDE:  snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Searching roundels"); break;
 293                   case CAD_TYPE_H_COCARDE:  snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Searching H roundels"); break;
 294                   case CAD_TYPE_H_ORIENTED_COCARDE:  snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Searching H roundels"); break;
 295                   case CAD_TYPE_STRIPE:  snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Searching ground stripe (15Hz)"); break;
 296                   case CAD_TYPE_STRIPE_V:  snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Searching ground stripe (60Hz)"); break;
 297                   default : snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Navdata error");
 298           }
 299
 300           char nbDetectedTags_label_buffer[1024];
 301
 302           if (pndvision->nb_detected==1 && ( pnd->navdata_demo.detection_camera_type==CAD_TYPE_ORIENTED_COCARDE ||  pnd->navdata_demo.detection_camera_type==CAD_TYPE_H_ORIENTED_COCARDE ) )
 303           {  snprintf(nbDetectedTags_label_buffer,sizeof(nbDetectedTags_label_buffer),"Found : %i (%4.2f deg)",pndvision->nb_detected,pndvision->orientation_angle[0]); }
 304           else if (pndvision->nb_detected==1 && (pnd->navdata_demo.detection_camera_type==CAD_TYPE_STRIPE || pnd->navdata_demo.detection_camera_type==CAD_TYPE_STRIPE_V))
 305           {  snprintf(nbDetectedTags_label_buffer,sizeof(nbDetectedTags_label_buffer),"Found : %i (%4.2f deg - w: %i)",
 306                           pndvision->nb_detected,
 307                           pndvision->orientation_angle[0],
 308                           pndvision->width[0]);
 309           }
 310           else
 311           {  snprintf(nbDetectedTags_label_buffer,sizeof(nbDetectedTags_label_buffer),"Found : %i",pndvision->nb_detected);       }
 312
 313           if(nbDetectedTags_label!=NULL)
 314                   gtk_label_set_text((GtkLabel*)nbDetectedTags_label,(const gchar*)nbDetectedTags_label_buffer);
 315
 316
 317
 318
 319
 320   gdk_threads_leave();
 321
 322   return C_OK;
 323 }
 324
 325 C_RESULT navdata_ihm_release( void )
 326 {
 327   return C_OK;
 328 }
 329