Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
at_com.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2013 Freek van Tienen
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
27 #include "at_com.h"
28 #include "boards/ardrone2_sdk.h"
29 #include "generated/airframe.h"
30 
31 #include <stdlib.h>
32 #include <stdio.h>
33 #include <netdb.h>
34 #include <netinet/in.h>
35 #include <arpa/inet.h>
36 #include <errno.h>
37 
38 int packet_seq = 1; //Packet sequence number
39 
40 int at_socket = -1, //AT socket connection
41  navdata_socket = -1; //Navdata socket connection
42 
43 struct sockaddr_in pc_addr, //Own pc address
44  drone_at, //Drone AT address
45  drone_nav, //Drone nav address
46  from; //From address
47 
48 bool_t at_com_ready = FALSE; //Status of the at communication
49 char sessionId[9]; //THe config session ID
50 
51 void at_com_send(char* command);
52 void init_at_config(void);
53 
54 //Init the at_com
55 void init_at_com(void) {
56  //Check if already initialized
57  if (at_com_ready)
58  return;
59 
60  //Create the at and navdata socket
61  if ((at_socket = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
62  printf("at_com: at_socket error (%s)\n", strerror(errno));
63  }
64  if ((navdata_socket = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
65  printf("at_com: navdata_socket error (%s)\n", strerror(errno));
66  }
67 
68  //For recvfrom
69  pc_addr.sin_family = AF_INET;
70  pc_addr.sin_addr.s_addr = htonl(INADDR_ANY);
71  pc_addr.sin_port = htons(9800);
72 
73  //For sendto AT
74  drone_at.sin_family = AF_INET;
75  drone_at.sin_addr.s_addr = inet_addr(ARDRONE_IP);
76  drone_at.sin_port = htons(ARDRONE_AT_PORT);
77 
78  //For sendto navadata init
79  drone_nav.sin_family = AF_INET;
80  drone_nav.sin_addr.s_addr = inet_addr(ARDRONE_IP);
81  drone_nav.sin_port = htons(ARDRONE_NAVDATA_PORT);
82 
83  //Bind the navdata socket
84  if (bind(navdata_socket, (struct sockaddr *) &pc_addr, sizeof(pc_addr)) < 0) {
85  printf("at_com: bind error (%s)\n", strerror(errno));
86  }
87 
88  //Set unicast mode on
89  int one = 1;
90  sendto(navdata_socket, &one, 4, 0, (struct sockaddr *) &drone_nav,
91  sizeof(drone_nav));
92 
93  //Init at config
95 
96  //Set at_com to ready
98 }
99 
100 //Init the at config
101 void init_at_config(void) {
102  //Generate a session id
103  uint32_t binaryId = (uint32_t) rand();
104  binaryId = (0 != binaryId) ? binaryId : 1u;
105  snprintf(sessionId, 9, "%08x", binaryId);
106  sessionId[8] = '\0';
107 
108  //Send session, application and user id:
109  at_com_send_config("custom:session_id", sessionId);
110  at_com_send_config("custom:application_id", "9D7BFD45");
111  at_com_send_config("custom:profile_id", "2BF07F58");
112 
113  //Send config values
114  at_com_send_config("control:euler_angle_max", "0.52");
115  at_com_send_config("control:altitude_max", "20000");
116  at_com_send_config("control:control_vz_max", "2000");
117  at_com_send_config("control:control_yaw", "6.11");
118 
119  //Send config values with the airframe.h
120 #ifndef ARDRONE_FLIGHT_INDOOR
121  at_com_send_config("control:outdoor", "TRUE");
122 #else
123  at_com_send_config("control:outdoor","FALSE");
124 #endif
125 #ifndef ARDRONE_WITHOUT_SHELL
126  at_com_send_config("control:flight_without_shell", "FALSE");
127 #else
128  at_com_send_config("control:flight_without_shell","TRUE");
129 #endif
130 #ifdef ARDRONE_OWNER_MAC
131  at_com_send_config("network:owner_mac",ARDRONE_OWNER_MAC);
132 #endif
133 }
134 
135 //Recieve a navdata packet
136 int at_com_recieve_navdata(unsigned char* buffer) {
137  int l = sizeof(from);
138  int n;
139  // FIXME(ben): not clear why recvfrom() and not recv() is used.
140  n = recvfrom(navdata_socket, buffer, ARDRONE_NAVDATA_BUFFER_SIZE, 0x0,
141  (struct sockaddr *) &from, (socklen_t *) &l);
142 
143  return n;
144 }
145 
146 //Send an AT command
147 void at_com_send(char* command) {
148  sendto(at_socket, command, strlen(command), 0, (struct sockaddr*) &drone_at,
149  sizeof(drone_at));
150 }
151 
152 //Send a Config
153 void at_com_send_config(char* key, char* value) {
154  char command[256];
155  sprintf(command, "AT*CONFIG_IDS=%d,\"%s\",\"2BF07F58\",\"9D7BFD45\"\r",
156  packet_seq++, sessionId);
157  at_com_send(command);
158  sprintf(command, "AT*CONFIG=%d,\"%s\",\"%s\"\r", packet_seq++, key, value);
159  at_com_send(command);
160 }
161 
162 //Send a Flat trim
163 void at_com_send_ftrim(void) {
164  char command[256];
165  sprintf(command, "AT*FTRIM=%d\r", packet_seq++);
166  at_com_send(command);
167 }
168 
169 //Send a Ref
170 void at_com_send_ref(int bits) {
171  char command[256];
172  sprintf(command, "AT*REF=%d,%d\r", packet_seq++, bits | REF_DEFAULT);
173  at_com_send(command);
174 }
175 
176 //Send a Pcmd
177 void at_com_send_pcmd(int mode, float thrust, float roll, float pitch,
178  float yaw) {
179  int f_thrust, f_roll, f_pitch, f_yaw;
180  char command[256];
181 
182  //Change the floats to ints(dereferencing)
183  memcpy(&f_thrust, &thrust, sizeof thrust);
184  memcpy(&f_roll, &roll, sizeof roll);
185  memcpy(&f_pitch, &pitch, sizeof pitch);
186  memcpy(&f_yaw, &yaw, sizeof yaw);
187 
188  sprintf(command, "AT*PCMD=%d,%d,%d,%d,%d,%d\r", packet_seq++, mode, f_roll,
189  f_pitch, f_thrust, f_yaw);
190  at_com_send(command);
191 }
192 
193 //Send a Calib
194 void at_com_send_calib(int device) {
195  char command[256];
196  sprintf(command, "AT*CALIB=%d,%d\r", packet_seq++, device);
197  at_com_send(command);
198 }
uint16_t value
Definition: adc_arch.c:585
#define ARDRONE_AT_PORT
Definition: ardrone2_sdk.h:8
void at_com_send_pcmd(int mode, float thrust, float roll, float pitch, float yaw)
Definition: at_com.c:177
#define ARDRONE_NAVDATA_BUFFER_SIZE
Definition: ardrone2_sdk.h:9
#define ARDRONE_IP
Definition: ardrone2_sdk.h:10
void at_com_send_ref(int bits)
Definition: at_com.c:170
int at_com_recieve_navdata(unsigned char *buffer)
Definition: at_com.c:136
unsigned char buffer[4096]
Definition: ahrs_ardrone2.c:47
bool_t at_com_ready
Definition: at_com.c:48
#define FALSE
Definition: imu_chimu.h:141
char sessionId[9]
Definition: at_com.c:49
void at_com_send_ftrim(void)
Definition: at_com.c:163
void init_at_config(void)
Definition: at_com.c:101
unsigned long uint32_t
Definition: types.h:18
int packet_seq
Definition: at_com.c:38
void at_com_send(char *command)
Definition: at_com.c:147
#define TRUE
Definition: imu_chimu.h:144
void at_com_send_calib(int device)
Definition: at_com.c:194
#define ARDRONE_NAVDATA_PORT
Definition: ardrone2_sdk.h:7
void init_at_com(void)
Definition: at_com.c:55
int at_socket
Definition: at_com.c:40
Sending and receiving of AT-commands specified by the ardrone API.
void at_com_send_config(char *key, char *value)
Definition: at_com.c:153
struct sockaddr_in pc_addr drone_at drone_nav from
Definition: at_com.c:43
int navdata_socket
Definition: at_com.c:41