git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/Soft/Lib/ardrone_tool/Navdata/ardrone_general_navdata.c

   1 #include <stdio.h>
   2 #include <sys/time.h>
   3 #include <ardrone_api.h>
   4 #include <ardrone_tool/ardrone_tool.h>
   5 #include <ardrone_tool/ardrone_tool_configuration.h>
   6 #include <ardrone_tool/Navdata/ardrone_navdata_client.h>
   7 #include <ardrone_tool/Navdata/ardrone_general_navdata.h>
   8 #include <utils/ardrone_gen_ids.h>
   9
  10 static MULTICONFIG_STATE configState;
  11 static NAVDATA_REQUEST_STATE navdataState;
  12 static int appSwitch = 0;
  13 static int usrSwitch = 0;
  14 static int sesSwitch = 0;
  15 static int navdataNeeded = 0;
  16 static int droneSupportsMulticonfig = 0;
  17
  18 #define __MULTICONFIGURATION_MIN_VERSION_MAJOR__        (1U)
  19 #define __MULTICONFIGURATION_MIN_VERSION_MINOR__        (6U)
  20 #define __MULTICONFIGURATION_MIN_VERSION_REVISION__ (0U)
  21 #define __MULTICONFIGURATION_MIN_VERSION__ ((__MULTICONFIGURATION_MIN_VERSION_MAJOR__ << 16) | (__MULTICONFIGURATION_MIN_VERSION_MINOR__ << 8) | (__MULTICONFIGURATION_MIN_VERSION_REVISION__))
  22 static inline int versionSupportsMulticonfiguration (const char *currentString)
  23 {
  24         uint32_t currentMajor = 0, currentMinor = 0, currentRevision = 0;
  25         uint32_t currentVersion;
  26         sscanf (currentString, "%d.%d.%d", &currentMajor, &currentMinor, &currentRevision);
  27         currentVersion = ((currentMajor << 16) | (currentMinor << 8) | (currentRevision));
  28         return currentVersion >= __MULTICONFIGURATION_MIN_VERSION__;
  29 }
  30
  31
  32 #define _GENERAL_NAVDATA_DEBUG (0)
  33 #define _GENERAL_NAVDATA_DEBUG_PREFIX "General Navdata : "
  34 #if _GENERAL_NAVDATA_DEBUG
  35 #define PRINTDBG(...)                                                                                   \
  36 do                                                                                                                              \
  37 {                                                                                                                               \
  38         printf ("[%d] %s", __LINE__, _GENERAL_NAVDATA_DEBUG_PREFIX);\
  39         printf (__VA_ARGS__);                                                                           \
  40 } while (0)
  41 #else
  42 #define PRINTDBG(...)
  43 #endif
  44
  45 static void switchToSession(void)
  46 {
  47         ARDRONE_TOOL_CONFIGURATION_ADDEVENT (session_id, ses_id, NULL);
  48 }
  49
  50 static void switchToUser(void)
  51 {
  52     ARDRONE_TOOL_CONFIGURATION_ADDEVENT (profile_id, usr_id, NULL);
  53 }
  54
  55 static void switchToApplication(void)
  56 {
  57     ARDRONE_TOOL_CONFIGURATION_ADDEVENT (application_id , app_id, NULL);
  58 }
  59
  60 void configurationCallback (int res)
  61 {
  62         PRINTDBG ("Config callback called with result %d\n", res);
  63         if (0 != res)
  64         {
  65                 PRINTDBG ("State : %d\n", configState);
  66                 switch (configState)
  67                 {
  68                         case MULTICONFIG_IN_PROGRESS_VERSION: // got config version
  69                                 PRINTDBG ("Got config file\n");
  70                                 configState = MULTICONFIG_GOT_DRONE_VERSION;
  71                                 break;
  72                         case MULTICONFIG_IN_PROGRESS_LIST: // got multiconfig ids
  73                                 PRINTDBG ("Got ids list\n");
  74                                 configState = MULTICONFIG_GOT_IDS_LIST;
  75                                 break;
  76             case MULTICONFIG_IN_PROGRESS_IDS:
  77                 PRINTDBG ("Got current ids\n");
  78                 configState = MULTICONFIG_GOT_CURRENT_IDS;
  79                 break;
  80                         case MULTICONFIG_IDLE:
  81                         case MULTICONFIG_GOT_IDS_LIST:
  82                         case MULTICONFIG_GOT_DRONE_VERSION:
  83             case MULTICONFIG_GOT_CURRENT_IDS:
  84                         case MULTICONFIG_NEEDED:
  85             case MULTICONFIG_REQUEST_NAVDATA:
  86                         default:
  87                                 break;
  88                 }
  89         }
  90         PRINTDBG ("End of config callback call\n");
  91 }
  92
  93 void navdataCallback (int res)
  94 {
  95     PRINTDBG ("Navdata callback called with result %d\n", res);
  96         if (0 != res)
  97         {
  98                 PRINTDBG ("State : %d\n", navdataState);
  99                 switch (navdataState)
 100                 {
 101                         case NAVDATA_REQUEST_IN_PROGRESS: // Navdata request got acknowledged by the AR.Drone
 102                 navdataState = NAVDATA_REQUEST_IDLE;
 103                 break;
 104             case NAVDATA_REQUEST_IDLE:
 105             case NAVDATA_REQUEST_NEEDED:
 106                         default:
 107                                 break;
 108                 }
 109         }
 110         PRINTDBG ("End of navdata callback call\n");
 111 }
 112
 113 C_RESULT ardrone_general_navdata_init( void* data )
 114 {
 115         ARDRONE_TOOL_CONFIGURATION_ADDEVENT (session_id, "-all", NULL);
 116
 117     navdataState = NAVDATA_REQUEST_IDLE;
 118         configState = MULTICONFIG_NEEDED;
 119     appSwitch = 1;
 120     usrSwitch = 1;
 121     sesSwitch = 1;
 122     navdataNeeded = 1;
 123
 124         return C_OK;
 125 }
 126
 127 C_RESULT ardrone_general_navdata_process( const navdata_unpacked_t* const pnd )
 128 {
 129         navdata_mode_t current_navdata_state = NAVDATA_BOOTSTRAP;
 130
 131         /* Makes sure the navdata stream will be resumed if the drone is disconnected and reconnected.
 132          * Allows changing the drone battery during debugging sessions.  */
 133         if( ardrone_get_mask_from_state(pnd->ardrone_state, ARDRONE_NAVDATA_BOOTSTRAP) )
 134         {
 135                 current_navdata_state = NAVDATA_BOOTSTRAP;
 136         }
 137         else
 138         {
 139                 current_navdata_state = (ardrone_get_mask_from_state(pnd->ardrone_state, ARDRONE_NAVDATA_DEMO_MASK)) ? NAVDATA_DEMO : NAVDATA_FULL ;
 140         }
 141
 142         if (current_navdata_state == NAVDATA_BOOTSTRAP && configState == MULTICONFIG_IDLE && navdataState == NAVDATA_REQUEST_IDLE)
 143         {
 144         navdataState = NAVDATA_REQUEST_NEEDED;
 145         }
 146
 147         /* Multiconfig settings */
 148         int configIndex, userNeedInit, appNeedInit;
 149         userNeedInit = 0; appNeedInit = 0;
 150         switch (configState)
 151         {
 152                 case MULTICONFIG_GOT_DRONE_VERSION:
 153             PRINTDBG ("Checking drone version ...\n");
 154                         // Check if drone version is >= 1.6
 155                         if (versionSupportsMulticonfiguration (ardrone_control_config.num_version_soft))
 156                         {
 157                 PRINTDBG ("Drone supports multiconfig\n");
 158                                 configState = MULTICONFIG_IN_PROGRESS_LIST;
 159                                 ARDRONE_TOOL_CUSTOM_CONFIGURATION_GET (configurationCallback);
 160                 droneSupportsMulticonfig = 1;
 161                         }
 162                         else
 163                         {
 164                 PRINTDBG ("Drone does not support multiconfig\n");
 165                                 // Drone does not support multiconfig ... don't call init functions because we don't want to mess up things in default config
 166                                 configState = MULTICONFIG_REQUEST_NAVDATA;
 167                         }
 168                         break;
 169                 case MULTICONFIG_GOT_IDS_LIST:
 170                         // At this point, we're sure that the AR.Drone supports multiconfiguration, so we'll wheck if our ids exists, and send them.
 171             PRINTDBG ("Got AR.Drone ID list. Switch->%d,%d,%d. ND->%d\n", sesSwitch, usrSwitch, appSwitch, navdataNeeded);
 172             if (1 == sesSwitch)
 173             {
 174                 switchToSession(); // Go to session ...
 175             }
 176
 177             if (1 == appSwitch)
 178             {
 179                 if (0 != strcmp(ardrone_control_config_default.application_id, app_id)) // Check for application only if we're not asking for the default one
 180                 {
 181                     appNeedInit = 1;
 182                     for (configIndex = 0; configIndex < available_configurations[CAT_APPLI].nb_configurations; configIndex++) // Check all existing app_ids
 183                     {
 184                         PRINTDBG ("Checking application %s (desc : %s)\n", available_configurations[CAT_APPLI].list[configIndex].id,
 185                                   available_configurations[CAT_APPLI].list[configIndex].description);
 186                         if (0 == strcmp(available_configurations[CAT_APPLI].list[configIndex].id, app_id))
 187                         {
 188                             PRINTDBG ("Found our application ... should not init\n");
 189                             appNeedInit = 0;
 190                             break;
 191                         }
 192                     }
 193                     switchToApplication();
 194                 }
 195                 else
 196                 {
 197                     PRINTDBG ("We're requesting default application (%s), do nothing.\n", app_id);
 198                 }
 199             }
 200
 201             if (1 == usrSwitch)
 202             {
 203                 if (0 != strcmp(ardrone_control_config_default.profile_id, usr_id)) // Check for user only if we're not asking for the default one
 204                 {
 205                     userNeedInit = 1;
 206                     for (configIndex = 0; configIndex < available_configurations[CAT_USER].nb_configurations; configIndex++) // Check all existing user_ids
 207                     {
 208                         PRINTDBG ("Checking user %s (desc : %s)\n", available_configurations[CAT_USER].list[configIndex].id,
 209                                   available_configurations[CAT_USER].list[configIndex].description);
 210                         if (0 == strcmp(available_configurations[CAT_USER].list[configIndex].id, usr_id))
 211                         {
 212                             PRINTDBG ("Found our user ... should not init\n");
 213                             userNeedInit = 0;
 214                             break;
 215                         }
 216                     }
 217                     switchToUser();
 218                 }
 219                 else
 220                 {
 221                     PRINTDBG ("We're requesting default user (%s), do nothing.\n", usr_id);
 222                 }
 223             }
 224
 225                         if (1 == appNeedInit)
 226                         {
 227                                 // Send application defined default values
 228                                 ardrone_tool_send_application_default();
 229                                 PRINTDBG ("Creating app. profile on AR.Drone\n");
 230                                 ARDRONE_TOOL_CONFIGURATION_ADDEVENT (application_desc, app_name, NULL);
 231                         }
 232                         if (1 == userNeedInit)
 233                         {
 234                                 // Send user defined default values
 235                                 ardrone_tool_send_user_default();
 236                                 PRINTDBG ("Creating usr. profile on AR.Drone\n");
 237                                 ARDRONE_TOOL_CONFIGURATION_ADDEVENT (profile_desc, usr_name, NULL);
 238                         }
 239             if (1 == sesSwitch)
 240             {
 241                 if (0 != strcmp(ardrone_control_config_default.session_id, ses_id)) // Send session info only if session is not the default one
 242                 {
 243                     ARDRONE_TOOL_CONFIGURATION_ADDEVENT (session_desc, ses_name, NULL);
 244                     // Send session specific default values
 245                     ardrone_tool_send_session_default();
 246                 }
 247                 else
 248                 {
 249                     PRINTDBG ("We're requesting default session (%s), do nothing.\n", ses_id);
 250                 }
 251             }
 252             configState = MULTICONFIG_IN_PROGRESS_IDS;
 253             ARDRONE_TOOL_CONFIGURATION_GET (configurationCallback);
 254
 255         case MULTICONFIG_GOT_CURRENT_IDS:
 256             if (0 != strcmp(ardrone_control_config.session_id, ses_id) ||
 257                 0 != strcmp(ardrone_control_config.profile_id, usr_id) ||
 258                 0 != strcmp(ardrone_control_config.application_id, app_id))
 259             {
 260                 configState = MULTICONFIG_GOT_DRONE_VERSION; // We failed at setting up the application ids ... restart (but assume that drone supports multiconfig as we already checked)
 261             }
 262             else if (1 == navdataNeeded)
 263             {
 264                 configState = MULTICONFIG_REQUEST_NAVDATA;
 265             }
 266             else
 267             {
 268                 configState = MULTICONFIG_IDLE;
 269             }
 270                         break;
 271                 case MULTICONFIG_NEEDED:
 272             PRINTDBG ("Need to check multiconfig ... request config file\n");
 273                         // Get config file for reset
 274                         configState = MULTICONFIG_IN_PROGRESS_VERSION;
 275                         ARDRONE_TOOL_CONFIGURATION_GET (configurationCallback);
 276                         break;
 277         case MULTICONFIG_REQUEST_NAVDATA:
 278             PRINTDBG ("Send application navdata demo/options\n");
 279             // Send application navdata demo/options to start navdatas from AR.Drone
 280                         ARDRONE_TOOL_CONFIGURATION_ADDEVENT (navdata_demo, &ardrone_application_default_config.navdata_demo, NULL);
 281             if (TRUE == ardrone_application_default_config.navdata_demo)
 282             {   // Send navdata options only for navdata demo mode
 283                 ARDRONE_TOOL_CONFIGURATION_ADDEVENT (navdata_options, &ardrone_application_default_config.navdata_options, NULL);
 284             }
 285             configState = MULTICONFIG_IDLE;
 286             break;
 287                 case MULTICONFIG_IDLE:
 288                 case MULTICONFIG_IN_PROGRESS_LIST:
 289                 case MULTICONFIG_IN_PROGRESS_VERSION:
 290         case MULTICONFIG_IN_PROGRESS_IDS:
 291                 default:
 292                         break;
 293         }
 294
 295     /* Navdata request settings */
 296     switch (navdataState)
 297     {
 298         case NAVDATA_REQUEST_NEEDED:
 299             PRINTDBG ("Resetting navdatas to %s\n", (0 == ardrone_application_default_config.navdata_demo) ? "full" : "demo");
 300             navdataState = NAVDATA_REQUEST_IN_PROGRESS;
 301             switchToSession(); // Resend session id when reconnecting.
 302                         ARDRONE_TOOL_CONFIGURATION_ADDEVENT(navdata_demo, &ardrone_application_default_config.navdata_demo, NULL);
 303             if (TRUE == ardrone_application_default_config.navdata_demo)
 304             {   // Send navdata options only for navdata demo mode
 305                 ARDRONE_TOOL_CONFIGURATION_ADDEVENT (navdata_options, &ardrone_application_default_config.navdata_options, navdataCallback);
 306             }
 307             break;
 308         case NAVDATA_REQUEST_IN_PROGRESS:
 309         case NAVDATA_REQUEST_IDLE:
 310         default:
 311             break;
 312     }
 313
 314         return C_OK;
 315 }
 316
 317 C_RESULT ardrone_general_navdata_release( void )
 318 {
 319         return C_OK;
 320 }
 321
 322 /* User switch/list functions */
 323
 324
 325 /* User functions */
 326 void ardrone_refresh_user_list(void)
 327 {
 328     if (1 == droneSupportsMulticonfig)
 329     {
 330         appSwitch = 0;
 331         usrSwitch = 0;
 332         sesSwitch = 0;
 333         navdataNeeded = 0;
 334         configState = MULTICONFIG_GOT_DRONE_VERSION;
 335     }
 336 }
 337
 338 void ardrone_switch_to_user(const char *new_user)
 339 {
 340     if (1 == droneSupportsMulticonfig)
 341     {
 342         ardrone_gen_usrid (new_user, usr_id, usr_name, USER_NAME_SIZE);
 343         appSwitch = 0;
 344         usrSwitch = 1;
 345         sesSwitch = 0;
 346         navdataNeeded = 0;
 347         configState = MULTICONFIG_GOT_DRONE_VERSION;
 348     }
 349 }
 350
 351 void ardrone_switch_to_user_id(const char *new_user_id)
 352 {
 353     if (1 == droneSupportsMulticonfig)
 354     {
 355         // We assume that the userlist is up to date
 356         int userExists = 0;
 357         int configIndex;
 358         for (configIndex = 0; configIndex < available_configurations[CAT_USER].nb_configurations; configIndex++) // Check all existing user_ids
 359         {
 360             if (0 == strcmp(available_configurations[CAT_USER].list[configIndex].id, new_user_id))
 361             {
 362                 userExists = 1;
 363                 break;
 364             }
 365         }
 366
 367         if (1 == userExists)
 368         {
 369             strncpy (usr_id, new_user_id, MULTICONFIG_ID_SIZE);
 370             appSwitch = 0;
 371             usrSwitch = 0;
 372             sesSwitch = 0;
 373             navdataNeeded = 0;
 374             configState = MULTICONFIG_GOT_DRONE_VERSION;
 375         }
 376     }
 377 }
 378
 379 ardrone_users_t *ardrone_get_user_list(void)
 380 {
 381     if (0 == droneSupportsMulticonfig)
 382     {
 383         return NULL;
 384     }
 385     ardrone_users_t *retVal = vp_os_malloc (sizeof (ardrone_users_t));
 386     if (NULL == retVal)
 387         return NULL;
 388
 389
 390     // Assume that userlist is up to date
 391     int validUserCount = 0; // User whose descriptions start with a dot ('.') are hidden users that may not be shown to the application user (e.g. default user for each iPhone, or user specific to a control mode
 392     int configIndex;
 393     for (configIndex = 0; configIndex < available_configurations[CAT_USER].nb_configurations; configIndex++) // Check all existing user_ids
 394     {
 395         if ('.' != available_configurations[CAT_USER].list[configIndex].description[0]) // Not an hidden user
 396         {
 397             validUserCount++;
 398             retVal->userList = vp_os_realloc (retVal->userList, validUserCount * sizeof (ardrone_user_t));
 399             if (NULL == retVal->userList)
 400             {
 401                 vp_os_free (retVal);
 402                 return NULL;
 403             }
 404             strncpy (retVal->userList[validUserCount-1].ident, available_configurations[CAT_USER].list[configIndex].id, MULTICONFIG_ID_SIZE);
 405             strncpy (retVal->userList[validUserCount-1].description, available_configurations[CAT_USER].list[configIndex].description, USER_NAME_SIZE);
 406         }
 407     }
 408     retVal->userCount = validUserCount;
 409     return retVal;
 410 }
 411
 412 void ardrone_free_user_list (ardrone_users_t **users)
 413 {
 414     if (NULL != *users)
 415     {
 416         if (NULL != (*users)->userList)
 417         {
 418             vp_os_free ((*users)->userList);
 419         }
 420         vp_os_free (*users);
 421         *users = NULL;
 422     }
 423 }