MAVLINK—C语言demoWindows版本

这篇具有很好参考价值的文章主要介绍了MAVLINK—C语言demoWindows版本。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

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>
#include<ws2tcpip.h>
#include<iostream>
#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);
}
#endif


uint64_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 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
	if(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 Initial
	if((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)
#else
	if(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 config
	memset(&locAddr, 0, sizeof(locAddr));
	locAddr.sin_family = AF_INET;			// AF_UNIX/AF_LOCAL/AF_INET/AF_INET6/PF_INET
	locAddr.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 config
	memset(&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 Messages
		
		recsize = 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 received
					printf("\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客户端,然后就可以收到消息文章来源地址https://www.toymoban.com/news/detail-631262.html

到了这里,关于MAVLINK—C语言demoWindows版本的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 51单片机汇编语言指令大全

    参考链接 https://blog.csdn.net/weixin_45702442/article/details/115874162 https://blog.csdn.net/acslsr/article/details/106881712 在这里稍作终结,方便复习 51系列单片机按照功能分科分为5大类 数据传送类指令28条; 算数操作类指令24条; 逻辑运算类指令25条; 控制转移类17条; 位操作类17条。 MOV A,R

    2024年02月07日
    浏览(41)
  • 51单片机电子时钟(C语言)

     全部硬件电路分为四大部分: 控制模块51单片机; 显示模块LCD1602,由P0口控制; 计时模块DS1302,由P1口控制; 输入按键,由P3口控制。         实际设计硬件电路时应考虑到51单片机的电源、复位电路以及外接晶振,时钟芯片DS1302的备用电源等。 从1302的寄存器中读取数据

    2024年02月07日
    浏览(41)
  • 单片机C语言实例:14、音频输出

    一、喇叭发声原理 程序实例1: 二、警车声音 程序实例2: 三、救护车声音 程序实例3: 四、喇叭滴答声 程序实例4: 五、报警声音 程序实例5: 六、消防车警报 程序实例6: 七、音乐播放 程序实例7:

    2024年01月20日
    浏览(37)
  • 单片机----汇编语言入门知识点

    目录 汇编语句的格式 汇编语句的两个基本语句 子程序的调用 查表程序设计 1.x和y均为单字节数的查表程序设计 2.x为单字节数y为双字节数的查表程序设计 3.x和y均为双字节数的查表程序设计 分支转移程序设计 1.单分支选择结构 2.多分支选择结构 循环程序设计 (1) 计数循环控

    2024年04月28日
    浏览(46)
  • 单片机原理与接口技术(汇编语言)

    学习建议 结合哈工大的教程视频及课件学习 【公开课】哈工大 单片机原理及应用 单片机基础 计算机 计算机只能识别存储数码0和1 微型计算机中,负数常用补码表示 电子元器件的电位被区分为两种状态:高电位和低电位 常见高电位范围:2V ~ 5V 常见低电位范围:0V ~ 0.8V 正

    2024年01月20日
    浏览(46)
  • 单片机-c语言LED灯循环闪烁

    1.1一灯循环 1.2for循环实现八个依次亮 1.3使用intrins.h库函数实现 (连接完电路后,双击单片机,在programfiles中导入keil代码生成的 .HEX文件即可)

    2024年02月06日
    浏览(39)
  • 【RTOS学习】单片机中的C语言

    🐱作者:一只大喵咪1201 🐱专栏:《理解ARM架构》 🔥格言: 你只管努力,剩下的交给时间! 本喵默认各位小伙伴都会C语言,我们平时学习C语言都是在Windows环境下学习的,对于程序执行的底层逻辑了解的不是非常清楚,本喵在这里给大家介绍一下,C语言在单片机中是如何

    2024年02月08日
    浏览(35)
  • 单片机中使用C语言实现延时函数

    在单片机编程中,经常需要使用延时函数来控制程序的执行时间,特别是在需要等待一段时间后执行某些操作的情况下。在C语言中,可以通过一些方法来实现延时函数,下面我将介绍两种常用的方法。 方法一:使用循环延时 通过使用循环来实现延时是一种简单而常见的方法

    2024年01月22日
    浏览(35)
  • 51单片机编程应用(C语言):串口通信

    目录 通信的基本概念和种类    1.1串行通信与并行通信 ​编辑 1.2同步通信与异步通信 1.3单工,半双工,全双工   1.4通信速率  二、波特率和比特率的关系 串口通信简介: 1.接口标准 RS-232 2、D型9针接口定义 3.通信协议: 2.串口内部结构  51单片机串口相关寄存器   1.80c

    2024年02月19日
    浏览(49)
  • 【C C++开源库】适合单片机 嵌入式的C语言单元测试库_单片机 单元测试框架

    #define TEST_ASSERT_LESS_THAN_UINT64(threshold, actual) UNITY_TEST_ASSERT_SMALLER_THAN_UINT64((threshold), (actual), __LINE__, NULL) #define TEST_ASSERT_LESS_THAN_size_t(threshold, actual) UNITY_TEST_ASSERT_SMALLER_THAN_UINT((threshold), (actual), __LINE__, NULL) #define TEST_ASSERT_LESS_THAN_HEX8(threshold, actual) UNITY_TEST_ASSERT_SMALLER_THAN_HEX8((thres

    2024年04月25日
    浏览(47)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包