广告平面设计网站,高埗镇网站仿做,凡科怎么样,wordpress门户建站mavlink/examples/c/udp_example.c 在学习mavlink时准备学习一下官网的C语言example#xff0c;发现是unix系统的#xff0c;打算在Windows系统下尝试#xff0c;于是将示例修改了一下。
#include stdio.h
#include errno.h
#include string.h
#in…mavlink/examples/c/udp_example.c 在学习mavlink时准备学习一下官网的C语言example发现是unix系统的打算在Windows系统下尝试于是将示例修改了一下。
#include stdio.h
#include errno.h
#include string.h
#include Winsock2.h
#pragma comment(lib,ws2_32.lib)
#pragma comment(lib,wsock32.lib)
#include sys/types.h
#include windows.h
#include stdlib.h
#include fcntl.h
#include time.h
#includews2tcpip.h
#includeiostream
#include include2/common/mavlink.h
#include include2/common/mavlink_msg_local_position_ned.h
#include include2/common/mavlink_msg_sys_status.h
#include wtypes.h
#define ulong ULONG
#define uint UINT
#define CLIENT_IP 127.0.0.1
#define CLIENT_PORT 14550 // QGroundControl
#define SERVER_PORT 14551 // Vehicle
#define SYSTEM_ID 1
#define COMPONENT_ID 200
#define BUFFER_LENGTH 2041
#include time.h
#ifdef WIN32
#include windows.h
#else
#include sys/time.h
#endif
#ifdef WIN32
int gettimeofday(struct timeval *tp, void *tzp)
{time_t clock;struct tm tm;SYSTEMTIME wtm;GetLocalTime(wtm);tm.tm_year wtm.wYear - 1900;tm.tm_mon wtm.wMonth - 1;tm.tm_mday wtm.wDay;tm.tm_hour wtm.wHour;tm.tm_min wtm.wMinute;tm.tm_sec wtm.wSecond;tm. tm_isdst -1;clock mktime(tm);tp-tv_sec clock;tp-tv_usec wtm.wMilliseconds * 1000;return (0);
}
#endifuint64_t microsSinceEpoch();
using namespace std;
int main(int argc, char* argv[])
{char help[] --help;char target_ip[100];float position[6] {10.1, 23.7, 34.5, 3.2, 1.1, 3.3};int sock 0;struct sockaddr_in gcAddr; struct sockaddr_in locAddr;uint8_t buf[BUFFER_LENGTH];ssize_t recsize;socklen_t fromlen sizeof(gcAddr);int bytes_sent;mavlink_message_t msg;uint16_t len;int i 0;// Check if --help flag was usedif((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 givenif(argc 2){strcpy(target_ip, argv[1]);}else{strcpy(target_ip, CLIENT_IP);}//初始化WSA WORD sockVersion MAKEWORD(2,2); WSADATA wsaData; if(WSAStartup(sockVersion, wsaData)!0) { return 0; } // Socket Initialif((sock socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP)) -1){perror([ERR] socket initial failed);exit(EXIT_FAILURE);}/* Attempt to make it non blocking */
/*#if (defined __QNX__) | (defined __QNXNTO__)if(fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) -1)
#elseif(fcntl(sock, F_SETFL, O_NONBLOCK | O_ASYNC) -1)
#endif{fprintf(stderr, [ERR] setting nonblocking: %s\n, strerror(errno));closesocket(sock);exit(EXIT_FAILURE);}*/// Server Address configmemset(locAddr, 0, sizeof(locAddr));locAddr.sin_family AF_INET; // AF_UNIX/AF_LOCAL/AF_INET/AF_INET6/PF_INETlocAddr.sin_addr.s_addr INADDR_ANY; // inet_addr(127.0.0.1)locAddr.sin_port htons(SERVER_PORT);/* Bind the socket to Local Address:SERVER_PORT - necessary to receive packets from qgroundcontrol */ if(bind(sock, (struct sockaddr *)locAddr, sizeof(struct sockaddr)) -1){perror([ERR] bind failed);closesocket(sock);exit(EXIT_FAILURE);} // Client Address configmemset(gcAddr, 0, sizeof(gcAddr));gcAddr.sin_family AF_INET;gcAddr.sin_addr.s_addr inet_addr(target_ip);gcAddr.sin_port htons(CLIENT_PORT);printf(Start sending/receiving MAVLink message to/from QGroundControl...\n);while(1) {//发消息/* Send Heartbeat HEARTBEAT */mavlink_msg_heartbeat_pack(SYSTEM_ID, COMPONENT_ID, msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_PX4, MAV_MODE_STABILIZE_ARMED, 0, MAV_STATE_ACTIVE);len mavlink_msg_to_send_buffer(buf, msg);bytes_sent sendto(sock, (const char*)buf, len, 0, (struct sockaddr*)gcAddr, sizeof(struct sockaddr_in));/* Send Status SYS_STATUS */mavlink_msg_sys_status_pack(SYSTEM_ID, COMPONENT_ID, msg, 0, 0, 0, 500, 11000, -1, 100, 0, 0, 0, 0, 0,0);len mavlink_msg_to_send_buffer(buf, msg);bytes_sent sendto(sock, (const char*)buf, len, 0, (struct sockaddr*)gcAddr, sizeof (struct sockaddr_in));/* Send Local Position LOCAL_POSITION_NED */mavlink_msg_local_position_ned_pack(SYSTEM_ID, COMPONENT_ID, 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, (const char*)buf, len, 0, (struct sockaddr*)gcAddr, sizeof(struct sockaddr_in));/* Send attitude ATTITUDE */mavlink_msg_attitude_pack(SYSTEM_ID, COMPONENT_ID, 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, (const char*)buf, len, 0, (struct sockaddr*)gcAddr, sizeof(struct sockaddr_in));memset(buf, 0, BUFFER_LENGTH);// Receive Messagesrecsize recvfrom(sock, (char *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)gcAddr, fromlen);if(recsize 0){mavlink_message_t msg;mavlink_status_t status;printf(Bytes Received: %d\nDatagram: , (int)recsize);for(i 0; i recsize; i){printf(%02x , (unsigned char)buf[i]);if(mavlink_parse_char(MAVLINK_COMM_0, buf[i], msg, status) MAVLINK_FRAMING_OK){// Packet receivedprintf(\nReceived packet: SYS:%d, COMP:%d, LEN:%d, MSG:%d, SEQ:%d, \n, msg.sysid, msg.compid, msg.len, msg.msgid, msg.seq);}}printf(\n);}memset(buf, 0, BUFFER_LENGTH);Sleep(1); // Sleep one second}WSACleanup();
}/* 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运行的时候打开QGroundControl客户端然后就可以收到消息
