vcs.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/VP_SDK/Examples/common/atcodec_server.c

   1
   2 #include <stdio.h>
   3 #include <stdlib.h>
   4 #include <string.h>
   5
   6 #ifndef AT_MESSAGES_HEADER
   7 # define AT_MESSAGES_HEADER <ATcodec/ATcodec_Messages_ex.h>
   8 #endif // > AT_MESSAGES_HEADER
   9
  10
  11 #include <ATcodec/ATcodec_api.h>
  12 #include <VP_Os/vp_os_types.h>
  13 #include <VP_Com/vp_com.h>
  14 #include <VP_Api/vp_api_error.h>
  15 #include <VP_Os/vp_os_signal.h>
  16 #include <VP_Os/vp_os_delay.h>
  17 #include <VP_Os/vp_os_thread.h>
  18 #include <VP_Os/vp_os_print.h>
  19 #include <VP_Os/vp_os_malloc.h>
  20 #include <Examples/common/common.h>
  21 #include <VP_Stages/vp_stages_io_com.h>
  22
  23
  24 #ifndef AT_MESSAGES_HEADER
  25 #error You need to define AT_MESSAGES_HEADER
  26 #endif
  27
  28
  29 typedef struct _AT_CODEC_MSG_IDS_
  30 {
  31 # define ATCODEC_DEFINE_AT_CMD(ID,Str,From,Cb,Prio) AT_CODEC_MSG_ID ID;
  32 # define ATCODEC_DEFINE_AT_RESU(ID,Str,From,Cb) AT_CODEC_MSG_ID ID;
  33 # include AT_MESSAGES_HEADER
  34 }
  35 AT_CODEC_MSG_IDS;
  36
  37
  38 static AT_CODEC_MSG_IDS ids;
  39
  40
  41 static vp_stages_output_com_config_t occ;
  42
  43 static Write atcodec_write;
  44 static Read atcodec_read;
  45
  46 static vp_com_t com;
  47
  48 #ifdef NO_COM
  49   static vp_com_serial_config_t    config;
  50 #else // ! NO_COM
  51 #ifdef USE_WIFI
  52   vp_com_wifi_connection_t         connection;
  53   vp_com_wifi_config_t             config;
  54 #endif // ! USE_WIFI
  55 #endif // > NO_COM
  56
  57
  58
  59 AT_CODEC_ERROR_CODE at_pm_qw(ATcodec_Memory_t *mem, ATcodec_Memory_t *output, int *id)
  60 {
  61   static int foo = 0;
  62   int motor = ATcodec_Memory_Raw_Get_Int(mem);
  63
  64   PRINT("Motor selected : %d\n", motor);
  65
  66   if(foo)
  67     *id = ids.AT_MSG_ATRESU_OK;
  68   else
  69     *id = ids.AT_MSG_ATRESU_ERROR;
  70
  71   foo = 1-foo;
  72
  73   return AT_CODEC_GENERAL_OK;
  74 }
  75
  76 AT_CODEC_ERROR_CODE at_pm_exe(ATcodec_Memory_t *mem, ATcodec_Memory_t *output, int *id)
  77 {
  78   static int foo = 1;
  79   int speed = ATcodec_Memory_Raw_Get_Int(mem);
  80   int timestamp = ATcodec_Memory_Raw_Get_Int(mem);
  81   PRINT("Motor command : speed=%d , timestamp=%d\n", speed, timestamp);
  82
  83   if(foo)
  84     *id = ids.AT_MSG_ATRESU_OK;
  85   else
  86     *id = ids.AT_MSG_ATRESU_ERROR;
  87
  88   foo = 1-foo;
  89
  90   return AT_CODEC_GENERAL_OK;
  91 }
  92
  93 AT_CODEC_ERROR_CODE at_cgmi(ATcodec_Memory_t *mem, ATcodec_Memory_t *output, int *id)
  94 {
  95   PRINT("CGMI command\n");
  96
  97   ATcodec_Memory_Put_String(output, "By Aurelien Morelle");
  98
  99   *id = ids.AT_MSG_ATRESU_CGMI;
 100
 101   return AT_CODEC_GENERAL_OK;
 102 }
 103
 104
 105 AT_CODEC_ERROR_CODE AT_CODEC_init(void)
 106 {
 107   vp_os_memset(&com, 0, sizeof(vp_com_t));
 108
 109   occ.com = &com;
 110
 111 # undef ATCODEC_DEFINE_AT_CMD
 112 # define ATCODEC_DEFINE_AT_CMD(ID,Str,From,Cb,Prio) \
 113     if((ids.ID = ATcodec_Add_Hashed_Message(Str,From,Cb,Prio)) == -1) \
 114       { \
 115         PRINT("Error Add_Hashed \"%s\" library\n", Str); \
 116         return AT_CODEC_INIT_ERROR; \
 117       }
 118
 119 # undef ATCODEC_DEFINE_AT_RESU
 120 # define ATCODEC_DEFINE_AT_RESU(ID,Str,From,Cb) \
 121     if((ids.ID = ATcodec_Add_Defined_Message(Str)) == -1) \
 122       { \
 123         PRINT("Error Add_Defined \"%s\" library\n", Str); \
 124         return AT_CODEC_INIT_ERROR; \
 125       }
 126
 127 # include AT_MESSAGES_HEADER
 128
 129   //ATcodec_Print_Tree();
 130
 131 #ifdef NO_COM
 132 #ifdef __linux__
 133   strcpy(config.itfName, "/dev/ttyS0");
 134 #else
 135   strcpy(config.itfName, "/dev/ser1");
 136 #endif
 137   config.initial_baudrate = VP_COM_BAUDRATE_115200;
 138   config.baudrate = VP_COM_BAUDRATE_115200;
 139   config.sync = 0; //1;
 140
 141   com.type                          = VP_COM_SERIAL;
 142
 143   occ.socket.type                   = VP_COM_SERVER;
 144   occ.config                        = (vp_com_config_t *)&config;
 145   occ.buffer_size                   = 16;
 146 #else // ! NO_COM
 147 #ifdef USE_WIFI
 148   vp_os_memset(&connection,0,sizeof(vp_com_connection_t));
 149   strcpy(connection.networkName,"linksys");
 150
 151   strcpy(config.itfName,    "wl0");
 152   strcpy(config.localHost,  "192.168.1.23");
 153   strcpy(config.netmask,    "255.255.255.0");
 154   strcpy(config.broadcast,  "192.168.1.255");
 155   strcpy(config.gateway,    "192.168.1.1");
 156   strcpy(config.server,     "192.168.1.1");
 157   strcpy(config.passkey,    "9F1C3EE11CBA230B27BF1C1B6F");
 158   config.infrastructure = 1;
 159   config.secure = 1;
 160
 161   com.type                          = VP_COM_WIFI;
 162
 163   occ.config                        = (vp_com_config_t*)&config;
 164   occ.connection                    = (vp_com_connection_t*)&connection;
 165   occ.socket.type                   = VP_COM_SERVER;
 166   occ.socket.protocol               = VP_COM_TCP;
 167   occ.socket.port                   = 5555;
 168   occ.buffer_size                   = 128;
 169 #endif // ! USE_WIFI
 170 #endif // > NO_COM
 171
 172
 173   if(FAILED(vp_com_init(occ.com)))
 174   {
 175     PRINT("VP_Com : Failed to init\n");
 176     vp_com_shutdown(occ.com);
 177     return AT_CODEC_OPEN_ERROR;
 178   }
 179
 180   if(FAILED(vp_com_local_config(occ.com, occ.config)))
 181   {
 182     PRINT("VP_Com : Failed to configure\n");
 183     vp_com_shutdown(occ.com);
 184     return AT_CODEC_OPEN_ERROR;
 185   }
 186
 187   PRINT("com_init OK\n");
 188
 189   return AT_CODEC_INIT_OK;
 190 }
 191
 192 AT_CODEC_ERROR_CODE AT_CODEC_shutdown(void)
 193 {
 194   ATcodec_Shutdown_Library();
 195
 196   return AT_CODEC_SHUTDOWN_OK;
 197 }
 198
 199 AT_CODEC_ERROR_CODE AT_CODEC_open(void)
 200 {
 201   PRINT("ATcodec_open\n");
 202
 203   if(FAILED(vp_com_open(occ.com, &occ.socket, &atcodec_read,&atcodec_write)))
 204     return AT_CODEC_OPEN_ERROR;
 205
 206   if(FAILED(vp_com_wait_connections(occ.com, &occ.socket, &occ.socket_client, 1)))
 207     return AT_CODEC_OPEN_ERROR;
 208
 209   PRINT("Connection RFCOMM incoming\n");
 210
 211   return AT_CODEC_OPEN_OK;
 212 }
 213
 214 AT_CODEC_ERROR_CODE AT_CODEC_close(void)
 215 {
 216   PRINT("ATcodec_close\n");
 217
 218   vp_com_close(occ.com, &occ.socket);
 219   vp_com_close(occ.com, &occ.socket_client);
 220
 221   PRINT("socket closed\n");
 222
 223   return AT_CODEC_CLOSE_OK;
 224 }
 225
 226 AT_CODEC_ERROR_CODE AT_CODEC_write(int8_t *buffer, int32_t *len)
 227 {
 228 /*   int i; */
 229
 230   if(FAILED(atcodec_write(&occ.socket_client, buffer, len)))
 231      return AT_CODEC_WRITE_ERROR;
 232
 233 /*   if(*len) */
 234 /*     { */
 235 /*       PRINT("ATcodec_write < "); */
 236 /*       for(i = 0 ; i < *len ; i++) */
 237 /*      { */
 238 /*        if(buffer[i] == '\r') */
 239 /*          PRINT("_CR_"); */
 240 /*        else if(buffer[i] == '\n') */
 241 /*          PRINT("_LF_"); */
 242 /*        else */
 243 /*          PRINT("%c", buffer[i]); */
 244 /*      } */
 245 /*       PRINT(" > (%d)\n", *len); */
 246 /*     } */
 247
 248   return AT_CODEC_WRITE_OK;
 249 }
 250
 251 AT_CODEC_ERROR_CODE AT_CODEC_read(int8_t *buffer, int32_t *len)
 252 {
 253 /*   int i; */
 254
 255   if(FAILED(atcodec_read(&occ.socket_client, buffer, len)))
 256      return AT_CODEC_READ_ERROR;
 257
 258   if(*len)
 259     {
 260       buffer[*len] = '\0';
 261 /*       PRINT("ATcodec_read < "); */
 262 /*       for(i = 0 ; i < *len ; i++) */
 263 /*      { */
 264 /*        if(buffer[i] == '\r') */
 265 /*          PRINT("_CR_"); */
 266 /*        else if(buffer[i] == '\n') */
 267 /*          PRINT("_LF_"); */
 268 /*        else if(buffer[i] == '\0') */
 269 /*          PRINT("_\\0_"); */
 270 /*        else */
 271 /*          PRINT("%c", buffer[i]); */
 272 /*      } */
 273 /*       PRINT(" > (%d)\n", *len); */
 274     }
 275
 276   return AT_CODEC_READ_OK;
 277 }