forked from mavlink/mavlink
-
Notifications
You must be signed in to change notification settings - Fork 296
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Added example to the distribution. Tested on Mac OS
- Loading branch information
1 parent
9de3760
commit f5f5eb3
Showing
3 changed files
with
233 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1 @@ | ||
mavlink_udp |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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. |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 <http://www.gnu.org/licenses/>. | ||
****************************************************************************/ | ||
/* | ||
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 <stdio.h> | ||
#include <errno.h> | ||
#include <string.h> | ||
#include <sys/socket.h> | ||
#include <sys/types.h> | ||
#include <netinet/in.h> | ||
#include <unistd.h> | ||
#include <stdlib.h> | ||
#include <fcntl.h> | ||
#include <time.h> | ||
#if (defined __QNX__) | (defined __QNXNTO__) | ||
/* QNX specific headers */ | ||
#include <unix.h> | ||
#else | ||
/* Linux / MacOS POSIX timer headers */ | ||
#include <sys/time.h> | ||
#include <time.h> | ||
#include <arpa/inet.h> | ||
#endif | ||
|
||
/* This assumes you have the mavlink headers on your include path | ||
or in the same folder as this source file */ | ||
#include <mavlink.h> | ||
|
||
|
||
#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(" <ip address of QGroundControl>\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 |