main.c
Go to the documentation of this file.
00001 /*******************************************************************************
00002  
00003  Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch
00004  and Bryan Godbolt godbolt ( a t ) ualberta.ca
00005  
00006  adapted from example written by Bryan Godbolt godbolt ( a t ) ualberta.ca
00007  
00008  This program is free software: you can redistribute it and/or modify
00009  it under the terms of the GNU General Public License as published by
00010  the Free Software Foundation, either version 3 of the License, or
00011  (at your option) any later version.
00012  
00013  This program is distributed in the hope that it will be useful,
00014  but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016  GNU General Public License for more details.
00017  
00018  You should have received a copy of the GNU General Public License
00019  along with this program.  If not, see <http://www.gnu.org/licenses/>.
00020  
00021  ****************************************************************************/
00022 /*
00023  This program sends some data to qgroundcontrol using the mavlink protocol.  The sent packets
00024  cause qgroundcontrol to respond with heartbeats.  Any settings or custom commands sent from
00025  qgroundcontrol are printed by this program along with the heartbeats.
00026  
00027  
00028  I compiled this program sucessfully on Ubuntu 10.04 with the following command
00029  
00030  gcc -I ../../pixhawk/mavlink/include -o udp-server udp.c
00031  
00032  the rt library is needed for the clock_gettime on linux
00033  */
00034 /* These headers are for QNX, but should all be standard on unix/linux */
00035 #include <stdio.h>
00036 #include <errno.h>
00037 #include <string.h>
00038 #include <sys/socket.h>
00039 #include <sys/types.h>
00040 #include <netinet/in.h>
00041 #include <unistd.h>
00042 #include <stdlib.h>
00043 #include <fcntl.h>
00044 #include <time.h>
00045 #include <math.h>
00046 #if (defined __QNX__) | (defined __QNXNTO__)
00047 /* QNX specific headers */
00048 #include <unix.h>
00049 #else
00050 /* Linux / MacOS POSIX timer headers */
00051 #include <sys/time.h>
00052 #include <time.h>
00053 #include <arpa/inet.h>
00054 #endif
00055 
00056 /* FIRST: MAVLink setup */
00057 //#define MAVLINK_CONFIGURED
00058 //#define MAVLINK_NO_DATA
00059 //#define MAVLINK_WPM_VERBOSE
00060 
00061 /* 0: Include MAVLink types */
00062 #include "mavlink_types.h"
00063 
00064 /* 1: Define mavlink system storage */
00065 mavlink_system_t mavlink_system;
00066 
00067 /* 2: Include actual protocol, REQUIRES mavlink_system */
00068 #include "mavlink.h"
00069 
00070 /* 3: Define waypoint helper functions */
00071 void mavlink_wpm_send_message(mavlink_message_t* msg);
00072 void mavlink_wpm_send_gcs_string(const char* string);
00073 uint64_t mavlink_wpm_get_system_timestamp();
00074 void mavlink_missionlib_send_message(mavlink_message_t* msg);
00075 void mavlink_missionlib_send_gcs_string(const char* string);
00076 uint64_t mavlink_missionlib_get_system_timestamp();
00077 
00078 /* 4: Include waypoint protocol */
00079 #include "waypoints.h"
00080 mavlink_wpm_storage wpm;
00081 
00082 
00083 #include "mavlink_missionlib_data.h"
00084 #include "mavlink_parameters.h"
00085 
00086 mavlink_pm_storage pm;
00087 
00092 void mavlink_pm_reset_params(mavlink_pm_storage* pm)
00093 {
00094         pm->size = MAVLINK_PM_MAX_PARAM_COUNT;
00095         // 1) MAVLINK_PM_PARAM_SYSTEM_ID
00096         pm->param_values[MAVLINK_PM_PARAM_SYSTEM_ID] = 12;
00097         strcpy(pm->param_names[MAVLINK_PM_PARAM_SYSTEM_ID], "SYS_ID");
00098         // 2) MAVLINK_PM_PARAM_ATT_K_D
00099         pm->param_values[MAVLINK_PM_PARAM_ATT_K_D] = 0.3f;
00100         strcpy(pm->param_names[MAVLINK_PM_PARAM_ATT_K_D], "ATT_K_D");
00101 }
00102 
00103 
00104 #define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why)
00105 
00106 char help[] = "--help";
00107 
00108 
00109 char target_ip[100];
00110 
00111 float position[6] = {};
00112 int sock;
00113 struct sockaddr_in gcAddr; 
00114 struct sockaddr_in locAddr;
00115 uint8_t buf[BUFFER_LENGTH];
00116 ssize_t recsize;
00117 socklen_t fromlen;
00118 int bytes_sent;
00119 mavlink_message_t msg;
00120 uint16_t len;
00121 int i = 0;
00122 unsigned int temp = 0;
00123 
00124 uint64_t microsSinceEpoch();
00125 
00126 
00127 
00128 
00129 /* Provide the interface functions for the waypoint manager */
00130 
00131 /*
00132  *  @brief Sends a MAVLink message over UDP
00133  */
00134 
00135 void mavlink_missionlib_send_message(mavlink_message_t* msg)
00136 {
00137         uint8_t buf[MAVLINK_MAX_PACKET_LEN];
00138         uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
00139         uint16_t bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
00140         
00141         printf("SENT %d bytes\n", bytes_sent);
00142 }
00143 
00144 void mavlink_missionlib_send_gcs_string(const char* string)
00145 {
00146         const int len = 50;
00147         mavlink_statustext_t status;
00148         int i = 0;
00149         while (i < len - 1)
00150         {
00151                 status.text[i] = string[i];
00152                 if (string[i] == '\0')
00153                         break;
00154                 i++;
00155         }
00156         status.text[i] = '\0'; // Enforce null termination
00157         mavlink_message_t msg;
00158         
00159         mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &status);
00160         mavlink_missionlib_send_message(&msg);
00161 }
00162 
00163 uint64_t mavlink_missionlib_get_system_timestamp()
00164 {
00165         struct timeval tv;
00166         gettimeofday(&tv, NULL);
00167         return ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec;
00168 }
00169 
00170 float roll, pitch, yaw;
00171 uint32_t latitude, longitude, altitude;
00172 uint16_t speedx, speedy, speedz;
00173 float rollspeed, pitchspeed, yawspeed;
00174 bool hilEnabled = false;
00175 
00176 int main(int argc, char* argv[])
00177 {       
00178         // Initialize MAVLink
00179         mavlink_wpm_init(&wpm);
00180         mavlink_system.sysid = 5;
00181         mavlink_system.compid = 20;
00182         mavlink_pm_reset_params(&pm);
00183         
00184         int32_t ground_distance;
00185         uint32_t time_ms;
00186         
00187         
00188         
00189         // Create socket
00190         sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
00191         
00192         // Check if --help flag was used
00193         if ((argc == 2) && (strcmp(argv[1], help) == 0))
00194     {
00195                 printf("\n");
00196                 printf("\tUsage:\n\n");
00197                 printf("\t");
00198                 printf("%s", argv[0]);
00199                 printf(" <ip address of QGroundControl>\n");
00200                 printf("\tDefault for localhost: udp-server 127.0.0.1\n\n");
00201                 exit(EXIT_FAILURE);
00202     }
00203         
00204         
00205         // Change the target ip if parameter was given
00206         strcpy(target_ip, "127.0.0.1");
00207         if (argc == 2)
00208     {
00209                 strcpy(target_ip, argv[1]);
00210     }
00211         
00212         
00213         memset(&locAddr, 0, sizeof(locAddr));
00214         locAddr.sin_family = AF_INET;
00215         locAddr.sin_addr.s_addr = INADDR_ANY;
00216         locAddr.sin_port = htons(14551);
00217         
00218         /* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */ 
00219         if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr)))
00220     {
00221                 perror("error bind failed");
00222                 close(sock);
00223                 exit(EXIT_FAILURE);
00224     } 
00225         
00226         /* Attempt to make it non blocking */
00227         if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0)
00228     {
00229                 fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
00230                 close(sock);
00231                 exit(EXIT_FAILURE);
00232     }
00233         
00234         
00235         memset(&gcAddr, 0, sizeof(gcAddr));
00236         gcAddr.sin_family = AF_INET;
00237         gcAddr.sin_addr.s_addr = inet_addr(target_ip);
00238         gcAddr.sin_port = htons(14550);
00239         
00240         
00241         printf("MAVLINK MISSION LIBRARY EXAMPLE PROCESS INITIALIZATION DONE, RUNNING..\n");
00242         
00243         
00244         for (;;) 
00245     {
00246                 bytes_sent = 0;
00247                 
00248                 /* Send Heartbeat */
00249                 mavlink_msg_heartbeat_pack(mavlink_system.sysid, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, 0, 0, 0);
00250                 len = mavlink_msg_to_send_buffer(buf, &msg);
00251                 bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
00252                 
00253                 /* Send Status */
00254                 mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE, 500, 7500, 0, 0, 0, 0, 0, 0, 0, 0);
00255                 len = mavlink_msg_to_send_buffer(buf, &msg);
00256                 bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
00257                 
00258                 /* Send Local Position */
00259                 mavlink_msg_local_position_ned_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), 
00260                                                                                 position[0], position[1], position[2],
00261                                                                                 position[3], position[4], position[5]);
00262                 len = mavlink_msg_to_send_buffer(buf, &msg);
00263                 bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
00264                 
00265                 /* Send global position */
00266                 if (hilEnabled)
00267                 {
00268                         mavlink_msg_global_position_int_pack(mavlink_system.sysid, 200, &msg, time_ms, latitude, longitude, altitude, ground_distance, speedx, speedy, speedz, (yaw/M_PI)*180*100);
00269                         len = mavlink_msg_to_send_buffer(buf, &msg);
00270                         bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
00271                 }
00272                 
00273                 /* Send attitude */
00274                 mavlink_msg_attitude_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), roll, pitch, yaw, rollspeed, pitchspeed, yawspeed);
00275                 len = mavlink_msg_to_send_buffer(buf, &msg);
00276                 bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
00277                 
00278                 /* Send HIL outputs */
00279                 float roll_ailerons = 0;   // -1 .. 1
00280                 float pitch_elevator = 0.2;  // -1 .. 1
00281                 float yaw_rudder = 0.1;      // -1 .. 1
00282                 float throttle = 0.9;      //  0 .. 1
00283                 mavlink_msg_hil_controls_pack_chan(mavlink_system.sysid, mavlink_system.compid, MAVLINK_COMM_0, &msg, microsSinceEpoch(), roll_ailerons, pitch_elevator, yaw_rudder, throttle, mavlink_system.mode, mavlink_system.nav_mode, 0, 0, 0, 0);
00284                 len = mavlink_msg_to_send_buffer(buf, &msg);
00285                 bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
00286                 
00287                 memset(buf, 0, BUFFER_LENGTH);
00288                 recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
00289                 if (recsize > 0)
00290         {
00291                         // Something received - print out all bytes and parse packet
00292                         mavlink_message_t msg;
00293                         mavlink_status_t status;
00294                         
00295                         printf("Bytes Received: %d\nDatagram: ", (int)recsize);
00296                         for (i = 0; i < recsize; ++i)
00297                         {
00298                                 temp = buf[i];
00299                                 printf("%02x ", (unsigned char)temp);
00300                                 if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
00301                                 {
00302                                         // Packet received
00303                                         printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
00304                                         
00305                                         // Handle packet with waypoint component
00306                                         mavlink_wpm_message_handler(&msg);
00307                                         
00308                                         // Handle packet with parameter component
00309                                         mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
00310                                         
00311                                         // Print HIL values sent to system
00312                                         if (msg.msgid == MAVLINK_MSG_ID_HIL_STATE)
00313                                         {
00314                                                 mavlink_hil_state_t hil;
00315                                                 mavlink_msg_hil_state_decode(&msg, &hil);
00316                                                 printf("Received HIL state:\n");
00317                                                 printf("R: %f P: %f Y: %f\n", hil.roll, hil.pitch, hil.yaw);
00318                                                 roll = hil.roll;
00319                                                 pitch = hil.pitch;
00320                                                 yaw = hil.yaw;
00321                                                 rollspeed = hil.rollspeed;
00322                                                 pitchspeed = hil.pitchspeed;
00323                                                 yawspeed = hil.yawspeed;
00324                                                 speedx = hil.vx;
00325                                                 speedy = hil.vy;
00326                                                 speedz = hil.vz;
00327                                                 latitude = hil.lat;
00328                                                 longitude = hil.lon;
00329                                                 altitude = hil.alt;
00330                                                 hilEnabled = true;
00331                                         }
00332                                 }
00333                         }
00334                         printf("\n");
00335                 }
00336                 memset(buf, 0, BUFFER_LENGTH);
00337                 usleep(10000); // Sleep 10 ms
00338                 
00339                 
00340                 // Send one parameter
00341                 mavlink_pm_queued_send();
00342     }
00343 }
00344 
00345 
00346 /* QNX timer version */
00347 #if (defined __QNX__) | (defined __QNXNTO__)
00348 uint64_t microsSinceEpoch()
00349 {
00350         
00351         struct timespec time;
00352         
00353         uint64_t micros = 0;
00354         
00355         clock_gettime(CLOCK_REALTIME, &time);  
00356         micros = (uint64_t)time.tv_sec * 100000 + time.tv_nsec/1000;
00357         
00358         return micros;
00359 }
00360 #else
00361 uint64_t microsSinceEpoch()
00362 {
00363         
00364         struct timeval tv;
00365         
00366         uint64_t micros = 0;
00367         
00368         gettimeofday(&tv, NULL);  
00369         micros =  ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec;
00370         
00371         return micros;
00372 }
00373 #endif


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