MAVLINK—C语言demoWindows版本
mavlink/examples/c/udp_example.c
在学习mavlink时准备学习一下官网的C语言example，发现是unix系统的，打算在Windows系统下尝试，于是将示例修改了一下。
#include \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客户端，然后就可以收到消息