在Linux飞控环境下使用MAVLink教程
2016-02-22 20:49
441 查看
在Linux飞控环境下使用MAVLink教程
主要内容MAVLink在Linux/QNX/MacOs系统上的综合教程(UDP)
概述
代码(C)
MAVLink在Windows系统上的综合教程Dev C++(TCP)
代码(C)
原标题:LINUX INTEGRATION tutorial for using MAVLink in your Linux flight environment. (MAVLink Linux/QNX/MacOs Integration Tutorial (UDP))
概述
该程序是用来测试QGroundControl通过UDP协议的连接情况。它会给QGroundControl发送必要的MavLink数据包并接受相应的数据包。这个代码可以不经修改直接在Linux、QNX、MacOS系统上编译。请找到适合的GCC编译命令在文章的底部。注意:为了使该程序正常工作,你必须调整主机(即运行QGroundControl的电脑)的IP地址。默认的地址是:127.0.0.1,但是你可以在通过传递参数的方式来修改它。如果要得到更多的帮助,在编译完成后,可以输入如下代码:
./mavlink_udp --help
你可以在wiki上找到“description of the details of th MAVLink protocol”
C-CODE(MAVLink Version v1.0.0)
代码注释完善中~~/******************************************************************************* 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"; //用于后面接受来自命令行的 ./mavlink_udp --help 命令 char target_ip[100];//用于存放IP地址 float position[6] = {};//记录飞机的坐标信息 int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);//linux中的socket函数,用于发送数据 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;//定义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 * 1000000 + 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
编译方法
在Ubuntu系统或者Mac系统上,设置好MAVLink的文件路径,然后输入编译命令
gcc -I (MAVLink文件夹的路径) -o (可执行文件名称) (源文件名称) 示例:gcc -I ../../include/common -o mavlink_udp mavlink_udp.c
在QNX系统上:
gcc -I ../../include/common -lsocket -o mavlink_udp mavlink_udp.c
MAVLink 在Windows系统上的综合教程
编译工具:DEV C++ 传输协议:TCP/** * @file * @brief The tcp interface process * * This process connects any external MAVLink tcp device and prints data * * @author Tsao, Chia-Cheng, <chiacheng.tsao@gmail.com> * */ #include <winsock2.h> #include <stdio.h> /* Standard input/output definitions */ #include <string.h> /* String function definitions */ #include <common/mavlink.h> bool silent = true; ///< Wether console output should be enabled bool verbose = false; ///< Enable verbose output bool debug = false; ///< Enable debug functions and output /* Create a TCP socket */ int main(int argc , char *argv[]) { WSADATA wsa; SOCKET s; struct sockaddr_in server; printf("Initialising Winsock... \n"); if (WSAStartup(MAKEWORD(2,2),&wsa) != 0) { printf("Failed. Error Code : %d \n",WSAGetLastError()); return 1; } printf("Initialised.\n"); //Create a socket if((s = socket(AF_INET , SOCK_STREAM , 0 )) == INVALID_SOCKET) { printf("Could not create socket : %d \n" , WSAGetLastError()); } printf("Socket created.\n"); server.sin_addr.s_addr = inet_addr("192.168.0.99"); server.sin_family = AF_INET; server.sin_port = htons( 6789 ); //Connect to remote server if (connect(s , (struct sockaddr *)&server , sizeof(server)) < 0) { puts("connect error! \n"); return 1; } printf("Connected. \n"); mavlink_status_t lastStatus; lastStatus.packet_rx_drop_count = 0; //Receive a reply from the server while(1) { uint8_t cp; mavlink_message_t message; mavlink_status_t status; uint8_t msgReceived = false; if((msgReceived = recv(s , (char*)&cp , 1 , 0)) == SOCKET_ERROR) { printf("recv failed! \n"); } if(msgReceived > 0) { // Check if a message could be decoded, return the message in case yes msgReceived = mavlink_parse_char(MAVLINK_COMM_1, cp, &message, &status); if (lastStatus.packet_rx_drop_count != status.packet_rx_drop_count) { if (verbose || debug) printf("ERROR: DROPPED %d PACKETS! \n", status.packet_rx_drop_count); if (debug) { unsigned char v=cp; fprintf(stderr,"%02x, ", v); } } lastStatus = status; } else { if (!silent) fprintf(stderr, "ERROR: Could not read from tcp! \n"); } // If a message could be decoded, handle it if(msgReceived) { //if (verbose || debug) std::cout << std::dec << "Received and forwarded tcp port message with id " << static_cast<unsigned int>(message.msgid) << " from system " << static_cast<int>(message.sysid) << std::endl; // Do not send images over serial port // DEBUG output if (debug) { fprintf(stderr,"Received tcp data: "); unsigned int i; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; unsigned int messageLength = mavlink_msg_to_send_buffer(buffer, &message); if (messageLength > MAVLINK_MAX_PACKET_LEN) { fprintf(stderr, "\nFATAL ERROR: MESSAGE LENGTH IS LARGER THAN BUFFER SIZE \n"); } else { for (i=0; i<messageLength; i++) { unsigned char v=buffer[i]; fprintf(stderr,"%02x ", v); } fprintf(stderr,"\n"); } } if (verbose || debug) printf("Received message from tcp with ID #%d (sys:%d|comp:%d):\n", message.msgid, message.sysid, message.compid); /* decode and print */ // For full MAVLink message documentation, look at: // https://pixhawk.ethz.ch/mavlink/ // Only print every n-th message static unsigned int scaled_imu_receive_counter = 0; switch (message.msgid) { case MAVLINK_MSG_ID_HIGHRES_IMU: { mavlink_highres_imu_t imu; mavlink_msg_highres_imu_decode(&message, &imu); printf("Got message HIGHRES_IMU"); printf("\t time: %llu\n", imu.time_usec); printf("\t acc (NED):\t% f\t% f\t% f (m/s^2)\n", imu.xacc, imu.yacc, imu.zacc); printf("\t gyro (NED):\t% f\t% f\t% f (rad/s)\n", imu.xgyro, imu.ygyro, imu.zgyro); printf("\t mag (NED):\t% f\t% f\t% f (Ga)\n", imu.xmag, imu.ymag, imu.zmag); printf("\t baro: \t %f (mBar)\n", imu.abs_pressure); printf("\t altitude: \t %f (m)\n", imu.pressure_alt); printf("\t temperature: \t %f C\n", imu.temperature); printf("\n"); } break; case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: { mavlink_global_position_int_t packet; mavlink_msg_global_position_int_decode(&message, &packet); printf("eccolo!! %d %d %d %d\n",packet.time_boot_ms,packet.lat,packet.lon,packet.hdg ); } break; #if 0 case MAVLINK_MSG_ID_ATTITUDE: { if (scaled_imu_receive_counter % 50 == 0) { mavlink_attitude_t packet_in; mavlink_msg_attitude_decode(&message, &packet_in); printf("attitude %f %f\n",packet_in.roll,packet_in.pitch ); } scaled_imu_receive_counter++; } break; #endif } } } return 0; }
相关文章推荐
- linux Native AIO的使用
- 第2章 Linux如何学习
- linux环境变量PS1设置完整版附带颜色
- arm-linux-gcc工具链安装FreeTpye 编译的支持
- 《Linux操作系统编译构建指南》
- QT5-控件-QLineEdit-文本输入控件,用来输入密码什么的还不错,可以和Linux登录一样不移动光标哦
- Linux下的消息队列示例
- 使用synergy软件使两台电脑共用一个鼠标和键盘
- 查看网卡流量
- Centos 执行top命令详细解读
- Linux Grep命令详解
- Linux IO 同步/异步 阻塞/非阻塞
- Linux下查看某个软件安装路径
- linux文件命令
- centos 7 安装在vmware Workstation的网卡问题 RHEL7
- 第1章 Linux是什么
- linux mysql修改最大连接数
- #查看Linux的版本信息
- wdcp centos6.5多网站部署
- linux下mmap实例分析