git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/Examples/Linux/Testbenches/ftp_test/Sources/Navdata/navdata.c

   1 #include <ardrone_tool/Navdata/ardrone_navdata_client.h>
   2
   3 #include <Navdata/navdata.h>
   4
   5 #include <stdio.h>
   6
   7 #define CTRL_STATES_STRING
   8 #include "control_states.h"
   9
  10 /* Initialization local variables before event loop  */
  11 inline C_RESULT demo_navdata_client_init( void* data )
  12 {
  13   return C_OK;
  14 }
  15
  16 #define MAX_STR_CTRL_STATE 2048
  17 /*============================================================================*/
  18 /**
  19   * Builds a string describing the drone control state.
  20   * @param ctrl_state Integer containing the control state from the unpacked navdata.
  21   * @return A pointer to the built string. The returned address always points to the last built string.
  22   */
  23 const char* ctrl_state_str(uint32_t ctrl_state)
  24 {
  25   static char str_ctrl_state[MAX_STR_CTRL_STATE];
  26
  27   ctrl_string_t* ctrl_string;
  28   uint32_t major = ctrl_state >> 16;
  29   uint32_t minor = ctrl_state & 0xFFFF;
  30
  31   if( strlen(ctrl_states[major]) < MAX_STR_CTRL_STATE )
  32   {
  33     vp_os_memset(str_ctrl_state, 0, sizeof(str_ctrl_state));
  34
  35     strcat(str_ctrl_state, ctrl_states[major]);
  36     ctrl_string = control_states_link[major];
  37
  38     if( ctrl_string != NULL && (strlen(ctrl_states[major]) + strlen(ctrl_string[minor]) < MAX_STR_CTRL_STATE) )
  39     {
  40       strcat( str_ctrl_state, " | " );
  41       strcat( str_ctrl_state, ctrl_string[minor] );
  42     }
  43   }
  44   else
  45   {
  46     vp_os_memset( str_ctrl_state, '#', sizeof(str_ctrl_state) );
  47   }
  48
  49   return str_ctrl_state;
  50 }
  51
  52 /* Receving navdata during the event loop */
  53 inline C_RESULT demo_navdata_client_process( const navdata_unpacked_t* const navdata )
  54 {
  55         static int cpt=0;
  56
  57         return C_OK;
  58
  59         const navdata_demo_t*nd = &navdata->navdata_demo;
  60         const navdata_vision_detect_t*nv = &navdata->navdata_vision_detect;
  61
  62         printf("==========================\nNavdata for flight demonstrations \n==========================\n");
  63
  64         printf("Control state : %s\n",ctrl_state_str(nd->ctrl_state));
  65         printf("Battery level : %i %%\n",nd->vbat_flying_percentage);
  66         printf("Orientation   : [Theta] %4.3f  [Phi] %4.3f  [Psi] %4.3f\n",nd->theta,nd->phi,nd->psi);
  67         printf("Altitude      : %i\n",nd->altitude);
  68         printf("Speed         : [vX] %4.3f  [vY] %4.3f  [vZPsi] %4.3f\n",nd->vx,nd->vy,nd->vz);
  69         printf("Position      : [x] %4.3f [Y] %4.3f [Z] %4.3f\n",
  70                                                         nd->drone_camera_trans.x,
  71                                                         nd->drone_camera_trans.y,
  72                                                         nd->drone_camera_trans.z);
  73
  74         printf("Detec. trans. : [x] %4.3f [Y] %4.3f [Z] %4.3f\n",
  75                                                                 nd->detection_camera_trans.x,
  76                                                                 nd->detection_camera_trans.y,
  77                                                                 nd->detection_camera_trans.z);
  78
  79         printf("Orientation   :\n%4.3f | %4.3f | %4.3f\n%4.3f | %4.3f | %4.3f\n%4.3f | %4.3f | %4.3f\n",
  80                                                                 nd->drone_camera_rot.m11,nd->drone_camera_rot.m12,nd->drone_camera_rot.m13,
  81                                                                 nd->drone_camera_rot.m21,nd->drone_camera_rot.m22,nd->drone_camera_rot.m23,
  82                                                                 nd->drone_camera_rot.m31,nd->drone_camera_rot.m32,nd->drone_camera_rot.m33 );
  83
  84         printf("Detec. rot.   :\n%4.3f | %4.3f | %4.3f\n%4.3f | %4.3f | %4.3f\n%4.3f | %4.3f | %4.3f\n",
  85                                                                 nd->detection_camera_rot.m11,nd->detection_camera_rot.m12,nd->detection_camera_rot.m13,
  86                                                                 nd->detection_camera_rot.m21,nd->detection_camera_rot.m22,nd->detection_camera_rot.m23,
  87                                                                 nd->detection_camera_rot.m31,nd->detection_camera_rot.m32,nd->detection_camera_rot.m33 );
  88
  89
  90         printf("=====================\nNavdata for detection \n=====================\n");
  91         printf("Type     :  %i\n",nd->detection_camera_type);
  92         printf("Tag index:  %i\n",nd->detection_tag_index);
  93         printf("Targets  :  %i\n",nv->nb_detected);
  94         printf("Target 1 :  [X] %4i    [Y] %4i\n",nv->xc[0],nv->yc[0]);
  95         printf("Target 2 :  [X] %4i    [Y] %4i\n",nv->xc[1],nv->yc[1]);
  96
  97         printf("\033[27A");
  98
  99         if ((cpt++)==100)
 100           {
 101             cpt=system("clear"); // Avoid compiler warning (unused return value)
 102             cpt=0;
 103           }
 104
 105   return C_OK;
 106 }
 107
 108 /* Relinquish the local resources after the event loop exit */
 109 inline C_RESULT demo_navdata_client_release( void )
 110 {
 111   return C_OK;
 112 }
 113
 114 /* Registering to navdata client */
 115 BEGIN_NAVDATA_HANDLER_TABLE
 116   NAVDATA_HANDLER_TABLE_ENTRY(demo_navdata_client_init, demo_navdata_client_process, demo_navdata_client_release, NULL)
 117 END_NAVDATA_HANDLER_TABLE
 118