1 /********************************************************************
2 * COPYRIGHT PARROT 2010
3 ********************************************************************
4 * PARROT - A.R.Drone SDK Windows Client Example
5 *-----------------------------------------------------------------*/
8 * @brief Navdata handling code
10 * @author sylvain.gaeremynck@parrot.com
13 * @author Stephane Piskorski <stephane.piskorski.ext@parrot.fr>
16 *******************************************************************/
19 /* Includes required to handle navigation data */
20 #include <ardrone_tool/Navdata/ardrone_navdata_client.h>
21 #include <Navdata/navdata.h>
23 #include <custom_code.h>
29 /*---------------------------------------------------------------------------------------------------------------------
30 Function taking the drone control state stored as an integer, and prints in a string
31 the names of the set control bits.
33 The drone control state is an integer sent in the navdata which says if the drone is landed, flying,
34 hovering, taking-off, crashed, etc ...
35 ---------------------------------------------------------------------------------------------------------------------*/
37 #define CTRL_STATES_STRING
38 #include "control_states.h"
40 const char* ctrl_state_str(uint32_t ctrl_state)
42 #define MAX_STR_CTRL_STATE 256
43 static char str_ctrl_state[MAX_STR_CTRL_STATE];
45 ctrl_string_t* ctrl_string;
46 uint32_t major = ctrl_state >> 16;
47 uint32_t minor = ctrl_state & 0xFFFF;
49 if( strlen(ctrl_states[major]) < MAX_STR_CTRL_STATE )
51 vp_os_memset(str_ctrl_state, 0, sizeof(str_ctrl_state));
53 strcat_s(str_ctrl_state, sizeof(str_ctrl_state),ctrl_states[major]);
54 ctrl_string = control_states_link[major];
56 if( ctrl_string != NULL && (strlen(ctrl_states[major]) + strlen(ctrl_string[minor]) < MAX_STR_CTRL_STATE) )
58 strcat_s( str_ctrl_state,sizeof(str_ctrl_state), " | " );
59 strcat_s( str_ctrl_state, sizeof(str_ctrl_state),ctrl_string[minor] );
64 vp_os_memset( str_ctrl_state, '#', sizeof(str_ctrl_state) );
67 return str_ctrl_state;
72 /*---------------------------------------------------------------------------------------------------------------------
73 Initialization local variables before event loop
74 ---------------------------------------------------------------------------------------------------------------------*/
75 inline C_RESULT demo_navdata_client_init( void* data )
77 /** ======= INSERT USER CODE HERE ========== **/
78 /* Initialize your navdata handler */
79 /** ======= INSERT USER CODE HERE ========== **/
88 /*---------------------------------------------------------------------------------------------------------------------
89 Navdata handling function, which is called every time navdata are received
\r
90 ---------------------------------------------------------------------------------------------------------------------*/
91 inline C_RESULT demo_navdata_client_process( const navdata_unpacked_t* const navdata )
\r
95 const navdata_demo_t* const nd = &navdata->navdata_demo;
\r
98 /** ======= INSERT USER CODE HERE ========== **/
\r
99 ARWin32Demo_AcquireConsole();
\r
100 if ((cpt++)==90) { system("cls"); cpt=0; }
\r
102 ARWin32Demo_SetConsoleCursor(0,0); // Print at the top of the console
\r
103 printf("=================================\n");
\r
104 printf("Navdata for flight demonstrations\n");
\r
105 printf("=================================\n");
\r
107 printf("Control state : %s \n",ctrl_state_str(nd->ctrl_state));
\r
108 printf("Battery level : %i/100 \n",nd->vbat_flying_percentage);
\r
109 printf("Orientation : [Theta] %4.3f [Phi] %4.3f [Psi] %4.3f \n",nd->theta,nd->phi,nd->psi);
\r
110 printf("Altitude : %i \n",nd->altitude);
\r
111 printf("Speed : [vX] %4.3f [vY] %4.3f [vZ] %4.3f \n",nd->vx,nd->vy,nd->vz);
\r
112 ARWin32Demo_ReleaseConsole();
\r
114 /** ======= INSERT USER CODE HERE ========== **/
\r
124 /*---------------------------------------------------------------------------------------------------------------------
125 Relinquish the local resources after the event loop exit
126 ---------------------------------------------------------------------------------------------------------------------*/
127 inline C_RESULT demo_navdata_client_release( void )
129 /** ======= INSERT USER CODE HERE ========== **/
130 /* Clean up your navdata handler */
131 /** ======= INSERT USER CODE HERE ========== **/
140 Registering the navdata handling function to 'navdata client' which is part
142 You can add as many navdata handlers as you want.
143 Terminate the table with a NULL pointer.
145 BEGIN_NAVDATA_HANDLER_TABLE
146 NAVDATA_HANDLER_TABLE_ENTRY(demo_navdata_client_init, demo_navdata_client_process, demo_navdata_client_release, NULL)
147 END_NAVDATA_HANDLER_TABLE