mirror of
https://github.com/mavlink/mavlink.git
synced 2026-06-19 07:35:34 +00:00
Added example to the distribution. Tested on Mac OS
This commit is contained in:
LM
@@ -0,0 +1 @@
|
||||
mavlink_udp
|
||||
@@ -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.
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user