mavlink_udp.c
Go to the documentation of this file.
1 /*******************************************************************************
2  Copyright (C) 2010 Bryan Godbolt godbolt ( a t ) ualberta.ca
3 
4  This program is free software: you can redistribute it and/or modify
5  it under the terms of the GNU General Public License as published by
6  the Free Software Foundation, either version 3 of the License, or
7  (at your option) any later version.
8 
9  This program is distributed in the hope that it will be useful,
10  but WITHOUT ANY WARRANTY; without even the implied warranty of
11  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  GNU General Public License for more details.
13 
14  You should have received a copy of the GNU General Public License
15  along with this program. If not, see <http://www.gnu.org/licenses/>.
16 
17  ****************************************************************************/
18 /*
19  This program sends some data to qgroundcontrol using the mavlink protocol. The sent packets
20  cause qgroundcontrol to respond with heartbeats. Any settings or custom commands sent from
21  qgroundcontrol are printed by this program along with the heartbeats.
22 
23 
24  I compiled this program sucessfully on Ubuntu 10.04 with the following command
25 
26  gcc -I ../../pixhawk/mavlink/include -o udp-server udp-server-test.c
27 
28  the rt library is needed for the clock_gettime on linux
29  */
30 /* These headers are for QNX, but should all be standard on unix/linux */
31 #include <stdio.h>
32 #include <errno.h>
33 #include <string.h>
34 #include <sys/socket.h>
35 #include <sys/types.h>
36 #include <netinet/in.h>
37 #include <unistd.h>
38 #include <stdlib.h>
39 #include <fcntl.h>
40 #include <time.h>
41 #if (defined __QNX__) | (defined __QNXNTO__)
42 /* QNX specific headers */
43 #include <unix.h>
44 #else
45 /* Linux / MacOS POSIX timer headers */
46 #include <sys/time.h>
47 #include <time.h>
48 #include <arpa/inet.h>
49 #include <stdbool.h> /* required for the definition of bool in C99 */
50 #endif
51 
52 /* This assumes you have the mavlink headers on your include path
53  or in the same folder as this source file */
54 #include <mavlink.h>
55 
56 
57 #define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why)
58 
59 uint64_t microsSinceEpoch();
60 
61 int main(int argc, char* argv[])
62 {
63 
64  char help[] = "--help";
65 
66 
67  char target_ip[100];
68 
69  float position[6] = {};
70  int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
71  struct sockaddr_in gcAddr;
72  struct sockaddr_in locAddr;
73  //struct sockaddr_in fromAddr;
74  uint8_t buf[BUFFER_LENGTH];
75  ssize_t recsize;
76  socklen_t fromlen = sizeof(gcAddr);
77  int bytes_sent;
79  uint16_t len;
80  int i = 0;
81  //int success = 0;
82  unsigned int temp = 0;
83 
84  // Check if --help flag was used
85  if ((argc == 2) && (strcmp(argv[1], help) == 0))
86  {
87  printf("\n");
88  printf("\tUsage:\n\n");
89  printf("\t");
90  printf("%s", argv[0]);
91  printf(" <ip address of QGroundControl>\n");
92  printf("\tDefault for localhost: udp-server 127.0.0.1\n\n");
93  exit(EXIT_FAILURE);
94  }
95 
96 
97  // Change the target ip if parameter was given
98  strcpy(target_ip, "127.0.0.1");
99  if (argc == 2)
100  {
101  strcpy(target_ip, argv[1]);
102  }
103 
104 
105  memset(&locAddr, 0, sizeof(locAddr));
106  locAddr.sin_family = AF_INET;
107  locAddr.sin_addr.s_addr = INADDR_ANY;
108  locAddr.sin_port = htons(14551);
109 
110  /* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */
111  if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr)))
112  {
113  perror("error bind failed");
114  close(sock);
115  exit(EXIT_FAILURE);
116  }
117 
118  /* Attempt to make it non blocking */
119 #if (defined __QNX__) | (defined __QNXNTO__)
120  if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0)
121 #else
122  if (fcntl(sock, F_SETFL, O_NONBLOCK | O_ASYNC) < 0)
123 #endif
124 
125  {
126  fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
127  close(sock);
128  exit(EXIT_FAILURE);
129  }
130 
131 
132  memset(&gcAddr, 0, sizeof(gcAddr));
133  gcAddr.sin_family = AF_INET;
134  gcAddr.sin_addr.s_addr = inet_addr(target_ip);
135  gcAddr.sin_port = htons(14550);
136 
137 
138 
139  for (;;)
140  {
141 
142  /*Send Heartbeat */
143  mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
144  len = mavlink_msg_to_send_buffer(buf, &msg);
145  bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
146 
147  /* Send Status */
148  mavlink_msg_sys_status_pack(1, 200, &msg, 0, 0, 0, 500, 11000, -1, -1, 0, 0, 0, 0, 0, 0);
149  len = mavlink_msg_to_send_buffer(buf, &msg);
150  bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
151 
152  /* Send Local Position */
153  mavlink_msg_local_position_ned_pack(1, 200, &msg, microsSinceEpoch(),
154  position[0], position[1], position[2],
155  position[3], position[4], position[5]);
156  len = mavlink_msg_to_send_buffer(buf, &msg);
157  bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
158 
159  /* Send attitude */
160  mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03);
161  len = mavlink_msg_to_send_buffer(buf, &msg);
162  bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
163 
164 
165  memset(buf, 0, BUFFER_LENGTH);
166  recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
167  if (recsize > 0)
168  {
169  // Something received - print out all bytes and parse packet
170  mavlink_message_t msg;
171  mavlink_status_t status;
172 
173  printf("Bytes Received: %d\nDatagram: ", (int)recsize);
174  for (i = 0; i < recsize; ++i)
175  {
176  temp = buf[i];
177  printf("%02x ", (unsigned char)temp);
178  if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
179  {
180  // Packet received
181  printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
182  }
183  }
184  printf("\n");
185  }
186  memset(buf, 0, BUFFER_LENGTH);
187  sleep(1); // Sleep one second
188  }
189 }
190 
191 
192 /* QNX timer version */
193 #if (defined __QNX__) | (defined __QNXNTO__)
194 uint64_t microsSinceEpoch()
195 {
196 
197  struct timespec time;
198 
199  uint64_t micros = 0;
200 
201  clock_gettime(CLOCK_REALTIME, &time);
202  micros = (uint64_t)time.tv_sec * 1000000 + time.tv_nsec/1000;
203 
204  return micros;
205 }
206 #else
208 {
209 
210  struct timeval tv;
211 
212  uint64_t micros = 0;
213 
214  gettimeofday(&tv, NULL);
215  micros = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec;
216 
217  return micros;
218 }
219 #endif


mavlink
Author(s): Lorenz Meier
autogenerated on Sun Jul 7 2019 03:22:07