git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/Examples/Win32/sdk_demo/Sources/Navdata/navdata.c

   1 /********************************************************************
   2  *                    COPYRIGHT PARROT 2010
   3  ********************************************************************
   4  *       PARROT - A.R.Drone SDK Windows Client Example
   5  *-----------------------------------------------------------------*/
   6 /**
   7  * @file navdata.c
   8  * @brief Navdata handling code
   9  *
  10  * @author sylvain.gaeremynck@parrot.com
  11  * @date 2009/07/01
  12  *
  13  * @author Stephane Piskorski <stephane.piskorski.ext@parrot.fr>
  14  * @date   Sept, 8. 2010
  15  *
  16  *******************************************************************/
  17
  18
  19 /* Includes required to handle navigation data */
  20         #include <ardrone_tool/Navdata/ardrone_navdata_client.h>
  21         #include <Navdata/navdata.h>
  22
  23 #include <custom_code.h>
  24
  25
  26
  27
  28
  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.
  32
  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 ---------------------------------------------------------------------------------------------------------------------*/
  36
  37         #define CTRL_STATES_STRING
  38         #include "control_states.h"
  39
  40 const char* ctrl_state_str(uint32_t ctrl_state)
  41 {
  42   #define MAX_STR_CTRL_STATE 256
  43   static char str_ctrl_state[MAX_STR_CTRL_STATE];
  44
  45   ctrl_string_t* ctrl_string;
  46   uint32_t major = ctrl_state >> 16;
  47   uint32_t minor = ctrl_state & 0xFFFF;
  48
  49   if( strlen(ctrl_states[major]) < MAX_STR_CTRL_STATE )
  50   {
  51     vp_os_memset(str_ctrl_state, 0, sizeof(str_ctrl_state));
  52
  53     strcat_s(str_ctrl_state, sizeof(str_ctrl_state),ctrl_states[major]);
  54     ctrl_string = control_states_link[major];
  55
  56     if( ctrl_string != NULL && (strlen(ctrl_states[major]) + strlen(ctrl_string[minor]) < MAX_STR_CTRL_STATE) )
  57     {
  58       strcat_s( str_ctrl_state,sizeof(str_ctrl_state), " | " );
  59       strcat_s( str_ctrl_state, sizeof(str_ctrl_state),ctrl_string[minor] );
  60     }
  61   }
  62   else
  63   {
  64     vp_os_memset( str_ctrl_state, '#', sizeof(str_ctrl_state) );
  65   }
  66
  67   return str_ctrl_state;
  68 }
  69
  70
  71
  72 /*---------------------------------------------------------------------------------------------------------------------
  73 Initialization local variables before event loop
  74 ---------------------------------------------------------------------------------------------------------------------*/
  75 inline C_RESULT demo_navdata_client_init( void* data )
  76 {
  77         /**     ======= INSERT USER CODE HERE ========== **/
  78         /* Initialize your navdata handler */
  79         /**     ======= INSERT USER CODE HERE ========== **/
  80
  81   return C_OK;
  82 }
  83
  84
  85
  86
  87
  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
  92 {\r
  93         static int cpt=0;\r
  94 \r
  95     const navdata_demo_t* const nd = &navdata->navdata_demo;\r
  96 \r
  97 \r
  98                 /**     ======= INSERT USER CODE HERE ========== **/\r
  99                                 ARWin32Demo_AcquireConsole();\r
 100                                 if ((cpt++)==90) { system("cls"); cpt=0; }\r
 101                                 \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
 106 \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
 113 \r
 114                 /** ======= INSERT USER CODE HERE ========== **/\r
 115 \r
 116                 return C_OK;\r
 117 }\r
 118
 119
 120
 121
 122
 123
 124 /*---------------------------------------------------------------------------------------------------------------------
 125  Relinquish the local resources after the event loop exit
 126 ---------------------------------------------------------------------------------------------------------------------*/
 127 inline C_RESULT demo_navdata_client_release( void )
 128 {
 129         /**     ======= INSERT USER CODE HERE ========== **/
 130         /* Clean up your navdata handler */
 131         /**     ======= INSERT USER CODE HERE ========== **/
 132   return C_OK;
 133 }
 134
 135
 136
 137
 138
 139 /*
 140 Registering the navdata handling function to 'navdata client' which is part
 141 of the ARDroneTool.
 142 You can add as many navdata handlers as you want.
 143 Terminate the table with a NULL pointer.
 144 */
 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
 148