git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/Examples/Linux/Protocol/at_cmds.c

   1 /**
   2  * @file my_client.c
   3  * @author karl leplat
   4  * @date 2009/07/01
   5  */
   6 #include <sys/types.h>
   7 #include <sys/socket.h>
   8 #include <stdio.h>
   9 #include <netinet/in.h>
  10 #include <netdb.h>
  11 #include <errno.h>
  12 #include <arpa/inet.h>
  13 #include <pthread.h>
  14
  15 #include <stdio.h>
  16 #include <stdlib.h>
  17 #include <unistd.h>
  18 #include <string.h>
  19 #include <errno.h>
  20
  21 #include <sys/time.h>
  22
  23 #include <fcntl.h>
  24 #include <sys/ioctl.h>
  25 #include <net/if.h>
  26
  27 #include "app.h"
  28
  29 static pthread_mutex_t at_cmd_lock;
  30 static pthread_t at_thread = 0;
  31 static int32_t at_thread_alive = 1;
  32
  33 uint32_t mykonos_state = 0;
  34
  35 /* AT constant */
  36 #define AT_PORT                   5556
  37 #define AT_BUFFER_SIZE            1024
  38
  39 typedef enum {
  40     MYKONOS_UI_BIT_AG             = 0, /* Button turn to left */
  41     MYKONOS_UI_BIT_AB             = 1, /* Button altitude down (ah - ab)*/
  42     MYKONOS_UI_BIT_AD             = 2, /* Button turn to right */
  43     MYKONOS_UI_BIT_AH             = 3, /* Button altitude up (ah - ab)*/
  44     MYKONOS_UI_BIT_L1             = 4, /* Button - z-axis (r1 - l1) */
  45     MYKONOS_UI_BIT_R1             = 5, /* Not used */
  46     MYKONOS_UI_BIT_L2             = 6, /* Button + z-axis (r1 - l1) */
  47     MYKONOS_UI_BIT_R2             = 7, /* Not used */
  48     MYKONOS_UI_BIT_SELECT         = 8, /* Button emergency reset all */
  49     MYKONOS_UI_BIT_START          = 9, /* Button Takeoff / Landing */
  50     MYKONOS_UI_BIT_TRIM_THETA     = 18, /* y-axis trim +1 (Trim increase at +/- 1??/s) */
  51     MYKONOS_UI_BIT_TRIM_PHI       = 20, /* x-axis trim +1 (Trim increase at +/- 1??/s) */
  52     MYKONOS_UI_BIT_TRIM_YAW       = 22, /* z-axis trim +1 (Trim increase at +/- 1??/s) */
  53     MYKONOS_UI_BIT_X              = 24, /* x-axis +1 */
  54     MYKONOS_UI_BIT_Y              = 28, /* y-axis +1 */
  55 } mykonos_ui_bitfield_t;
  56
  57 typedef struct _radiogp_cmd_t {
  58   int32_t pitch;
  59   int32_t roll;
  60   int32_t gaz;
  61   int32_t yaw;
  62 } radiogp_cmd_t;
  63 static radiogp_cmd_t radiogp_cmd = {0};
  64
  65 #define MYKONOS_NO_TRIM                                                 \
  66     ((1 << MYKONOS_UI_BIT_TRIM_THETA) |                 \
  67          (1 << MYKONOS_UI_BIT_TRIM_PHI) |                       \
  68          (1 << MYKONOS_UI_BIT_TRIM_YAW) |                       \
  69          (1 << MYKONOS_UI_BIT_X) |                                      \
  70          (1 << MYKONOS_UI_BIT_Y))
  71
  72 static uint32_t user_input = MYKONOS_NO_TRIM;
  73
  74 static int at_udp_socket  = -1;
  75 static int overflow = 0;
  76 static unsigned long ocurrent = 0;
  77
  78 static void send_command(int nb_sequence);
  79 static void* at_cmds_loop(void *arg);
  80 static void boot_drone(void);
  81 static unsigned long get_time_ms(void);
  82
  83 static inline int get_mask_from_state( uint32_t state, uint32_t mask )
  84 {
  85     return state & mask ? TRUE : FALSE;
  86 }
  87
  88
  89 static unsigned long get_time_ms(void)
  90 {
  91         struct timeval tv;
  92         gettimeofday(&tv, NULL);
  93         return (tv.tv_sec*1000 + tv.tv_usec/1000);
  94 }
  95
  96 static void boot_drone(void)
  97 {
  98         const char cmds[] = "AT*CONFIG=\"general:navdata_demo\",\"TRUE\"\r";
  99
 100         if ( get_mask_from_state( mykonos_state,
 101                                                           MYKONOS_NAVDATA_BOOTSTRAP )) {
 102                 at_write ((int8_t*)cmds, strlen (cmds));
 103
 104                 int retry = 20;
 105                 int bcontinue = TRUE;
 106                 int next = 0;
 107                 while (bcontinue && retry) {
 108                                 if (next == 0) {
 109                                         if ( get_mask_from_state( mykonos_state, MYKONOS_COMMAND_MASK )) {
 110                                                 INFO ("[CONTROL] Processing the current command ... \n");
 111                                                 next++;
 112                                         }
 113                                 }
 114                                 else {
 115                                         char str[AT_BUFFER_SIZE];
 116                                         memset (str, 0, AT_BUFFER_SIZE);
 117                                         sprintf (str, "AT*CTRL=%d,%d\r", ACK_CONTROL_MODE, 0);
 118                                         at_write ((int8_t*)str, strlen (str));
 119
 120                                         if ( !get_mask_from_state( mykonos_state, MYKONOS_COMMAND_MASK )) {
 121                                                 INFO ("[CONTROL] Ack control event OK, send navdata demo\n");
 122                                                 bcontinue = FALSE;
 123                                         }
 124                                 }
 125                                 sleep (1);
 126                                 retry--;
 127                 }
 128         }
 129 }
 130
 131 static void* at_cmds_loop(void *arg)
 132 {
 133         unsigned long current, deadline;
 134         unsigned int nb_sequence = 0;
 135
 136         INFO("AT commands thread starting (thread=%d)...\n", (int)pthread_self());
 137
 138         user_input = MYKONOS_NO_TRIM;
 139         ocurrent = get_time_ms();
 140
 141     while (at_thread_alive) {
 142
 143                 if (get_mask_from_state(mykonos_state, MYKONOS_NAVDATA_BOOTSTRAP)) {
 144                         INFO("attempting to boot drone...\n");
 145                         boot_drone();
 146                         nb_sequence = 0;
 147                         continue;
 148                 }
 149
 150                 // compute next loop iteration deadline
 151                 deadline = get_time_ms() + MYKONOS_REFRESH_MS;
 152
 153                 // send pilot command
 154                 send_command( nb_sequence++ );
 155
 156                 // sleep until deadline
 157                 current = get_time_ms();
 158                 if (current < deadline) {
 159                         usleep(1000*(deadline-current));
 160                 }
 161         }
 162
 163         INFO("AT commands thread stopping\n");
 164     return NULL;
 165 }
 166
 167 static void send_command(int nb_sequence)
 168 {
 169         unsigned long current;
 170         char str[AT_BUFFER_SIZE];
 171
 172
 173    // send command to drone
 174         current = get_time_ms();
 175
 176         pthread_mutex_lock( &at_cmd_lock );
 177         snprintf(str, AT_BUFFER_SIZE, "AT*SEQ=%d\rAT*RADGP=%d,%d,%d,%d\rAT*REF=%d\r",
 178                         nb_sequence, radiogp_cmd.pitch, radiogp_cmd.roll, radiogp_cmd.gaz, radiogp_cmd.yaw, user_input);
 179    pthread_mutex_unlock( &at_cmd_lock );
 180
 181    //Send AT command
 182    at_write((int8_t*)str, strlen (str));
 183
 184         // check 30 ms overflow
 185         if (current > ocurrent + 30) {
 186                 overflow += current - ocurrent - MYKONOS_REFRESH_MS;
 187         }
 188         ocurrent = current;
 189
 190         /* dump command every 2s */
 191         if ((nb_sequence & 63) == 0) {
 192            pthread_mutex_lock( &at_cmd_lock );
 193                 INFO("seq=%d radgp(%d,%d,%d,%d) ui=0x%08x over=%d\n", nb_sequence,
 194                          radiogp_cmd.pitch, radiogp_cmd.roll, radiogp_cmd.gaz, radiogp_cmd.yaw, user_input, overflow);
 195       pthread_mutex_unlock( &at_cmd_lock );
 196                 overflow = 0;
 197         }
 198 }
 199
 200 void at_write (int8_t *buffer, int32_t len)
 201 {
 202         struct sockaddr_in to;
 203         int32_t flags;
 204
 205         pthread_mutex_lock( &at_cmd_lock );
 206         if( at_udp_socket < 0 ) {
 207                 struct sockaddr_in at_udp_addr;
 208
 209                 memset( (char*)&at_udp_addr, 0, sizeof(at_udp_addr) );
 210
 211                 at_udp_addr.sin_family      = AF_INET;
 212                 at_udp_addr.sin_addr.s_addr = INADDR_ANY;
 213                 at_udp_addr.sin_port        = htons( AT_PORT + 100 );
 214
 215                 at_udp_socket = socket( AF_INET, SOCK_DGRAM, 0 );
 216
 217                 if( at_udp_socket >= 0 )
 218                 {
 219                         flags = fcntl(at_udp_socket, F_GETFL, 0);
 220                         if( flags >= 0 )
 221                         {
 222                                 flags |= O_NONBLOCK;
 223
 224                                 flags = fcntl(at_udp_socket, F_SETFL, flags );
 225                         }
 226                         else
 227                         {
 228                                 INFO("Get Socket Options failed\n");
 229                         }
 230
 231                         if (bind(at_udp_socket, (struct sockaddr*)&at_udp_addr, sizeof(struct sockaddr)) < 0) {
 232                                 INFO ("at_write:bind: %s\n", strerror(errno));
 233                         }
 234                 }
 235         }
 236
 237         if( at_udp_socket >= 0 )
 238         {
 239                 int res;
 240
 241                 memset( (char*)&to, 0, sizeof(to) );
 242                 to.sin_family       = AF_INET;
 243                 to.sin_addr.s_addr  = inet_addr(WIFI_MYKONOS_IP); // BROADCAST address for subnet 192.168.1.xxx
 244                 to.sin_port         = htons (AT_PORT);
 245
 246                 res = sendto( at_udp_socket, (char*)buffer, len, 0, (struct sockaddr*)&to, sizeof(to) );
 247         }
 248    pthread_mutex_unlock( &at_cmd_lock );
 249 }
 250
 251 /* Public functions */
 252 void at_stop( void )
 253 {
 254         if ( !at_thread )
 255                 return;
 256
 257    at_thread_alive = 0;
 258         pthread_join(at_thread, NULL);
 259         at_thread = 0;
 260         pthread_mutex_destroy( &at_cmd_lock );
 261
 262         if (at_udp_socket >= 0){
 263                 close(at_udp_socket);
 264                 at_udp_socket = -1;
 265         }
 266 }
 267
 268 /************* at_init ****************
 269 * Description : Initialize AT process.
 270 */
 271 void at_run( void )
 272 {
 273         if (at_thread)
 274                 return;
 275
 276         /* Initialize mutex */
 277         if ( pthread_mutex_init( &at_cmd_lock, NULL ) != 0)
 278    {
 279            INFO("AT mutex init failed: %s\n", strerror(errno));
 280       return;
 281    }
 282
 283         // AT cmds loop
 284    at_thread_alive = 1;
 285         if ( pthread_create( &at_thread, NULL, at_cmds_loop, NULL ) )
 286    {
 287                 INFO("pthread_create: %s\n", strerror(errno));
 288         }
 289 }
 290
 291 /************* at_set_flat_trim ****************
 292 * Description : Calibration of the ARDrone.
 293 */
 294 void at_set_flat_trim( void )
 295 {
 296         const char cmd[] = "AT*FTRIM\r";
 297         at_write ((int8_t*)cmd, strlen (cmd));
 298 }
 299
 300 /************* at_ui_pad_start_pressed ****************
 301 * Description : Takeoff/Landing, used with at_cmds_loop function.
 302 */
 303 void at_ui_pad_start_pressed( void )
 304 {
 305         if (!at_thread)
 306                 return;
 307
 308    pthread_mutex_lock( &at_cmd_lock );
 309    user_input ^= 1 << MYKONOS_UI_BIT_START;
 310    pthread_mutex_unlock( &at_cmd_lock );
 311 }
 312
 313 /************* at_set_radiogp_input ****************
 314 * Description : Fill struct radiogp_cmd,
 315 * used with at_cmds_loop function.
 316 * pitch : y-axis (rad) (-25000, +25000)
 317 * roll : x-axis (rad) (-25000, +25000)
 318 * gaz :  altitude (mm/s) (-25000, +25000)
 319 * yaw : z-axis (rad/s) (-25000, +25000)
 320 */
 321 void at_set_radiogp_input( int32_t pitch, int32_t roll, int32_t gaz, int32_t yaw )
 322 {
 323         if (!at_thread)
 324                 return;
 325
 326    pthread_mutex_lock( &at_cmd_lock );
 327         radiogp_cmd.pitch = pitch;
 328         radiogp_cmd.roll = roll;
 329         radiogp_cmd.gaz = gaz;
 330         radiogp_cmd.yaw = yaw;
 331         pthread_mutex_unlock( &at_cmd_lock );
 332 }
 333
 334 /************* at_set_iphone_acceleros ****************
 335 * Description : Send directly accelero values.
 336 * enable : Apply values parameters on the ARDrone (only if iphone is used for computation).
 337 * fax : x-axis (-0.5, +0.5)
 338 * fay : y-axis (-0.5, +0.5)
 339 * faz : z-axis (-0.5, +0.5)
 340 */
 341 void at_set_iphone_acceleros( int enable, float32_t fax, float32_t fay, float32_t faz )
 342 {
 343         char cmd[AT_BUFFER_SIZE];
 344         memset (cmd, 0, AT_BUFFER_SIZE);
 345         sprintf (cmd, "AT*ACCS=%d,%d,%d,%d\r", (int32_t)enable, (int32_t)fax, (int32_t)fay, (int32_t)faz);
 346         at_write ((int8_t*)cmd, strlen (cmd));
 347 }
 348
 349
 350