#include "common/mavlink.h"
#include "common/mavlink_msg_local_position_ned.h"
#include "common/mavlink_msg_sys_status.h"
#define BUFFER_LENGTH \
2041 // minimum buffer size that can be used with qnx (I don't know why)
int main(int argc, char *argv[]) {
mavlink_system_t mavlink_system = {
1, // System ID (1-255)
1 // Component ID (a MAV_COMPONENT value)
};
char help[] = "--help";
char target_ip[100];
float position[6] = {};
uint8_t buf[BUFFER_LENGTH];
ssize_t recsize;
// socklen_t fromlen = sizeof(gcAddr);
int bytes_sent;
mavlink_message_t msg2;
// mavlink_message_t msg;
mavlink_status_t status;
uint16_t len;
int i = 0;
// int success = 0;
unsigned int temp = 0;
{
/*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(12, 234, &msg, 0, 0, 0, 500, 11000, -1, -1,
// 0, 0, 0, 0, 0, 0);
// len = mavlink_msg_to_send_buffer(buf, &msg);
mavlink_msg_local_position_ned_pack(1, 200, &msg2, 123, 2, 33, 35, 43, 52,
62);
len = mavlink_msg_to_send_buffer(buf, &msg2);
for (i = 0; i < len; i++)
printf("%x ", buf[i]);
printf("\n----\n");
mavlink_message_t msg;
for (i = 0; i < len; i++) {
temp = buf[i];
printf("%02x ", (unsigned char)temp);
if (mavlink_parse_char(12, 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);
if (msg.msgid == 32) {
mavlink_local_position_ned_t local_position_ned;
mavlink_msg_local_position_ned_decode(&msg, &local_position_ned);
printf("ned x %f, y%f, z%f\nned vx %f, vy%f, vz%f\n",
local_position_ned.x, local_position_ned.y,
local_position_ned.z, local_position_ned.vx,
local_position_ned.vy, local_position_ned.vz);
}
}
}
}
}