snippet_configure_drone.h
Go to the documentation of this file.
00001 #ifndef SNIPPET_CONFIGURE_DRONE_H
00002 #define SNIPPET_CONFIGURE_DRONE_H
00003 
00004 // This function will send the custom configuration to the drone
00005 // It doesn't work if sent during the SDK (which runs before the configuration profiles on the drone are setup)
00006 #undef ARDRONE_CONFIG_KEY_IMM_a10
00007 #undef ARDRONE_CONFIG_KEY_REF_a10
00008 #undef ARDRONE_CONFIG_KEY_STR_a10
00009 #undef LOAD_PARAM_STR
00010 #undef LOAD_PARAM_NUM
00011 
00012 #define SEND_PARAM_NUM(NAME,CATEGORY,DEFAULT)                                                          \
00013 {                                                                                                      \
00014     if(ardrone_application_default_config.NAME!=DEFAULT)                                               \
00015     {                                                                                                  \
00016         ROS_INFO("  SEND: "#CATEGORY"/"#NAME" = %f (DEFAULT = %f)", (float)ardrone_application_default_config.NAME, (float)DEFAULT);           \
00017         ARDRONE_TOOL_CONFIGURATION_ADDEVENT (NAME, &ardrone_application_default_config.NAME, NULL);    \
00018     }                                                                                                  \
00019 }
00020 
00021 #define SEND_PARAM_STR(NAME,CATEGORY,DEFAULT)                                                          \
00022 {                                                                                                      \
00023     if(0!=strcmp(ardrone_application_default_config.NAME,DEFAULT))                                     \
00024     {                                                                                                  \
00025         ROS_INFO("SEND: "#CATEGORY"/"#NAME" = %s (DEFAULT = %s)", ardrone_application_default_config.NAME, DEFAULT);           \
00026         ARDRONE_TOOL_CONFIGURATION_ADDEVENT (NAME, ardrone_application_default_config.NAME, NULL);     \
00027     }                                                                                                  \
00028 }
00029 
00030 // firstly we send the CAT_COMMON parameters, for example outdoor, as these settings can affect where the other parameters are stored
00031 #define ARDRONE_CONFIG_KEY_REF_a10(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, RW_CUSTOM, DEFAULT, CALLBACK, CATEGORY) //do nothing for reference-only parameters
00032 #define ARDRONE_CONFIG_KEY_IMM_a10(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, RW_CUSTOM, DEFAULT, CALLBACK, CATEGORY) { if(0!=strcmp(KEY,"custom") && CATEGORY==CAT_COMMON && ((RW & K_WRITE) != 0 || (RW_CUSTOM & K_WRITE) != 0)) SEND_PARAM_NUM(NAME,CATEGORY,DEFAULT) } // parameters under the custom key are for control of application/user/session, we don't want to change these!
00033 #define ARDRONE_CONFIG_KEY_STR_a10(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, RW_CUSTOM, DEFAULT, CALLBACK, CATEGORY) { if(0!=strcmp(KEY,"custom") && CATEGORY==CAT_COMMON && ((RW & K_WRITE) != 0 || (RW_CUSTOM & K_WRITE) != 0)) SEND_PARAM_STR(NAME,CATEGORY,DEFAULT) }
00034 
00035 #include <config_keys.h> // include the parameter definitions, which will be replaced by the above
00036 
00037 #undef ARDRONE_CONFIG_KEY_IMM_a10
00038 #undef ARDRONE_CONFIG_KEY_STR_a10
00039 
00040 // then we send the rest of the parameters. The problem is if we send euler_angle_max (for example) before sending outdoor, it will get written to the wrong parameter
00041 // (indoor_ not outdoor_euler_angle_max) and then will be overwritten by the default when changing state from indoor to outdoor, so we need to send common parameters first.
00042 #define ARDRONE_CONFIG_KEY_IMM_a10(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, RW_CUSTOM, DEFAULT, CALLBACK, CATEGORY) { if(0!=strcmp(KEY,"custom") && CATEGORY!=CAT_COMMON && ((RW & K_WRITE) != 0 || (RW_CUSTOM & K_WRITE) != 0)) SEND_PARAM_NUM(NAME,CATEGORY,DEFAULT) } // parameters under the custom key are for control of application/user/session, we don't want to change these!
00043 #define ARDRONE_CONFIG_KEY_STR_a10(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, RW_CUSTOM, DEFAULT, CALLBACK, CATEGORY) { if(0!=strcmp(KEY,"custom") && CATEGORY!=CAT_COMMON && ((RW & K_WRITE) != 0 || (RW_CUSTOM & K_WRITE) != 0)) SEND_PARAM_STR(NAME,CATEGORY,DEFAULT) }
00044 
00045 #include <config_keys.h> // include the parameter definitions, which will be replaced by the above
00046 
00047 #undef SEND_PARAM_NUM
00048 #undef SEND_PARAM_STR
00049 #undef ARDRONE_CONFIG_KEY_IMM_a10
00050 #undef ARDRONE_CONFIG_KEY_REF_a10
00051 #undef ARDRONE_CONFIG_KEY_STR_a10
00052 
00053 
00054 #define NAVDATA_STRUCTS_INITIALIZE
00055 #include <ardrone_autonomy/NavdataMessageDefinitions.h>
00056 #undef NAVDATA_STRUCTS_INITIALIZE
00057 
00058 #endif // SNIPPET_CONFIGURE_DRONE_H


ardrone_autonomy
Author(s): Mani Monajjemi, Mani Monajjemi
autogenerated on Fri Dec 9 2016 03:36:59