mavlink_udp.c
Go to the documentation of this file.
00001 /*******************************************************************************
00002  Copyright (C) 2010  Bryan Godbolt godbolt ( a t ) ualberta.ca
00003  
00004  This program is free software: you can redistribute it and/or modify
00005  it under the terms of the GNU General Public License as published by
00006  the Free Software Foundation, either version 3 of the License, or
00007  (at your option) any later version.
00008  
00009  This program is distributed in the hope that it will be useful,
00010  but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  GNU General Public License for more details.
00013  
00014  You should have received a copy of the GNU General Public License
00015  along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016  
00017  ****************************************************************************/
00018 /*
00019  This program sends some data to qgroundcontrol using the mavlink protocol.  The sent packets
00020  cause qgroundcontrol to respond with heartbeats.  Any settings or custom commands sent from
00021  qgroundcontrol are printed by this program along with the heartbeats.
00022  
00023  
00024  I compiled this program sucessfully on Ubuntu 10.04 with the following command
00025  
00026  gcc -I ../../pixhawk/mavlink/include -o udp-server udp-server-test.c
00027  
00028  the rt library is needed for the clock_gettime on linux
00029  */
00030 /* These headers are for QNX, but should all be standard on unix/linux */
00031 #include <stdio.h>
00032 #include <errno.h>
00033 #include <string.h>
00034 #include <sys/socket.h>
00035 #include <sys/types.h>
00036 #include <netinet/in.h>
00037 #include <unistd.h>
00038 #include <stdlib.h>
00039 #include <fcntl.h>
00040 #include <time.h>
00041 #if (defined __QNX__) | (defined __QNXNTO__)
00042 /* QNX specific headers */
00043 #include <unix.h>
00044 #else
00045 /* Linux / MacOS POSIX timer headers */
00046 #include <sys/time.h>
00047 #include <time.h>
00048 #include <arpa/inet.h>
00049 #endif
00050 
00051 /* This assumes you have the mavlink headers on your include path
00052  or in the same folder as this source file */
00053 #include <mavlink.h>
00054 
00055 
00056 #define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why)
00057 
00058 uint64_t microsSinceEpoch();
00059 
00060 int main(int argc, char* argv[])
00061 {
00062         
00063         char help[] = "--help";
00064         
00065         
00066         char target_ip[100];
00067         
00068         float position[6] = {};
00069         int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
00070         struct sockaddr_in gcAddr; 
00071         struct sockaddr_in locAddr;
00072         //struct sockaddr_in fromAddr;
00073         uint8_t buf[BUFFER_LENGTH];
00074         ssize_t recsize;
00075         socklen_t fromlen;
00076         int bytes_sent;
00077         mavlink_message_t msg;
00078         uint16_t len;
00079         int i = 0;
00080         //int success = 0;
00081         unsigned int temp = 0;
00082         
00083         // Check if --help flag was used
00084         if ((argc == 2) && (strcmp(argv[1], help) == 0))
00085     {
00086                 printf("\n");
00087                 printf("\tUsage:\n\n");
00088                 printf("\t");
00089                 printf("%s", argv[0]);
00090                 printf(" <ip address of QGroundControl>\n");
00091                 printf("\tDefault for localhost: udp-server 127.0.0.1\n\n");
00092                 exit(EXIT_FAILURE);
00093     }
00094         
00095         
00096         // Change the target ip if parameter was given
00097         strcpy(target_ip, "127.0.0.1");
00098         if (argc == 2)
00099     {
00100                 strcpy(target_ip, argv[1]);
00101     }
00102         
00103         
00104         memset(&locAddr, 0, sizeof(locAddr));
00105         locAddr.sin_family = AF_INET;
00106         locAddr.sin_addr.s_addr = INADDR_ANY;
00107         locAddr.sin_port = htons(14551);
00108         
00109         /* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */ 
00110         if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr)))
00111     {
00112                 perror("error bind failed");
00113                 close(sock);
00114                 exit(EXIT_FAILURE);
00115     } 
00116         
00117         /* Attempt to make it non blocking */
00118         if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0)
00119     {
00120                 fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
00121                 close(sock);
00122                 exit(EXIT_FAILURE);
00123     }
00124         
00125         
00126         memset(&gcAddr, 0, sizeof(gcAddr));
00127         gcAddr.sin_family = AF_INET;
00128         gcAddr.sin_addr.s_addr = inet_addr(target_ip);
00129         gcAddr.sin_port = htons(14550);
00130         
00131         
00132         
00133         for (;;) 
00134     {
00135                 
00136                 /*Send Heartbeat */
00137                 mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
00138                 len = mavlink_msg_to_send_buffer(buf, &msg);
00139                 bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
00140                 
00141                 /* Send Status */
00142                 mavlink_msg_sys_status_pack(1, 200, &msg, 0, 0, 0, 500, 11000, -1, -1, 0, 0, 0, 0, 0, 0);
00143                 len = mavlink_msg_to_send_buffer(buf, &msg);
00144                 bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
00145                 
00146                 /* Send Local Position */
00147                 mavlink_msg_local_position_ned_pack(1, 200, &msg, microsSinceEpoch(), 
00148                                                                                 position[0], position[1], position[2],
00149                                                                                 position[3], position[4], position[5]);
00150                 len = mavlink_msg_to_send_buffer(buf, &msg);
00151                 bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
00152                 
00153                 /* Send attitude */
00154                 mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03);
00155                 len = mavlink_msg_to_send_buffer(buf, &msg);
00156                 bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
00157                 
00158                 
00159                 memset(buf, 0, BUFFER_LENGTH);
00160                 recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
00161                 if (recsize > 0)
00162         {
00163                         // Something received - print out all bytes and parse packet
00164                         mavlink_message_t msg;
00165                         mavlink_status_t status;
00166                         
00167                         printf("Bytes Received: %d\nDatagram: ", (int)recsize);
00168                         for (i = 0; i < recsize; ++i)
00169                         {
00170                                 temp = buf[i];
00171                                 printf("%02x ", (unsigned char)temp);
00172                                 if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
00173                                 {
00174                                         // Packet received
00175                                         printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
00176                                 }
00177                         }
00178                         printf("\n");
00179                 }
00180                 memset(buf, 0, BUFFER_LENGTH);
00181                 sleep(1); // Sleep one second
00182     }
00183 }
00184 
00185 
00186 /* QNX timer version */
00187 #if (defined __QNX__) | (defined __QNXNTO__)
00188 uint64_t microsSinceEpoch()
00189 {
00190         
00191         struct timespec time;
00192         
00193         uint64_t micros = 0;
00194         
00195         clock_gettime(CLOCK_REALTIME, &time);  
00196         micros = (uint64_t)time.tv_sec * 1000000 + time.tv_nsec/1000;
00197         
00198         return micros;
00199 }
00200 #else
00201 uint64_t microsSinceEpoch()
00202 {
00203         
00204         struct timeval tv;
00205         
00206         uint64_t micros = 0;
00207         
00208         gettimeofday(&tv, NULL);  
00209         micros =  ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec;
00210         
00211         return micros;
00212 }
00213 #endif


mavlink
Author(s): Lorenz Meier
autogenerated on Wed Sep 9 2015 18:06:17