1 #include <ardrone_tool/Navdata/ardrone_navdata_client.h>
3 #include <Navdata/navdata.h>
7 #define CTRL_STATES_STRING
8 #include "control_states.h"
10 /* Initialization local variables before event loop */
11 inline C_RESULT demo_navdata_client_init( void* data )
16 #define MAX_STR_CTRL_STATE 2048
17 /*============================================================================*/
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.
23 const char* ctrl_state_str(uint32_t ctrl_state)
25 static char str_ctrl_state[MAX_STR_CTRL_STATE];
27 ctrl_string_t* ctrl_string;
28 uint32_t major = ctrl_state >> 16;
29 uint32_t minor = ctrl_state & 0xFFFF;
31 if( strlen(ctrl_states[major]) < MAX_STR_CTRL_STATE )
33 vp_os_memset(str_ctrl_state, 0, sizeof(str_ctrl_state));
35 strcat(str_ctrl_state, ctrl_states[major]);
36 ctrl_string = control_states_link[major];
38 if( ctrl_string != NULL && (strlen(ctrl_states[major]) + strlen(ctrl_string[minor]) < MAX_STR_CTRL_STATE) )
40 strcat( str_ctrl_state, " | " );
41 strcat( str_ctrl_state, ctrl_string[minor] );
46 vp_os_memset( str_ctrl_state, '#', sizeof(str_ctrl_state) );
49 return str_ctrl_state;
52 /* Receving navdata during the event loop */
53 inline C_RESULT demo_navdata_client_process( const navdata_unpacked_t* const navdata )
59 const navdata_demo_t*nd = &navdata->navdata_demo;
60 const navdata_vision_detect_t*nv = &navdata->navdata_vision_detect;
62 printf("==========================\nNavdata for flight demonstrations \n==========================\n");
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);
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);
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 );
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 );
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]);
101 cpt=system("clear"); // Avoid compiler warning (unused return value)
108 /* Relinquish the local resources after the event loop exit */
109 inline C_RESULT demo_navdata_client_release( void )
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