From ce33abdad72618c22b741485c3b33aed48a63d37 Mon Sep 17 00:00:00 2001 From: LM Date: Thu, 22 Sep 2011 18:17:58 +0200 Subject: [PATCH] Added MAVLink examples --- thirdParty/mavlink/examples/linux/.gitignore | 1 + thirdParty/mavlink/examples/linux/README | 19 ++ .../mavlink/examples/linux/mavlink_udp.c | 213 ++++++++++++++++++ 3 files changed, 233 insertions(+) create mode 100644 thirdParty/mavlink/examples/linux/.gitignore create mode 100644 thirdParty/mavlink/examples/linux/README create mode 100644 thirdParty/mavlink/examples/linux/mavlink_udp.c diff --git a/thirdParty/mavlink/examples/linux/.gitignore b/thirdParty/mavlink/examples/linux/.gitignore new file mode 100644 index 000000000..ce4f5db09 --- /dev/null +++ b/thirdParty/mavlink/examples/linux/.gitignore @@ -0,0 +1 @@ +mavlink_udp diff --git a/thirdParty/mavlink/examples/linux/README b/thirdParty/mavlink/examples/linux/README new file mode 100644 index 000000000..d13801842 --- /dev/null +++ b/thirdParty/mavlink/examples/linux/README @@ -0,0 +1,19 @@ +A more detailed version of this quickstart is available at: + +http://qgroundcontrol.org/dev/mavlink_linux_integration_tutorial + +MAVLINK UDP QUICKSTART INSTRUCTIONS +=================================== + +MAVLink UDP Example for *nix system (Linux, MacOS, BSD, etc.) + +To compile with GCC, just enter: + +gcc -I ../../include/common -o mavlink_udp mavlink_udp.c + +To run, type: + +./mavlink_udp + + +If you run QGroundControl on the same machine, a MAV should pop up. diff --git a/thirdParty/mavlink/examples/linux/mavlink_udp.c b/thirdParty/mavlink/examples/linux/mavlink_udp.c new file mode 100644 index 000000000..02fd3f048 --- /dev/null +++ b/thirdParty/mavlink/examples/linux/mavlink_udp.c @@ -0,0 +1,213 @@ +/******************************************************************************* + Copyright (C) 2010 Bryan Godbolt godbolt ( a t ) ualberta.ca + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + ****************************************************************************/ +/* + This program sends some data to qgroundcontrol using the mavlink protocol. The sent packets + cause qgroundcontrol to respond with heartbeats. Any settings or custom commands sent from + qgroundcontrol are printed by this program along with the heartbeats. + + + I compiled this program sucessfully on Ubuntu 10.04 with the following command + + gcc -I ../../pixhawk/mavlink/include -o udp-server udp-server-test.c + + the rt library is needed for the clock_gettime on linux + */ +/* These headers are for QNX, but should all be standard on unix/linux */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#if (defined __QNX__) | (defined __QNXNTO__) +/* QNX specific headers */ +#include +#else +/* Linux / MacOS POSIX timer headers */ +#include +#include +#include +#endif + +/* This assumes you have the mavlink headers on your include path + or in the same folder as this source file */ +#include + + +#define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why) + +uint64_t microsSinceEpoch(); + +int main(int argc, char* argv[]) +{ + + char help[] = "--help"; + + + char target_ip[100]; + + float position[6] = {}; + int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); + struct sockaddr_in gcAddr; + struct sockaddr_in locAddr; + //struct sockaddr_in fromAddr; + uint8_t buf[BUFFER_LENGTH]; + ssize_t recsize; + socklen_t fromlen; + int bytes_sent; + mavlink_message_t msg; + uint16_t len; + int i = 0; + //int success = 0; + unsigned int temp = 0; + + // Check if --help flag was used + if ((argc == 2) && (strcmp(argv[1], help) == 0)) + { + printf("\n"); + printf("\tUsage:\n\n"); + printf("\t"); + printf("%s", argv[0]); + printf(" \n"); + printf("\tDefault for localhost: udp-server 127.0.0.1\n\n"); + exit(EXIT_FAILURE); + } + + + // Change the target ip if parameter was given + strcpy(target_ip, "127.0.0.1"); + if (argc == 2) + { + strcpy(target_ip, argv[1]); + } + + + memset(&locAddr, 0, sizeof(locAddr)); + locAddr.sin_family = AF_INET; + locAddr.sin_addr.s_addr = INADDR_ANY; + locAddr.sin_port = htons(14551); + + /* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */ + if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr))) + { + perror("error bind failed"); + close(sock); + exit(EXIT_FAILURE); + } + + /* Attempt to make it non blocking */ + if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0) + { + fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno)); + close(sock); + exit(EXIT_FAILURE); + } + + + memset(&gcAddr, 0, sizeof(gcAddr)); + gcAddr.sin_family = AF_INET; + gcAddr.sin_addr.s_addr = inet_addr(target_ip); + gcAddr.sin_port = htons(14550); + + + + for (;;) + { + + /*Send Heartbeat */ + mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE); + len = mavlink_msg_to_send_buffer(buf, &msg); + bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); + + /* Send Status */ + mavlink_msg_sys_status_pack(1, 200, &msg, 0, 0, 0, 500, 11000, -1, -1, 0, 0, 0, 0, 0, 0); + len = mavlink_msg_to_send_buffer(buf, &msg); + bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); + + /* Send Local Position */ + mavlink_msg_local_position_ned_pack(1, 200, &msg, microsSinceEpoch(), + position[0], position[1], position[2], + position[3], position[4], position[5]); + len = mavlink_msg_to_send_buffer(buf, &msg); + bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); + + /* Send attitude */ + mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03); + len = mavlink_msg_to_send_buffer(buf, &msg); + bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); + + + memset(buf, 0, BUFFER_LENGTH); + recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen); + if (recsize > 0) + { + // Something received - print out all bytes and parse packet + mavlink_message_t msg; + mavlink_status_t status; + + printf("Bytes Received: %d\nDatagram: ", (int)recsize); + for (i = 0; i < recsize; ++i) + { + temp = buf[i]; + printf("%02x ", (unsigned char)temp); + if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) + { + // Packet received + printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid); + } + } + printf("\n"); + } + memset(buf, 0, BUFFER_LENGTH); + sleep(1); // Sleep one second + } +} + + +/* QNX timer version */ +#if (defined __QNX__) | (defined __QNXNTO__) +uint64_t microsSinceEpoch() +{ + + struct timespec time; + + uint64_t micros = 0; + + clock_gettime(CLOCK_REALTIME, &time); + micros = (uint64_t)time.tv_sec * 100000 + time.tv_nsec/1000; + + return micros; +} +#else +uint64_t microsSinceEpoch() +{ + + struct timeval tv; + + uint64_t micros = 0; + + gettimeofday(&tv, NULL); + micros = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; + + return micros; +} +#endif -- 2.22.0